How to get world coordinates in mm(!) from projectPixelTo3dRay
My two cameras are calibrated and intrinsic and extrinsic parameters are stored in the camera_info
Now i get world coordinates from a pixel point (u,v) via projectPixelTo3dRay
.
Example: point (98,200) = [-0.269315, -0.0648566, 1] is the output. I know its a 3d ray passing trough my point(u,v) but how do i get know my real world coordinates in millimeter? Could any body explain it with an example for a dumb person like me? Please help it's important for me!
Code:
image_geometry::PinholeCameraModel model1;
image_geometry::PinholeCameraModel model2;
void callback(const ImageConstPtr& sub1, const CameraInfoConstPtr& cam_info1,const ImageConstPtr& sub2 , const CameraInfoConstPtr& cam_info2){
double stamp_1 =sub1->header.stamp.toSec();
double stamp_2 =sub2->header.stamp.toSec();
ROS_INFO("\nCALLBACK GETS CALLED\n TIME_STAMP_IMAGE1: %f\n TIME_STAMP_IMAGE2: %f\n", stamp_1, stamp_2);
model1.fromCameraInfo(cam_info1);
model2.fromCameraInfo(cam_info2);
cv::Point2d pixel_point(240,320);
cv::Point3d xyz = model1.projectPixelTo3dRay(pixel_point);
cout << "point (3D) = " << xyz << endl << endl;
}
int main(int argc, char** argv) {
ros::init( argc, argv, "sync_test" );
ros::NodeHandle nh;
//namedWindow("window1", CV_WINDOW_NORMAL);
//namedWindow("window2", CV_WINDOW_NORMAL);
//cv::startWindowThread();
message_filters::Subscriber<Image> sub1(nh, "/usb_cam/image_rect_color", 5);
message_filters::Subscriber<CameraInfo> info_sub1(nh, "/usb_cam/camera_info", 5);
message_filters::Subscriber<Image> sub2(nh, "/usb_cam/image_rect_color", 5);
message_filters::Subscriber<CameraInfo> info_sub2(nh, "/usb_cam/camera_info", 5);
typedef sync_policies::ApproximateTime<Image,CameraInfo,Image,CameraInfo> MySyncPolicy;
// ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), sub1,info_sub1, sub2,info_sub2);
//sync.setAgePenalty(1.0);
sync.registerCallback(boost::bind(&callback, _1, _2,_3,_4));
ros::spin();
return 0;
}
Asked by Gren8 on 2016-01-12 14:14:29 UTC
Comments