Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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::utils;
19 using namespace mrpt::math;
20 using namespace mrpt::random;
21 using namespace mrpt::poses;
22 using namespace std;
23 
24 typedef std::vector< std::vector< double > > TPoints;
25 
26 // ------------------------------------------------------
27 // Generate both sets of points
28 // ------------------------------------------------------
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); pA[0][0] = 0.0; pA[0][1] = 0.5; pA[0][2] = 0.4;
43  pA[1].resize(3); pA[1][0] = 1.0; pA[1][1] = 1.5; pA[1][2] = -0.1;
44  pA[2].resize(3); pA[2][0] = 1.2; pA[2][1] = 1.1; pA[2][2] = 0.9;
45  pA[3].resize(3); pA[3][0] = 0.7; pA[3][1] = 0.3; pA[3][2] = 3.4;
46  pA[4].resize(3); pA[4][0] = 1.9; pA[4][1] = 2.5; pA[4][2] = -1.7;
47 
48  CPose3DQuat qPose = CPose3DQuat(CPose3D( Dx, Dy, Dz, yaw, pitch, roll ));
49  for( unsigned int i = 0; i < 5; ++i )
50  {
51  pB[i].resize( 3 );
52  qPose.inverseComposePoint( pA[i][0], pA[i][1], pA[i][2], pB[i][0], pB[i][1], pB[i][2] );
53  }
54 
55  return qPose;
56 
57 } // end generate_points
58 
59 // ------------------------------------------------------
60 // Generate a list of matched points
61 // ------------------------------------------------------
62 void generate_list_of_points( const TPoints &pA, const TPoints &pB, TMatchingPairList &list )
63 {
64  TMatchingPair pair;
65  for( unsigned int i = 0; i < 5; ++i )
66  {
67  pair.this_idx = pair.other_idx = i;
68  pair.this_x = pA[i][0];
69  pair.this_y = pA[i][1];
70  pair.this_z = pA[i][2];
71 
72  pair.other_x = pB[i][0];
73  pair.other_y = pB[i][1];
74  pair.other_z = pB[i][2];
75 
76  list.push_back( pair );
77  }
78 } // end generate_list_of_points
79 
80 // ------------------------------------------------------
81 // Genreate a vector of matched points
82 // ------------------------------------------------------
83 void generate_vector_of_points( const TPoints &pA, const TPoints &pB, vector<mrpt::math::TPoint3D> &ptsA, vector<mrpt::math::TPoint3D> &ptsB )
84 {
85  // The input vector: inV = [pA1x, pA1y, pA1z, pB1x, pB1y, pB1z, ... ]
86  ptsA.resize( pA.size() );
87  ptsB.resize( pA.size() );
88  for( unsigned int i = 0; i < pA.size(); ++i )
89  {
90  ptsA[i] = mrpt::math::TPoint3D( pA[i][0], pA[i][1], pA[i][2] );
91  ptsB[i] = mrpt::math::TPoint3D( pB[i][0], pB[i][1], pB[i][2] );
92  }
93 } // end generate_vector_of_points
94 
95 TEST(tfest, se3_l2_MatchList)
96 {
97  TPoints pA, pB; // The input points
98  CPose3DQuat qPose = generate_points( pA, pB );
99 
100  TMatchingPairList list;
101  generate_list_of_points( pA, pB, list ); // Generate a list of matched points
102 
103  CPose3DQuat outQuat; // Output CPose3DQuat for the LSRigidTransformation
104  double scale; // Output scale value
105 
106  bool res = mrpt::tfest::se3_l2(list,outQuat,scale);
107  EXPECT_TRUE( res );
108 
109  double err = 0.0;
110  if( (qPose[3]*outQuat[3] > 0 && qPose[4]*outQuat[4] > 0 && qPose[5]*outQuat[5] > 0 && qPose[6]*outQuat[6] > 0) ||
111  (qPose[3]*outQuat[3] < 0 && qPose[4]*outQuat[4] < 0 && qPose[5]*outQuat[5] < 0 && qPose[6]*outQuat[6] < 0) )
112  {
113  for( unsigned int i = 0; i < 7; ++i )
114  err += square( std::fabs(qPose[i])-std::fabs(outQuat[i]) );
115  err = sqrt(err);
116  EXPECT_TRUE( err< 1e-6 )
117  << "Applied quaternion: " << endl << qPose << endl
118  << "Out CPose3DQuat: " << endl << outQuat << " [Err: " << err << "]" << endl;
119  }
120  else
121  {
122  GTEST_FAIL( )
123  << "Applied quaternion: " << endl << qPose << endl
124  << "Out CPose3DQuat: " << endl << outQuat << endl;
125  }
126 }
127 
128 TEST(tfest, se3_l2_PtsLists)
129 {
130  TPoints pA, pB; // The input points
131  CPose3DQuat qPose = generate_points( pA, pB );
132 
133  vector<mrpt::math::TPoint3D> ptsA, ptsB;
134  generate_vector_of_points( pA, pB, ptsA, ptsB ); // Generate a vector of matched points
135 
137  double scale;
138  mrpt::tfest::se3_l2(ptsA, ptsB,qu,scale); // Output quaternion for the Horn Method
139 
140  double err = 0.0;
141  if( (qPose[3]*qu[3] > 0 && qPose[4]*qu[4] > 0 && qPose[5]*qu[5] > 0 && qPose[6]*qu[6] > 0) ||
142  (qPose[3]*qu[3] < 0 && qPose[4]*qu[4] < 0 && qPose[5]*qu[5] < 0 && qPose[6]*qu[6] < 0) )
143  {
144 
145  for( unsigned int i = 0; i < 7; ++i )
146  err += square( std::fabs(qPose[i])-std::fabs(qu[i]) );
147  err = sqrt(err);
148  EXPECT_TRUE( err< 1e-6 )
149  << "Applied quaternion: " << endl << qPose << endl
150  << "Out CPose3DQuat: " << endl << qu << " [Err: " << err << "]" << endl;
151  }
152  else
153  {
154  GTEST_FAIL( )
155  << "Applied quaternion: " << endl << qPose << endl
156  << "Out CPose3DQuat: " << endl << qu << endl;
157  }
158 } // end
159 
161 {
162  TPoints pA, pB; // The input points
163  CPose3DQuat qPose = generate_points( pA, pB );
164 
165  TMatchingPairList list;
166  generate_list_of_points( pA, pB, list ); // Generate a list of matched points
167 
168  mrpt::tfest::TSE3RobustResult estim_result;
170  params.ransac_minSetSize = 3;
171  params.ransac_maxSetSizePct = 3.0 / list.size();
172  mrpt::tfest::se3_l2_robust(list,params,estim_result);
173 
174  EXPECT_GT( estim_result.inliers_idx.size(), 0u);
175  const CPose3DQuat & outQuat = estim_result.transformation;
176  double err = 0.0;
177  if( (qPose[3]*outQuat[3] > 0 && qPose[4]*outQuat[4] > 0 && qPose[5]*outQuat[5] > 0 && qPose[6]*outQuat[6] > 0) ||
178  (qPose[3]*outQuat[3] < 0 && qPose[4]*outQuat[4] < 0 && qPose[5]*outQuat[5] < 0 && qPose[6]*outQuat[6] < 0) )
179  {
180  for( unsigned int i = 0; i < 7; ++i )
181  err += square( std::fabs(qPose[i])-std::fabs(outQuat[i]) );
182  err = sqrt(err);
183  EXPECT_TRUE( err< 1e-6 )
184  << "Applied quaternion: " << endl << qPose << endl
185  << "Out CPose3DQuat: " << endl << outQuat << " [Err: " << err << "]" << endl;
186  }
187  else
188  {
189  GTEST_FAIL( )
190  << "Applied quaternion: " << endl << qPose << endl
191  << "Out CPose3DQuat: " << endl << outQuat << endl;
192  }
193 }
double ransac_maxSetSizePct
(Default=0.5) The minimum ratio (0.0 - 1.0) of the input set that is considered to be inliers...
Definition: se3.h:64
GLenum GLenum GLenum GLenum GLenum scale
Definition: glew.h:8359
bool TFEST_IMPEXP se3_l2_robust(const mrpt::utils::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 ...
Parameters for se3_l2_robust().
Definition: se3.h:60
void generate_list_of_points(const TPoints &pA, const TPoints &pB, TMatchingPairList &list)
TEST(tfest, se3_l2_MatchList)
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
mrpt::vector_int inliers_idx
Indexes within the in_correspondences list which corresponds with inliers.
Definition: se3.h:99
std::vector< std::vector< double > > TPoints
A list of TMatchingPair.
Definition: TMatchingPair.h:78
bool TFEST_IMPEXP se3_l2(const mrpt::utils::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:201
double DEG2RAD(const double x)
Degrees to radians.
Definition: bits.h:82
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
GLuint res
Definition: glew.h:7143
unsigned int ransac_minSetSize
(Default=5) The minimum amount of points in a set to start a consensus set.
Definition: se3.h:62
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
GLfloat * params
Definition: glew.h:1436
CPose3DQuat generate_points(TPoints &pA, TPoints &pB)
Lightweight 3D point.
mrpt::poses::CPose3DQuat transformation
The best transformation found.
Definition: se3.h:97
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=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=NULL) const
Computes the 3D point L such as .
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
Output placeholder for se3_l2_robust()
Definition: se3.h:95
void generate_vector_of_points(const TPoints &pA, const TPoints &pB, vector< mrpt::math::TPoint3D > &ptsA, vector< mrpt::math::TPoint3D > &ptsB)



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018