pose_cov_ops.cpp
Go to the documentation of this file.
1 /*
2  * pose_cov_ops.cpp
3  *
4  * Created on: Mar 25, 2012
5  * Author: JLBC
6  *
7  */
8 
10 
11 #include <mrpt/poses/CPose3D.h>
13 
14 using namespace mrpt::poses;
15 using namespace mrpt::math;
16 
18  const geometry_msgs::Pose &b,
19  geometry_msgs::Pose &out) {
21 
24 
25  OUT.composeFrom(A, B);
26  mrpt_bridge::convert(OUT, out);
27 }
32 
35 
36  const CPose3DPDFGaussian OUT = A + B;
37  mrpt_bridge::convert(OUT, out);
38 }
40  const geometry_msgs::Pose &b,
44 
47 
48  A += B;
49  mrpt_bridge::convert(A, out);
50 }
56 
59 
60  B.changeCoordinatesReference(A); // b = a (+) b
61  mrpt_bridge::convert(B, out);
62 }
63 
65  const geometry_msgs::Pose &b,
66  geometry_msgs::Pose &out) {
68 
71 
72  OUT.inverseComposeFrom(A, B);
73  mrpt_bridge::convert(OUT, out);
74 }
75 
80 
83 
84  const CPose3DPDFGaussian OUT = A - B;
85  mrpt_bridge::convert(OUT, out);
86 }
88  const geometry_msgs::Pose &b,
92 
94  mrpt_bridge::convert(b, B_mean);
95 
96  const CPose3DPDFGaussian B(B_mean, CMatrixDouble66()); // Cov=zeros
97 
98  const CPose3DPDFGaussian OUT = A - B;
99  mrpt_bridge::convert(OUT, out);
100 }
104  CPose3D A_mean(UNINITIALIZED_POSE);
106 
107  mrpt_bridge::convert(a, A_mean);
108  mrpt_bridge::convert(b, B);
109 
110  const CPose3DPDFGaussian A(A_mean, CMatrixDouble66()); // Cov=zeros
111 
112  const CPose3DPDFGaussian OUT = A - B;
113  mrpt_bridge::convert(OUT, out);
114 }
GLboolean GLboolean GLboolean GLboolean a
void compose(const geometry_msgs::Pose &a, const geometry_msgs::Pose &b, geometry_msgs::Pose &out)
void composeFrom(const CPose3D &A, const CPose3D &B)
void changeCoordinatesReference(const CPose3D &newReferenceBase) MRPT_OVERRIDE
void convert(const ros::Time &src, mrpt::system::TTimeStamp &des)
void inverseCompose(const geometry_msgs::Pose &a, const geometry_msgs::Pose &b, geometry_msgs::Pose &out)
GLdouble GLdouble GLdouble b
void inverseComposeFrom(const CPose3D &A, const CPose3D &B)


pose_cov_ops
Author(s):
autogenerated on Tue Dec 24 2019 04:03:31