Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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;

    }