utils.cpp
Go to the documentation of this file.
00001 
00009 #include <ar_sys/utils.h>
00010 #include <ros/console.h>
00011 #include <iostream>
00012 #include <tf/transform_datatypes.h>
00013 #include <opencv2/calib3d/calib3d.hpp>
00014 
00015 aruco::CameraParameters ar_sys::getCamParams(const sensor_msgs::CameraInfo& cam_info,
00016         bool useRectifiedParameters)
00017 {
00018         cv::Mat cameraMatrix(3, 3, CV_32FC1);
00019         cv::Mat distorsionCoeff(4, 1, CV_32FC1);
00020         cv::Size size(cam_info.height, cam_info.width);
00021 
00022         if ( useRectifiedParameters )
00023         {
00024                 cameraMatrix.setTo(0);
00025                 cameraMatrix.at<float>(0,0) = cam_info.P[0]; cameraMatrix.at<float>(0,1) = cam_info.P[1]; cameraMatrix.at<float>(0,2) = cam_info.P[2];
00026                 cameraMatrix.at<float>(1,0) = cam_info.P[4]; cameraMatrix.at<float>(1,1) = cam_info.P[5]; cameraMatrix.at<float>(1,2) = cam_info.P[6];
00027                 cameraMatrix.at<float>(2,0) = cam_info.P[8]; cameraMatrix.at<float>(2,1) = cam_info.P[9]; cameraMatrix.at<float>(2,2) = cam_info.P[10];
00028 
00029                 for(int i=0; i<4; ++i)
00030                         distorsionCoeff.at<float>(i, 0) = 0;
00031         }
00032         else
00033         {
00034                 for(int i=0; i<9; ++i)
00035                         cameraMatrix.at<float>(i%3, i-(i%3)*3) = cam_info.K[i];
00036 
00037                 for(int i=0; i<4; ++i)
00038                         distorsionCoeff.at<float>(i, 0) = cam_info.D[i];
00039         }
00040 
00041         return aruco::CameraParameters(cameraMatrix, distorsionCoeff, size);
00042 }
00043 
00044 tf::Transform ar_sys::getTf(const cv::Mat &Rvec, const cv::Mat &Tvec)
00045 {
00046         cv::Mat rot(3, 3, CV_32FC1);
00047         cv::Rodrigues(Rvec, rot);
00048 
00049         cv::Mat rotate_to_sys(3, 3, CV_32FC1);
00057         //      1       0       0
00058         //      0       -1      0
00059         //      0       0       -1
00060         rotate_to_sys.at<float>(0,0) = 1.0;
00061         rotate_to_sys.at<float>(0,1) = 0.0;
00062         rotate_to_sys.at<float>(0,2) = 0.0;
00063         rotate_to_sys.at<float>(1,0) = 0.0;
00064         rotate_to_sys.at<float>(1,1) = -1.0;
00065         rotate_to_sys.at<float>(1,2) = 0.0;
00066         rotate_to_sys.at<float>(2,0) = 0.0;
00067         rotate_to_sys.at<float>(2,1) = 0.0;
00068         rotate_to_sys.at<float>(2,2) = -1.0;
00069         rot = rot*rotate_to_sys.t();
00070 
00071         tf::Matrix3x3 tf_rot(rot.at<float>(0,0), rot.at<float>(0,1), rot.at<float>(0,2),
00072                 rot.at<float>(1,0), rot.at<float>(1,1), rot.at<float>(1,2),
00073                 rot.at<float>(2,0), rot.at<float>(2,1), rot.at<float>(2,2));
00074 
00075         tf::Vector3 tf_orig(Tvec.at<float>(0,0), Tvec.at<float>(1,0), Tvec.at<float>(2,0));
00076 
00077         return tf::Transform(tf_rot, tf_orig);
00078 }


ar_sys
Author(s): Hamdi Sahloul , Rafael Muñoz Salinas , Bence Magyar
autogenerated on Sun Oct 5 2014 22:12:49