image_geometry projectPixelTo3dRay World Coordinates
Good evening,
i have a problem and i need help as fast as possible. For a tracking program i try to subscribe from two image_rect_color
+ their camera_infos
. i have already calibrated the cameras with the ros package camera_calibration
and i stored in the camera_info file. Now i am trying to get real world coordinates with the image_geometry
package.
Put if i pick a pixel point (98,200) and compute it via projectPixelTo3dRay
it only gives me strange values and z is always 1.
Example: point (98,200) = [-0.269315, -0.0648566, 1]. Do i understand something wrong? What else can I do to get real world coordinates? The intrinsic and extrinsic values are stored in the camera_info
.
Please help its important for me!
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(98, 200);
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;
}