Project point cloud to image coordinates using camerainfo
Hi, I'm new to ROS (and PCL as well) so sorry if the question is too trivial...but I've been searching for quite a long time and couldn't find an answer..
I have the pointcloud and I need to project it to the image coordinates with corresponding CameraInfo. I found there is K (intrinsic) matrix which I should use for multiplication the coordinates (K*[x y z]^transposed) but the results don't seem to be correct...
EDIT: Okay, I followed the tutorial and now I have something like
PointCloud<PointXYZI>::Ptr output_xyz(new PointCloud<PointXYZI>); // init PointCloud
sensor_msgs::CameraInfo cam_info; // init CameraInfo
.... // do some processing
image_geometry::PinholeCameraModel cam_model; // init cam_model
cam_model.fromCameraInfo(cam_info); // fill cam_model with CameraInfo
cv::Point3d pt_cv(output_xyz->at(0).x, output_xyz->at(0).y, output_xyz->at(0).z);//init Point3d
cv::Point2d uv = cam_model.project3dToPixel(pt_cv); // project 3d point to 2d point
and I'm getting an assertion error "Assertion `P_(2, 3) == 0.0' failed" . That is obvious because my cam_model.projectionMatrix() looks like this
[868.9074096679688, 0, 641.492403333923, 0;
0, 879.0201416015625, 542.1365647019775, 0;
0, 0, 1, 1]
I think there is no problem with CameraInfo, because rviz displays the pointcloud correctly in the image with it, but I probably need to do yet some other manipulation to get the correct result. Don't you know what might be the problem?
Thanks in advance, JP