Main MRPT website > C++ reference for MRPT 1.9.9
se3_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <mrpt/tfest.h>
11 #include <mrpt/random.h>
12 #include <mrpt/math/ops_vectors.h>
13 #include <mrpt/poses/CPose3D.h>
14 #include <mrpt/poses/CPose3DQuat.h>
15 #include <gtest/gtest.h>
16 
17 using namespace mrpt;
18 using namespace mrpt::tfest;
19 using namespace mrpt::math;
20 using namespace mrpt::random;
21 using namespace mrpt::poses;
22 using namespace std;
23 
24 using TPoints = std::vector<std::vector<double>>;
25 
26 // ------------------------------------------------------
27 // Generate both sets of points
28 // ------------------------------------------------------
29 CPose3DQuat generate_points(TPoints& pA, TPoints& pB)
30 {
31  const double Dx = 0.5;
32  const double Dy = 1.5;
33  const double Dz = 0.75;
34 
35  const double yaw = DEG2RAD(10);
36  const double pitch = DEG2RAD(20);
37  const double roll = DEG2RAD(5);
38 
39  pA.resize(5); // A set of points at "A" reference system
40  pB.resize(5); // A set of points at "B" reference system
41 
42  pA[0].resize(3);
43  pA[0][0] = 0.0;
44  pA[0][1] = 0.5;
45  pA[0][2] = 0.4;
46  pA[1].resize(3);
47  pA[1][0] = 1.0;
48  pA[1][1] = 1.5;
49  pA[1][2] = -0.1;
50  pA[2].resize(3);
51  pA[2][0] = 1.2;
52  pA[2][1] = 1.1;
53  pA[2][2] = 0.9;
54  pA[3].resize(3);
55  pA[3][0] = 0.7;
56  pA[3][1] = 0.3;
57  pA[3][2] = 3.4;
58  pA[4].resize(3);
59  pA[4][0] = 1.9;
60  pA[4][1] = 2.5;
61  pA[4][2] = -1.7;
62 
63  CPose3DQuat qPose = CPose3DQuat(CPose3D(Dx, Dy, Dz, yaw, pitch, roll));
64  for (unsigned int i = 0; i < 5; ++i)
65  {
66  pB[i].resize(3);
67  qPose.inverseComposePoint(
68  pA[i][0], pA[i][1], pA[i][2], pB[i][0], pB[i][1], pB[i][2]);
69  }
70 
71  return qPose;
72 
73 } // end generate_points
74 
75 // ------------------------------------------------------
76 // Generate a list of matched points
77 // ------------------------------------------------------
78 void generate_list_of_points(
79  const TPoints& pA, const TPoints& pB, TMatchingPairList& list)
80 {
81  TMatchingPair pair;
82  for (unsigned int i = 0; i < 5; ++i)
83  {
84  pair.this_idx = pair.other_idx = i;
85  pair.this_x = pA[i][0];
86  pair.this_y = pA[i][1];
87  pair.this_z = pA[i][2];
88 
89  pair.other_x = pB[i][0];
90  pair.other_y = pB[i][1];
91  pair.other_z = pB[i][2];
92 
93  list.push_back(pair);
94  }
95 } // end generate_list_of_points
96 
97 // ------------------------------------------------------
98 // Genreate a vector of matched points
99 // ------------------------------------------------------
100 void generate_vector_of_points(
101  const TPoints& pA, const TPoints& pB, vector<mrpt::math::TPoint3D>& ptsA,
102  vector<mrpt::math::TPoint3D>& ptsB)
103 {
104  // The input vector: inV = [pA1x, pA1y, pA1z, pB1x, pB1y, pB1z, ... ]
105  ptsA.resize(pA.size());
106  ptsB.resize(pA.size());
107  for (unsigned int i = 0; i < pA.size(); ++i)
108  {
109  ptsA[i] = mrpt::math::TPoint3D(pA[i][0], pA[i][1], pA[i][2]);
110  ptsB[i] = mrpt::math::TPoint3D(pB[i][0], pB[i][1], pB[i][2]);
111  }
112 } // end generate_vector_of_points
113 
114 TEST(tfest, se3_l2_MatchList)
115 {
116  TPoints pA, pB; // The input points
117  CPose3DQuat qPose = generate_points(pA, pB);
118 
119  TMatchingPairList list;
120  generate_list_of_points(pA, pB, list); // Generate a list of matched points
121 
122  CPose3DQuat outQuat; // Output CPose3DQuat for the LSRigidTransformation
123  double scale; // Output scale value
124 
125  bool res = mrpt::tfest::se3_l2(list, outQuat, scale);
126  EXPECT_TRUE(res);
127 
128  double err = 0.0;
129  if ((qPose[3] * outQuat[3] > 0 && qPose[4] * outQuat[4] > 0 &&
130  qPose[5] * outQuat[5] > 0 && qPose[6] * outQuat[6] > 0) ||
131  (qPose[3] * outQuat[3] < 0 && qPose[4] * outQuat[4] < 0 &&
132  qPose[5] * outQuat[5] < 0 && qPose[6] * outQuat[6] < 0))
133  {
134  for (unsigned int i = 0; i < 7; ++i)
135  err += square(std::fabs(qPose[i]) - std::fabs(outQuat[i]));
136  err = sqrt(err);
137  EXPECT_TRUE(err < 1e-6) << "Applied quaternion: " << endl
138  << qPose << endl
139  << "Out CPose3DQuat: " << endl
140  << outQuat << " [Err: " << err << "]" << endl;
141  }
142  else
143  {
144  GTEST_FAIL() << "Applied quaternion: " << endl
145  << qPose << endl
146  << "Out CPose3DQuat: " << endl
147  << outQuat << endl;
148  }
149 }
150 
151 TEST(tfest, se3_l2_PtsLists)
152 {
153  TPoints pA, pB; // The input points
154  CPose3DQuat qPose = generate_points(pA, pB);
155 
156  vector<mrpt::math::TPoint3D> ptsA, ptsB;
157  generate_vector_of_points(
158  pA, pB, ptsA, ptsB); // Generate a vector of matched points
159 
160  mrpt::poses::CPose3DQuat qu;
161  double scale;
162  mrpt::tfest::se3_l2(
163  ptsA, ptsB, qu, scale); // Output quaternion for the Horn Method
164 
165  double err = 0.0;
166  if ((qPose[3] * qu[3] > 0 && qPose[4] * qu[4] > 0 && qPose[5] * qu[5] > 0 &&
167  qPose[6] * qu[6] > 0) ||
168  (qPose[3] * qu[3] < 0 && qPose[4] * qu[4] < 0 && qPose[5] * qu[5] < 0 &&
169  qPose[6] * qu[6] < 0))
170  {
171  for (unsigned int i = 0; i < 7; ++i)
172  err += square(std::fabs(qPose[i]) - std::fabs(qu[i]));
173  err = sqrt(err);
174  EXPECT_TRUE(err < 1e-6) << "Applied quaternion: " << endl
175  << qPose << endl
176  << "Out CPose3DQuat: " << endl
177  << qu << " [Err: " << err << "]" << endl;
178  }
179  else
180  {
181  GTEST_FAIL() << "Applied quaternion: " << endl
182  << qPose << endl
183  << "Out CPose3DQuat: " << endl
184  << qu << endl;
185  }
186 } // end
187 
188 TEST(tfest, se3_l2_robust)
189 {
190  TPoints pA, pB; // The input points
191  CPose3DQuat qPose = generate_points(pA, pB);
192 
193  TMatchingPairList list;
194  generate_list_of_points(pA, pB, list); // Generate a list of matched points
195 
196  mrpt::tfest::TSE3RobustResult estim_result;
197  mrpt::tfest::TSE3RobustParams params;
198  params.ransac_minSetSize = 3;
199  params.ransac_maxSetSizePct = 3.0 / list.size();
200  mrpt::tfest::se3_l2_robust(list, params, estim_result);
201 
202  EXPECT_GT(estim_result.inliers_idx.size(), 0u);
203  const CPose3DQuat& outQuat = estim_result.transformation;
204  double err = 0.0;
205  if ((qPose[3] * outQuat[3] > 0 && qPose[4] * outQuat[4] > 0 &&
206  qPose[5] * outQuat[5] > 0 && qPose[6] * outQuat[6] > 0) ||
207  (qPose[3] * outQuat[3] < 0 && qPose[4] * outQuat[4] < 0 &&
208  qPose[5] * outQuat[5] < 0 && qPose[6] * outQuat[6] < 0))
209  {
210  for (unsigned int i = 0; i < 7; ++i)
211  err += square(std::fabs(qPose[i]) - std::fabs(outQuat[i]));
212  err = sqrt(err);
213  EXPECT_TRUE(err < 1e-6) << "Applied quaternion: " << endl
214  << qPose << endl
215  << "Out CPose3DQuat: " << endl
216  << outQuat << " [Err: " << err << "]" << endl;
217  }
218  else
219  {
220  GTEST_FAIL() << "Applied quaternion: " << endl
221  << qPose << endl
222  << "Out CPose3DQuat: " << endl
223  << outQuat << endl;
224  }
225 }
mrpt::tfest::se3_l2_robust
bool se3_l2_robust(const mrpt::tfest::TMatchingPairList &in_correspondences, const TSE3RobustParams &in_params, TSE3RobustResult &out_results)
Least-squares (L2 norm) solution to finding the optimal SE(3) transform between two reference frames ...
Definition: se3_l2_ransac.cpp:32
generate_vector_of_points
void generate_vector_of_points(const TPoints &pA, const TPoints &pB, vector< mrpt::math::TPoint3D > &ptsA, vector< mrpt::math::TPoint3D > &ptsB)
Definition: se3_unittest.cpp:100
mrpt::tfest::TSE3RobustParams
Parameters for se3_l2_robust().
Definition: se3.h:65
mrpt::tfest::TMatchingPair::other_z
float other_z
Definition: TMatchingPair.h:66
generate_points
CPose3DQuat generate_points(TPoints &pA, TPoints &pB)
Definition: se3_unittest.cpp:29
mrpt::tfest::TMatchingPair::other_idx
uint32_t other_idx
Definition: TMatchingPair.h:64
mrpt::obs::gnss::roll
double roll
Definition: gnss_messages_novatel.h:264
mrpt::tfest
Functions for estimating the optimal transformation between two frames of references given measuremen...
Definition: indiv-compat-decls.h:16
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::tfest::TSE3RobustResult::transformation
mrpt::poses::CPose3DQuat transformation
The best transformation found.
Definition: se3.h:106
mrpt::tfest::TMatchingPair
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:33
mrpt::poses::CPose3DQuat
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
Definition: CPose3DQuat.h:48
mrpt::square
T square(const T x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:18
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
TPoints
std::vector< std::vector< double > > TPoints
Definition: se3_unittest.cpp:24
random.h
mrpt::poses::CPose3DQuat::inverseComposePoint
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point L such as .
Definition: CPose3DQuat.cpp:187
generate_list_of_points
void generate_list_of_points(const TPoints &pA, const TPoints &pB, TMatchingPairList &list)
Definition: se3_unittest.cpp:78
ops_vectors.h
mrpt::tfest::TMatchingPair::this_x
float this_x
Definition: TMatchingPair.h:65
mrpt::tfest::TMatchingPair::this_idx
uint32_t this_idx
Definition: TMatchingPair.h:63
mrpt::tfest::se3_l2
bool se3_l2(const mrpt::tfest::TMatchingPairList &in_correspondences, mrpt::poses::CPose3DQuat &out_transform, double &out_scale, bool forceScaleToUnity=false)
Least-squares (L2 norm) solution to finding the optimal SE(3) transform between two reference frames ...
Definition: se3_l2.cpp:221
mrpt::tfest::TMatchingPair::other_y
float other_y
Definition: TMatchingPair.h:66
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
res
GLuint res
Definition: glext.h:7268
CPose3DQuat.h
tfest.h
mrpt::obs::gnss::pitch
double pitch
Definition: gnss_messages_novatel.h:264
mrpt::tfest::TMatchingPair::this_y
float this_y
Definition: TMatchingPair.h:65
mrpt::tfest::TSE3RobustResult::inliers_idx
std::vector< uint32_t > inliers_idx
Indexes within the in_correspondences list which corresponds with inliers.
Definition: se3.h:112
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:378
scale
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:6503
CPose3D.h
TEST
TEST(tfest, se3_l2_MatchList)
Definition: se3_unittest.cpp:114
mrpt::tfest::TSE3RobustResult
Output placeholder for se3_l2_robust()
Definition: se3.h:103
mrpt::tfest::TMatchingPair::this_z
float this_z
Definition: TMatchingPair.h:65
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
mrpt::tfest::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:83
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:17
mrpt::tfest::TMatchingPair::other_x
float other_x
Definition: TMatchingPair.h:66
params
GLenum const GLfloat * params
Definition: glext.h:3534
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST