aruco_ros_utils.cpp
Go to the documentation of this file.
00001 #include <aruco_ros/aruco_ros_utils.h>
00002 #include <ros/console.h>
00003 #include <ros/assert.h>
00004 #include <iostream>
00005 #include <tf/transform_datatypes.h>
00006 
00007 aruco::CameraParameters aruco_ros::rosCameraInfo2ArucoCamParams(const sensor_msgs::CameraInfo& cam_info,
00008                                                                 bool useRectifiedParameters)
00009 {
00010     cv::Mat cameraMatrix(3, 3, CV_64FC1);
00011     cv::Mat distorsionCoeff(4, 1, CV_64FC1);
00012     cv::Size size(cam_info.height, cam_info.width);
00013 
00014     if ( useRectifiedParameters )
00015     {
00016       cameraMatrix.setTo(0);
00017       cameraMatrix.at<double>(0,0) = cam_info.P[0];   cameraMatrix.at<double>(0,1) = cam_info.P[1];   cameraMatrix.at<double>(0,2) = cam_info.P[2];
00018       cameraMatrix.at<double>(1,0) = cam_info.P[4];   cameraMatrix.at<double>(1,1) = cam_info.P[5];   cameraMatrix.at<double>(1,2) = cam_info.P[6];
00019       cameraMatrix.at<double>(2,0) = cam_info.P[8];   cameraMatrix.at<double>(2,1) = cam_info.P[9];   cameraMatrix.at<double>(2,2) = cam_info.P[10];
00020 
00021       for(int i=0; i<4; ++i)
00022         distorsionCoeff.at<double>(i, 0) = 0;
00023     }
00024     else
00025     {
00026       for(int i=0; i<9; ++i)
00027         cameraMatrix.at<double>(i%3, i-(i%3)*3) = cam_info.K[i];
00028 
00029       if(cam_info.D.size() == 4)
00030       {
00031         for(int i=0; i<4; ++i)
00032           distorsionCoeff.at<double>(i, 0) = cam_info.D[i];
00033       }
00034       else
00035       {
00036         ROS_WARN("length of camera_info D vector is not 4, assuming zero distortion...");
00037         for(int i=0; i<4; ++i)
00038           distorsionCoeff.at<double>(i, 0) = 0;
00039       }
00040     }
00041 
00042     return aruco::CameraParameters(cameraMatrix, distorsionCoeff, size);
00043 }
00044 
00045 tf::Transform aruco_ros::arucoMarker2Tf(const aruco::Marker &marker)
00046 {
00047     cv::Mat rot(3, 3, CV_64FC1);
00048     cv::Mat Rvec64;
00049     marker.Rvec.convertTo(Rvec64, CV_64FC1);
00050     cv::Rodrigues(Rvec64, rot);
00051     cv::Mat tran64;
00052     marker.Tvec.convertTo(tran64, CV_64FC1);
00053 
00054     cv::Mat rotate_to_ros(3, 3, CV_64FC1);
00055     // -1 0 0
00056     // 0 0 1
00057     // 0 1 0
00058     rotate_to_ros.at<double>(0,0) = -1.0;
00059     rotate_to_ros.at<double>(0,1) = 0.0;
00060     rotate_to_ros.at<double>(0,2) = 0.0;
00061     rotate_to_ros.at<double>(1,0) = 0.0;
00062     rotate_to_ros.at<double>(1,1) = 0.0;
00063     rotate_to_ros.at<double>(1,2) = 1.0;
00064     rotate_to_ros.at<double>(2,0) = 0.0;
00065     rotate_to_ros.at<double>(2,1) = 1.0;
00066     rotate_to_ros.at<double>(2,2) = 0.0;
00067     rot = rot*rotate_to_ros.t();
00068 
00069     tf::Matrix3x3 tf_rot(rot.at<double>(0,0), rot.at<double>(0,1), rot.at<double>(0,2),
00070                          rot.at<double>(1,0), rot.at<double>(1,1), rot.at<double>(1,2),
00071                          rot.at<double>(2,0), rot.at<double>(2,1), rot.at<double>(2,2));
00072 
00073     tf::Vector3 tf_orig(tran64.at<double>(0,0), tran64.at<double>(1,0), tran64.at<double>(2,0));
00074 
00075 
00076     return tf::Transform(tf_rot, tf_orig);
00077 }


aruco_ros
Author(s): Rafael Muñoz Salinas , Bence Magyar
autogenerated on Thu Jun 6 2019 17:52:20