pose.cpp
Go to the documentation of this file.
00001 /*
00002  * pose.cpp
00003  *
00004  *  Created on: Mar 14, 2012
00005  *      Author: Pablo IƱigo Blasco
00006  *
00007  *      To understand better how this is implemented see the references:
00008  *      - http://www.mrpt.org/2D_3D_Geometry
00009  *
00010  */
00011 
00012 #include <mrpt/poses/CPose2D.h>
00013 #include <mrpt/poses/CPose3D.h>
00014 #include <mrpt/poses/CPosePDFGaussian.h>
00015 #include <mrpt/poses/CPose3DPDFGaussian.h>
00016 #include <mrpt/math/CQuaternion.h>
00017 #include <geometry_msgs/PoseWithCovariance.h>
00018 #include <geometry_msgs/Pose.h>
00019 #include <geometry_msgs/Quaternion.h>
00020 #include <mrpt/math/CMatrixFixedNumeric.h>
00021 #include <tf/tf.h>
00022 #include "mrpt_bridge/pose.h"
00023 
00024 mrpt::math::CMatrixDouble33 &mrpt_bridge::convert( const tf::Matrix3x3& _src, mrpt::math::CMatrixDouble33& _des){
00025     MRPT_TODO("MRPT this can be done faster with memcopy, but one has to check the obj structure!");
00026     for(int r = 0; r < 3; r++)
00027         for(int c = 0; c < 3; c++)
00028             _des(r,c) = _src[r][c];
00029     return _des;
00030 }
00031 
00032 tf::Matrix3x3 &mrpt_bridge::convert( const mrpt::math::CMatrixDouble33& _src, tf::Matrix3x3& _des){
00033     MRPT_TODO("MRPT this can be done faster with memcopy, but one has to check the obj structure!");
00034     for(int r = 0; r < 3; r++)
00035         for(int c = 0; c < 3; c++)
00036              _des[r][c] = _src(r,c);
00037     return _des;
00038 }
00039 
00040 tf::Transform& mrpt_bridge::convert( const mrpt::poses::CPose3DPDFGaussian& _src, tf::Transform& _des){
00041     return convert(_src.mean, _des);
00042 }
00043 
00044 geometry_msgs::PoseWithCovariance& mrpt_bridge::convert(const mrpt::poses::CPose3DPDFGaussian& _src, geometry_msgs::PoseWithCovariance& _des)
00045 {
00046   convert(_src.mean, _des.pose);
00047 
00048   // Read REP103: http://ros.org/reps/rep-0103.html#covariance-representation
00049   // # Row-major representation of the 6x6 covariance matrix
00050   // # The orientation parameters use a fixed-axis representation.
00051   // # In order, the parameters are:
00052   // # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
00053   // float64[36] covariance
00054   MRPT_TODO("MRPT uses non-fixed axis for 6x6 covariance: should use a transform Jacobian here!")
00055 
00056   const unsigned int indxs_map[6] = {0, 1, 2, 5, 4, 3};
00057 
00058   for (int i = 0; i < 6; i++)
00059     for (int j = 0; j < 6; j++)
00060       _des.covariance[indxs_map[i] * 6 + indxs_map[j]] = _src.cov(i, j);
00061   return _des;
00062 }
00063 
00064 tf::Transform &mrpt_bridge::convert(const mrpt::poses::CPose3D& _src, tf::Transform &_des){
00065     tf::Vector3 origin(_src[0], _src[1], _src[2]);
00066     mrpt::math::CMatrixDouble33 R;
00067     _src.getRotationMatrix(R);
00068     tf::Matrix3x3 basis;
00069     _des.setBasis(convert(R,basis));
00070     _des.setOrigin(origin);
00071     return _des;
00072 }
00073 mrpt::poses::CPose3D &mrpt_bridge::convert(const tf::Transform &_src, mrpt::poses::CPose3D &_des){
00074     const tf::Vector3 &t = _src.getOrigin();
00075     _des.x() = t[0], _des.y() = t[1], _des.z() = t[2];
00076     const tf::Matrix3x3 &basis = _src.getBasis();
00077     mrpt::math::CMatrixDouble33 R;
00078     convert(basis, R);
00079     _des.setRotationMatrix(R);
00080     return _des;
00081 }
00082 
00083 geometry_msgs::Pose &mrpt_bridge::convert(const mrpt::poses::CPose3D& _src, geometry_msgs::Pose& _des)
00084 {
00085   _des.position.x = _src[0];
00086   _des.position.y = _src[1];
00087   _des.position.z = _src[2];
00088 
00089   mrpt::math::CQuaternionDouble q;
00090   _src.getAsQuaternion(q);
00091 
00092   _des.orientation.x = q.x();
00093   _des.orientation.y = q.y();
00094   _des.orientation.z = q.z();
00095   _des.orientation.w = q.r();
00096   return _des;
00097 }
00098 
00100 geometry_msgs::Pose& mrpt_bridge::convert(const mrpt::poses::CPose2D& _src, geometry_msgs::Pose& _des)
00101 {
00102   _des.position.x = _src.x();
00103   _des.position.y = _src.y();
00104   _des.position.z = 0;
00105 
00106   const double yaw = _src.phi();
00107   if (std::abs(yaw) < 1e-10)
00108   {
00109     _des.orientation.x = 0.;
00110     _des.orientation.y = 0.;
00111     _des.orientation.z = .5 * yaw;
00112     _des.orientation.w = 1.;
00113   }
00114   else
00115   {
00116     const double s = ::sin(yaw * .5);
00117     const double c = ::cos(yaw * .5);
00118     _des.orientation.x = 0.;
00119     _des.orientation.y = 0.;
00120     _des.orientation.z = s;
00121     _des.orientation.w = c;
00122   }
00123   return _des;
00124 }
00125 
00126 mrpt::poses::CPose2D& mrpt_bridge::convert(const geometry_msgs::Pose& _src, mrpt::poses::CPose2D& _des)
00127 {
00128   _des.x(_src.position.x);
00129   _des.y(_src.position.y);
00130 
00131   MRPT_TODO("Optimize this?");
00132   mrpt::math::CQuaternionDouble quat;
00133   convert(_src.orientation, quat);
00134 
00135   double roll, pitch, yaw;
00136   quat.rpy(roll, pitch, yaw);
00137 
00138   _des.phi(yaw);
00139   return _des;
00140 }
00141 
00142 geometry_msgs::PoseWithCovariance& mrpt_bridge::convert(const mrpt::poses::CPosePDFGaussian& _src, geometry_msgs::PoseWithCovariance& _des)
00143 {
00144   convert(_src.mean, _des.pose);
00145 
00146   // Read REP103: http://ros.org/reps/rep-0103.html#covariance-representation
00147   MRPT_TODO("MRPT uses non-fixed axis for 6x6 covariance: should use a transform Jacobian here!")
00148 
00149   for (int i = 0; i < 6; i++)
00150     for (int j = 0; j < 6; j++)
00151     {
00152       double cov_val;
00153       int ros_i = i;
00154       int ros_j = j;
00155       if (i > 2 || j > 2)
00156         cov_val = 0;
00157       else
00158       {
00159         ros_i = i == 2 ? 5 : i;
00160         ros_j = j == 2 ? 5 : j;
00161         cov_val = _src.cov(i, j);
00162       }
00163       _des.covariance[ros_i * 6 + ros_j] = cov_val;
00164     }
00165   return _des;
00166 }
00167 
00168 mrpt::poses::CPose3DPDFGaussian& mrpt_bridge::convert(const geometry_msgs::PoseWithCovariance& _src, mrpt::poses::CPose3DPDFGaussian& _des)
00169 {
00170   convert(_src.pose, _des.mean);
00171 
00172   const unsigned int indxs_map[6] = {0, 1, 2, 5, 4, 3};
00173 
00174   for (int i = 0; i < 6; i++)
00175     for (int j = 0; j < 6; j++)
00176       _des.cov(i, j) = _src.covariance[indxs_map[i] * 6 + indxs_map[j]];
00177   return _des;
00178 }
00179 
00180 mrpt::poses::CPosePDFGaussian& mrpt_bridge::convert(const geometry_msgs::PoseWithCovariance& _src, mrpt::poses::CPosePDFGaussian& _des)
00181 {
00182   convert(_src.pose, _des.mean);
00183   _des.cov(0, 0) = _src.covariance[0];
00184   _des.cov(0, 1) = _src.covariance[1];
00185   _des.cov(0, 2) = _src.covariance[5];
00186   _des.cov(1, 0) = _src.covariance[0+6];
00187   _des.cov(1, 1) = _src.covariance[1+6];
00188   _des.cov(1, 2) = _src.covariance[5+6];
00189   _des.cov(2, 0) = _src.covariance[0+30];
00190   _des.cov(2, 1) = _src.covariance[1+30];
00191   _des.cov(2, 2) = _src.covariance[5+30];
00192   return _des;
00193 }
00194 mrpt::poses::CQuaternionDouble&  mrpt_bridge::convert(const geometry_msgs::Quaternion& _src, mrpt::poses::CQuaternionDouble& _des)
00195 {
00196   _des.x(_src.x);
00197   _des.y(_src.y);
00198   _des.z(_src.z);
00199   _des.r(_src.w);
00200   return _des;
00201 }
00202 
00203 geometry_msgs::Quaternion& mrpt_bridge::convert(const mrpt::poses::CQuaternionDouble& _src, geometry_msgs::Quaternion& _des)
00204 {
00205   _des.x=_src.x();
00206   _des.y=_src.y();
00207   _des.z=_src.z();
00208   _des.w=_src.r();
00209   return _des;
00210 }
00211 
00212 mrpt::poses::CPose3D& mrpt_bridge::convert(const geometry_msgs::Pose& _src, mrpt::poses::CPose3D& _des)
00213 {
00214   const mrpt::math::CQuaternionDouble q(_src.orientation.w, _src.orientation.x, _src.orientation.y, _src.orientation.z);
00215     _des= mrpt::math::CPose3D(q,_src.position.x,_src.position.y,_src.position.z);
00216     return _des;
00217 }
00218 
00219 


mrpt_bridge
Author(s):
autogenerated on Tue Aug 5 2014 10:58:06