Ask Your Question
0

image_geometry projectPixelTo3dRay World Coordinates

asked 2016-01-12 12:17:46 -0600

Gren8 gravatar image

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 projectPixelTo3dRayit 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;

}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2016-01-12 12:25:57 -0600

NEngelhard gravatar image

updated 2016-01-12 12:28:28 -0600

The pixel corresponds to a ray that starts at your camera position and passes through the point (-0.269315, -0.0648566, 1) (in your camera frame). If you pass the center point of your image, the point should be (0,0,1). z = 1 is just a random point on this line.

The docu is also very helpful: http://docs.ros.org/jade/api/image_ge...

http://docs.ros.org/jade/api/image_ge...

edit flag offensive delete link more

Comments

thanks for the answer! If i take a random point (400,56) the output is [0.102615, -0.244058, ] Could you give me an example of how i get my real world coordinates in mm now? I know its in the link you posted but i don't really understand it.

Gren8 gravatar imageGren8 ( 2016-01-12 12:44:00 -0600 )edit
1

The point is in m, so just multiply with 1000?

NEngelhard gravatar imageNEngelhard ( 2016-01-12 13:33:00 -0600 )edit

but if my output is a unit vector [0.102615, -0.244058, 1 ] and i multiply it by 1000 why do i get a negative mm value?

Gren8 gravatar imageGren8 ( 2016-01-12 13:55:06 -0600 )edit
1

why do you expect positive values for all axis?

NEngelhard gravatar imageNEngelhard ( 2016-01-12 15:08:10 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2016-01-12 12:17:46 -0600

Seen: 1,406 times

Last updated: Jan 12 '16