pose_cov_ops: pose_cov_ops.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * pose_cov_ops.cpp
00003  *
00004  *  Created on: Mar 25, 2012
00005  *      Author: JLBC
00006  *
00007  */
00008 
00009 #include "pose_cov_ops/pose_cov_ops.h"
00010 
00011 #include <mrpt/poses/CPose3D.h>
00012 #include <mrpt/poses/CPose3DPDFGaussian.h>
00013 
00014 using namespace mrpt::poses;
00015 using namespace mrpt::math;
00016 
00017 void pose_cov_ops::compose(const geometry_msgs::Pose &a,
00018                            const geometry_msgs::Pose &b,
00019                            geometry_msgs::Pose &out) {
00020   CPose3D A(UNINITIALIZED_POSE), B(UNINITIALIZED_POSE), OUT(UNINITIALIZED_POSE);
00021 
00022   mrpt_bridge::convert(a, A);
00023   mrpt_bridge::convert(b, B);
00024 
00025   OUT.composeFrom(A, B);
00026   mrpt_bridge::convert(OUT, out);
00027 }
00028 void pose_cov_ops::compose(const geometry_msgs::PoseWithCovariance &a,
00029                            const geometry_msgs::PoseWithCovariance &b,
00030                            geometry_msgs::PoseWithCovariance &out) {
00031   CPose3DPDFGaussian A(UNINITIALIZED_POSE), B(UNINITIALIZED_POSE);
00032 
00033   mrpt_bridge::convert(a, A);
00034   mrpt_bridge::convert(b, B);
00035 
00036   const CPose3DPDFGaussian OUT = A + B;
00037   mrpt_bridge::convert(OUT, out);
00038 }
00039 void pose_cov_ops::compose(const geometry_msgs::PoseWithCovariance &a,
00040                            const geometry_msgs::Pose &b,
00041                            geometry_msgs::PoseWithCovariance &out) {
00042   CPose3DPDFGaussian A(UNINITIALIZED_POSE);
00043   CPose3D B(UNINITIALIZED_POSE);
00044 
00045   mrpt_bridge::convert(a, A);
00046   mrpt_bridge::convert(b, B);
00047 
00048   A += B;
00049   mrpt_bridge::convert(A, out);
00050 }
00051 void pose_cov_ops::compose(const geometry_msgs::Pose &a,
00052                            const geometry_msgs::PoseWithCovariance &b,
00053                            geometry_msgs::PoseWithCovariance &out) {
00054   CPose3D A(UNINITIALIZED_POSE);
00055   CPose3DPDFGaussian B(UNINITIALIZED_POSE);
00056 
00057   mrpt_bridge::convert(a, A);
00058   mrpt_bridge::convert(b, B);
00059 
00060   B.changeCoordinatesReference(A); // b = a (+) b
00061   mrpt_bridge::convert(B, out);
00062 }
00063 
00064 void pose_cov_ops::inverseCompose(const geometry_msgs::Pose &a,
00065                                   const geometry_msgs::Pose &b,
00066                                   geometry_msgs::Pose &out) {
00067   CPose3D A(UNINITIALIZED_POSE), B(UNINITIALIZED_POSE), OUT(UNINITIALIZED_POSE);
00068 
00069   mrpt_bridge::convert(a, A);
00070   mrpt_bridge::convert(b, B);
00071 
00072   OUT.inverseComposeFrom(A, B);
00073   mrpt_bridge::convert(OUT, out);
00074 }
00075 
00076 void pose_cov_ops::inverseCompose(const geometry_msgs::PoseWithCovariance &a,
00077                                   const geometry_msgs::PoseWithCovariance &b,
00078                                   geometry_msgs::PoseWithCovariance &out) {
00079   CPose3DPDFGaussian A(UNINITIALIZED_POSE), B(UNINITIALIZED_POSE);
00080 
00081   mrpt_bridge::convert(a, A);
00082   mrpt_bridge::convert(b, B);
00083 
00084   const CPose3DPDFGaussian OUT = A - B;
00085   mrpt_bridge::convert(OUT, out);
00086 }
00087 void pose_cov_ops::inverseCompose(const geometry_msgs::PoseWithCovariance &a,
00088                                   const geometry_msgs::Pose &b,
00089                                   geometry_msgs::PoseWithCovariance &out) {
00090   CPose3DPDFGaussian A(UNINITIALIZED_POSE);
00091   CPose3D B_mean(UNINITIALIZED_POSE);
00092 
00093   mrpt_bridge::convert(a, A);
00094   mrpt_bridge::convert(b, B_mean);
00095 
00096   const CPose3DPDFGaussian B(B_mean, CMatrixDouble66()); // Cov=zeros
00097 
00098   const CPose3DPDFGaussian OUT = A - B;
00099   mrpt_bridge::convert(OUT, out);
00100 }
00101 void pose_cov_ops::inverseCompose(const geometry_msgs::Pose &a,
00102                                   const geometry_msgs::PoseWithCovariance &b,
00103                                   geometry_msgs::PoseWithCovariance &out) {
00104   CPose3D A_mean(UNINITIALIZED_POSE);
00105   CPose3DPDFGaussian B(UNINITIALIZED_POSE);
00106 
00107   mrpt_bridge::convert(a, A_mean);
00108   mrpt_bridge::convert(b, B);
00109 
00110   const CPose3DPDFGaussian A(A_mean, CMatrixDouble66()); // Cov=zeros
00111 
00112   const CPose3DPDFGaussian OUT = A - B;
00113   mrpt_bridge::convert(OUT, out);
00114 }