tf::Transform aruco_ros::arucoMarker2Tf(const aruco::Marker &marker) { cv::Mat rot(3, 3, CV_32FC1); cv::Rodrigues(marker.Rvec, rot); cv::Mat tran = marker.Tvec; cv::Mat rotate_to_ros(3, 3, CV_32FC1); // -1 0 0 // 0 0 1 // 0 1 0 rotate_to_ros.at<float>(0,0) = -1.0; rotate_to_ros.at<float>(0,1) = 0.0; rotate_to_ros.at<float>(0,2) = 0.0; rotate_to_ros.at<float>(1,0) = 0.0; rotate_to_ros.at<float>(1,1) = 0.0; rotate_to_ros.at<float>(1,2) = 1.0; rotate_to_ros.at<float>(2,0) = 0.0; rotate_to_ros.at<float>(2,1) = 1.0; rotate_to_ros.at<float>(2,2) = 0.0; rot = rot*rotate_to_ros.t(); tf::Matrix3x3 tf_rot(rot.at<float>(0,0), rot.at<float>(0,1), rot.at<float>(0,2), rot.at<float>(1,0), rot.at<float>(1,1), rot.at<float>(1,2), rot.at<float>(2,0), rot.at<float>(2,1), rot.at<float>(2,2)); tf::Vector3 tf_orig(tran.at<float>(0,0), tran.at<float>(1,0), tran.at<float>(2,0)); return tf::Transform(tf_rot, tf_orig); }
tf::Transform aruco_ros::arucoMarker2Tf(const aruco::Marker &marker) { cv::Mat rot(3, 3, CV_64FC1); cv::Mat Rvec64; marker.Rvec.convertTo(Rvec64, CV_64FC1); cv::Rodrigues(Rvec64, rot); cv::Mat tran64; marker.Tvec.convertTo(tran64, CV_64FC1); cv::Mat rotate_to_ros(3, 3, CV_64FC1); // -1 0 0 // 0 0 1 // 0 1 0 rotate_to_ros.at<double>(0,0) = -1.0; rotate_to_ros.at<double>(0,1) = 0.0; rotate_to_ros.at<double>(0,2) = 0.0; rotate_to_ros.at<double>(1,0) = 0.0; rotate_to_ros.at<double>(1,1) = 0.0; rotate_to_ros.at<double>(1,2) = 1.0; rotate_to_ros.at<double>(2,0) = 0.0; rotate_to_ros.at<double>(2,1) = 1.0; rotate_to_ros.at<double>(2,2) = 0.0; rot = rot*rotate_to_ros.t(); tf::Matrix3x3 tf_rot(rot.at<double>(0,0), rot.at<double>(0,1), rot.at<double>(0,2), rot.at<double>(1,0), rot.at<double>(1,1), rot.at<double>(1,2), rot.at<double>(2,0), rot.at<double>(2,1), rot.at<double>(2,2)); tf::Vector3 tf_orig(tran64.at<double>(0,0), tran64.at<double>(1,0), tran64.at<double>(2,0)); return tf::Transform(tf_rot, tf_orig); }