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.h>
00012 
00013 using namespace mrpt::poses;
00014 using namespace mrpt::math;
00015 
00016 void pose_cov_ops::compose(const geometry_msgs::Pose& a, const geometry_msgs::Pose& b, geometry_msgs::Pose& out)
00017 {
00018         CPose3D A(UNINITIALIZED_POSE),B(UNINITIALIZED_POSE),OUT(UNINITIALIZED_POSE);
00019 
00020         mrpt_bridge::poses::ros2mrpt(a,A);
00021         mrpt_bridge::poses::ros2mrpt(b,B);
00022 
00023         OUT.composeFrom(A,B);
00024         mrpt_bridge::poses::mrpt2ros(OUT,out);
00025 }
00026 void pose_cov_ops::compose(const geometry_msgs::PoseWithCovariance& a, const geometry_msgs::PoseWithCovariance& b, geometry_msgs::PoseWithCovariance& out)
00027 {
00028         CPose3DPDFGaussian  A(UNINITIALIZED_POSE),B(UNINITIALIZED_POSE);
00029 
00030         mrpt_bridge::poses::ros2mrpt(a,A);
00031         mrpt_bridge::poses::ros2mrpt(b,B);
00032 
00033         const CPose3DPDFGaussian OUT = A + B;
00034         mrpt_bridge::poses::mrpt2ros(OUT,out);
00035 }
00036 void pose_cov_ops::compose(const geometry_msgs::PoseWithCovariance& a, const geometry_msgs::Pose& b, geometry_msgs::PoseWithCovariance& out)
00037 {
00038         CPose3DPDFGaussian A(UNINITIALIZED_POSE);
00039         CPose3D            B(UNINITIALIZED_POSE);
00040 
00041         mrpt_bridge::poses::ros2mrpt(a,A);
00042         mrpt_bridge::poses::ros2mrpt(b,B);
00043 
00044         A+=B;
00045         mrpt_bridge::poses::mrpt2ros(A,out);
00046 }
00047 void pose_cov_ops::compose(const geometry_msgs::Pose& a, const geometry_msgs::PoseWithCovariance& b, geometry_msgs::PoseWithCovariance& out)
00048 {
00049         CPose3D            A(UNINITIALIZED_POSE);
00050         CPose3DPDFGaussian B(UNINITIALIZED_POSE);
00051 
00052         mrpt_bridge::poses::ros2mrpt(a,A);
00053         mrpt_bridge::poses::ros2mrpt(b,B);
00054 
00055         B.changeCoordinatesReference(A);// b = a (+) b
00056         mrpt_bridge::poses::mrpt2ros(B,out);
00057 }
00058 
00059 
00060 
00061 void pose_cov_ops::inverseCompose(const geometry_msgs::Pose& a, const geometry_msgs::Pose& b, geometry_msgs::Pose& out)
00062 {
00063         CPose3D A(UNINITIALIZED_POSE),B(UNINITIALIZED_POSE),OUT(UNINITIALIZED_POSE);
00064 
00065         mrpt_bridge::poses::ros2mrpt(a,A);
00066         mrpt_bridge::poses::ros2mrpt(b,B);
00067 
00068         OUT.inverseComposeFrom(A,B);
00069         mrpt_bridge::poses::mrpt2ros(OUT,out);
00070 }
00071 
00072 void pose_cov_ops::inverseCompose(const geometry_msgs::PoseWithCovariance& a, const geometry_msgs::PoseWithCovariance& b, geometry_msgs::PoseWithCovariance& out)
00073 {
00074         CPose3DPDFGaussian  A(UNINITIALIZED_POSE),B(UNINITIALIZED_POSE);
00075 
00076         mrpt_bridge::poses::ros2mrpt(a,A);
00077         mrpt_bridge::poses::ros2mrpt(b,B);
00078 
00079         const CPose3DPDFGaussian OUT = A - B;
00080         mrpt_bridge::poses::mrpt2ros(OUT,out);
00081 }
00082 void pose_cov_ops::inverseCompose(const geometry_msgs::PoseWithCovariance& a, const geometry_msgs::Pose& b, geometry_msgs::PoseWithCovariance& out)
00083 {
00084         CPose3DPDFGaussian A(UNINITIALIZED_POSE);
00085         CPose3D            B_mean(UNINITIALIZED_POSE);
00086 
00087         mrpt_bridge::poses::ros2mrpt(a,A);
00088         mrpt_bridge::poses::ros2mrpt(b,B_mean);
00089 
00090         const CPose3DPDFGaussian B(B_mean, CMatrixDouble66() ); // Cov=zeros
00091 
00092         const CPose3DPDFGaussian OUT = A - B;
00093         mrpt_bridge::poses::mrpt2ros(OUT,out);
00094 }
00095 void pose_cov_ops::inverseCompose(const geometry_msgs::Pose& a, const geometry_msgs::PoseWithCovariance& b, geometry_msgs::PoseWithCovariance& out)
00096 {
00097         CPose3D            A_mean(UNINITIALIZED_POSE);
00098         CPose3DPDFGaussian B(UNINITIALIZED_POSE);
00099 
00100         mrpt_bridge::poses::ros2mrpt(a,A_mean);
00101         mrpt_bridge::poses::ros2mrpt(b,B);
00102 
00103         const CPose3DPDFGaussian A(A_mean, CMatrixDouble66() ); // Cov=zeros
00104 
00105         const CPose3DPDFGaussian OUT = A - B;
00106         mrpt_bridge::poses::mrpt2ros(OUT,out);
00107 }