[ros-users] rotation problem on published tf links related to a re-projected opencv checkerboard in rviz

[Markus Bader]

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)
-------------- next part --------------
A non-text attachment was scrubbed...
Name: roation_problem.jpg
Type: image/jpeg
Size: 42424 bytes
Desc: not available
URL: <http://lists.osuosl.org/pipermail/ros-users/attachments/20101209/2b46a5ca/attachment-0004.jpg>