Project point cloud to image coordinates using camerainfo

edit

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