Gazebo, camera 3d point to pixel

edit

Hi guy's,

I have implemented a gazebo model with a stereo camera plugin. I have got a seconde model that I would like to mark in the left image of the stereo camera. I use tf to transform the zero point from the second model to the left camera coordinate system. With project3dToPixel from image geometry::PinholeCameraModel I convert the 3d coordinats to the pixel coordinats.

The problem; they don't realy match. The trend is correct, if i move the second model to the left the marker also goes to the left. I have also performed a camera calibration to rule out possible errors in the projection matrix.

Have you tried the same thing before? What am I doing wrong? Are there other solutions?

Here is a part of the code:

cloud_model.points.push_back(pcl::PointXYZ(0,0,0));

listener.waitForTransform("/left_camera_optical_frame", "/model2", t1, ros::Duration(0.5));

pcl_ros::transformPointCloud("/left_camera_optical_frame", cloud_model, cloud_model_trans, listener)

....
{
boost::mutex::scoped_lock lock(mutex_);
point = pinholeCameraModel_.project3dToPixel(cv::Point3d(cloud_model_trans[0].x,cloud_model_trans[0].y,cloud_model_trans[0].z));
}

left_top.x = (point.x+2);
left_top.y = (point.y+2);

right_bot.x = (point.x);
right_bot.y = (point.y);

cv::Mat m_mat= cv_bridge::toCvShare(msg, "bgr8")->image;
cv::rectangle(m_mat, left_top, right_bot, cv::Scalar(255, 0, 0), 2, 8);

in the camerainfo callback I initialize the pinholeCameraModel_

boost::mutex::scoped_lock lock(mutex_);
pinholeCameraModel_.fromCameraInfo(msg)