[ros-users] rotation problem on published tf links related t…

Hello

I am using the opencv to find a checkerboard in a distorted image with
known parameters (distortions, fx, fy, cx, dy).
    cv::Mat_<double> rvec(3,0),  tvec(3,0);
    cv::solvePnP(mObjectPoints, mImagePoints, mCameraMatrix,
mDistCoeffs, rvec, tvec);

So I found all I needed to re-project a coordinate cross (rvec, tvec
to the checkerboard) and it worked.

cv::Mat_<double> Mext = cv::Mat_<double>::eye(4,4);
RT(0,3) = tvec.ptr<double>()[0];
RT(1,3) = tvec.ptr<double>()[1];
RT(2,3) = tvec.ptr<double>()[2];
cv::Mat Rc(RT, cv::Rect(0, 0, 3, 3));
cv::Rodrigues(rvec, Rc);
cv::Mat_<double> Pw0 = ( cv::Mat_<double> ( 4,1 ) << 0, 0, 0, 1 );
cv::Mat_<double> Pc0 = Mext * Pw0;
cv::Mat_<double> Pi0 = Mint * Pc0;
cv::Point2d pi0 ( Pi0 ( 0,0 ) / Pi0 ( 0,2 ), Pi0 ( 0,1 ) / Pi0 ( 0,2 ) );
cv::circle ( img, pi0, 3, CV_RGB ( 255,255,255 ) );

But If I to use rviz to show the image, the tf_link rotation does not
fit, the position is OK.
I created the tf by using the rvec and tvec without any translation or rotation

tf::Quaternion q = tf::createQuaternionFromRPY ( rvec( 0, 0),
rvec(1, 0), rvec( 2, 0) );
tf::Vector3 pos = tf::Vector3 ( tvec( 0, 0), tvec( 1, 0), tvec( 2, 0) );
ros::Time stamp = ros::Time::now();
mTF_broadcaster.sendTransform (tf::StampedTransform (tf::Transform (
q, pos ), stamp, "base_link", "checkerboard_link") );

The camera is located at a static tf link called "camera" which is
located at the base_link using a static_transform_publisher with
args="0 0 0 0 0 0 base_link camera 10"

Any ideas?

Greetings
Max

PS.: There is a picture of the problem attached (I hope it works)