Gazebo, camera 3d point to pixel
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)
Please edit your question and add the code in question, otherwise it is impossible to say where the issue might lie.