Main MRPT website > C++ reference for MRPT 1.5.7
se3_l2_ransac.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 "tfest-precomp.h" // Precompiled headers
11 
12 #include <mrpt/tfest/se3.h>
13 
14 #include <mrpt/poses/CPose3D.h>
15 #include <mrpt/poses/CPose3DQuat.h>
16 #include <mrpt/random.h>
17 #include <mrpt/utils/round.h>
18 #include <mrpt/math/utils.h> // linspace()
19 #include <numeric>
20 
21 using namespace mrpt;
22 using namespace mrpt::tfest;
23 using namespace mrpt::random;
24 using namespace mrpt::poses;
25 using namespace mrpt::math;
26 using namespace mrpt::utils;
27 using namespace std;
28 
29 /*---------------------------------------------------------------
30  se3_l2_robust
31  ---------------------------------------------------------------*/
33  const mrpt::utils::TMatchingPairList & in_correspondences,
34  const TSE3RobustParams & params,
35  TSE3RobustResult & results )
36 {
38 
39  const size_t N = in_correspondences.size();
40 
41  // -------------------------------------------
42  // Thresholds
43  // -------------------------------------------
44  Eigen::Matrix<double,7,1> th;
45  th[0] = // X (meters)
46  th[1] = // Y (meters)
47  th[2] = params.ransac_threshold_lin; // Z (meters)
48  th[3] = // YAW (degrees)
49  th[4] = // PITCH (degrees)
50  th[5] = params.ransac_threshold_ang;// ROLL (degrees)
51  th[6] = params.ransac_threshold_scale; // SCALE
52 
53  // -------------------------------------------
54  // RANSAC parameters
55  // -------------------------------------------
56  double min_err = std::numeric_limits<double>::max(); // Minimum error achieved so far
57  size_t max_size = 0; // Maximum size of the consensus set so far
58  double scale; // Output scale
59 
60  const size_t n = params.ransac_minSetSize; // Minimum number of points to fit the model
61  const size_t d = mrpt::utils::round( N*params.ransac_maxSetSizePct ); // Minimum number of points to be considered a good set
62  const size_t max_it = params.ransac_nmaxSimulations; // Maximum number of iterations
63 
64  ASSERTMSG_(d>=n,"Minimum number of points to be considered a good set is < Minimum number of points to fit the model")
65 
66  // -------------------------------------------
67  // MAIN loop
68  // -------------------------------------------
69  for(size_t iterations = 0; iterations < max_it; iterations++ )
70  {
71  //printf("Iteration %2u of %u", iterations+1, max_it );
72 
73  // Generate maybe inliers
74  vector_int rub, mbSet, cSet;
75  mrpt::math::linspace( (int)0, (int)N-1, (int)N, rub );
76  randomGenerator.permuteVector( rub, mbSet );
77 
78  // Compute first inliers output
79  TMatchingPairList mbInliers;
80  mbInliers.reserve( n );
81  for(size_t i = 0; mbInliers.size()<n && i<N; i++ )
82  {
83  const size_t idx = mbSet[i];
84 
85  // User-provided filter:
86  if (params.user_individual_compat_callback)
87  {
89  pm.idx_this = in_correspondences[ idx ].this_idx;
90  pm.idx_other = in_correspondences[ idx ].other_idx;
91  if (! (*params.user_individual_compat_callback)(pm,params.user_individual_compat_callback_userdata))
92  continue; // Skip this one!
93  }
94 
95  mbInliers.push_back( in_correspondences[ idx ] );
96  cSet.push_back( idx );
97  }
98 
99  // Check minimum number:
100  if (cSet.size()<n)
101  {
102  if (params.verbose)
103  std::cerr << "[tfest::se3_l2_robust] Iter " << iterations <<": It was not possible to find the min no of (compatible) matching pairs.\n";
104  continue; // Try again
105  }
106 
107  CPose3DQuat mbOutQuat;
108  const bool res = mrpt::tfest::se3_l2( mbInliers, mbOutQuat, scale, params.forceScaleToUnity );
109  if (!res) { std::cerr << "[tfest::se3_l2_robust] tfest::se3_l2() returned false for tentative subset during RANSAC iteration!\n"; continue; }
110 
111  // Maybe inliers Output
112  const CPose3D mbOut = CPose3D(mbOutQuat);
113  CVectorFloat mbOut_vec(7);
114  mbOut_vec[0] = mbOut.x();
115  mbOut_vec[1] = mbOut.y();
116  mbOut_vec[2] = mbOut.z();
117 
118  mbOut_vec[3] = mbOut.yaw();
119  mbOut_vec[4] = mbOut.pitch();
120  mbOut_vec[5] = mbOut.roll();
121 
122  mbOut_vec[6] = scale;
123 
124  // Inner loop: for each point NOT in the maybe inliers
125  for(size_t k = n; k < N; k++ )
126  {
127  const size_t idx = mbSet[k];
128 
129  // User-provided filter:
130  if (params.user_individual_compat_callback)
131  {
133  pm.idx_this = in_correspondences[ idx ].this_idx;
134  pm.idx_other = in_correspondences[ idx ].other_idx;
135  if (! (*params.user_individual_compat_callback)(pm,params.user_individual_compat_callback_userdata))
136  continue; // Skip this one!
137  }
138 
139  // Consensus set: Maybe inliers + new point
140  CPose3DQuat csOutQuat;
141  mbInliers.push_back( in_correspondences[ idx ] ); // Insert
142  const bool res = mrpt::tfest::se3_l2( mbInliers, csOutQuat, scale, params.forceScaleToUnity );
143  mbInliers.erase( mbInliers.end()-1 ); // Erase
144 
145  if (!res) { std::cerr << "[tfest::se3_l2_robust] tfest::se3_l2() returned false for tentative subset during RANSAC iteration!\n"; continue; }
146 
147  // Is this point a supporter of the initial inlier group?
148  const CPose3D csOut = CPose3D(csOutQuat);
149 
150  if( fabs( mbOut_vec[0] - csOut.x() ) < th[0] && fabs( mbOut_vec[1] - csOut.y() ) < th[1] &&
151  fabs( mbOut_vec[2] - csOut.z() ) < th[2] && fabs( mbOut_vec[3] - csOut.yaw() ) < th[3] &&
152  fabs( mbOut_vec[4] - csOut.pitch() ) < th[4] && fabs( mbOut_vec[5] - csOut.roll() ) < th[5] &&
153  fabs( mbOut_vec[6] - scale ) < th[6] )
154  {
155  // Inlier detected -> add to the inlier list
156  cSet.push_back( idx );
157  } // end if INLIERS
158  else
159  {
160  //cout << " It " << iterations << " - RANSAC Outlier Detected: " << k << endl;
161  }
162  } // end 'inner' for
163 
164  // Test cSet size
165  if( cSet.size() >= d )
166  {
167  // Good set of points found
168  TMatchingPairList cSetInliers;
169  cSetInliers.resize( cSet.size() );
170  for( unsigned int m = 0; m < cSet.size(); m++ )
171  cSetInliers[m] = in_correspondences[ cSet[m] ];
172 
173  // Compute output: Consensus Set + Initial Inliers Guess
174  CPose3DQuat cIOutQuat;
175  const bool res = mrpt::tfest::se3_l2( cSetInliers, cIOutQuat, scale, params.forceScaleToUnity ); // Compute output
176  ASSERTMSG_(res, "tfest::se3_l2() returned false for tentative subset during RANSAC iteration!")
177 
178  // Compute error for consensus_set
179  const CPose3D cIOut = CPose3D(cIOutQuat);
180  const double err = std::sqrt( square( mbOut_vec[0] - cIOut.x() ) + square( mbOut_vec[1] - cIOut.y() ) + square( mbOut_vec[2] - cIOut.z() ) +
181  square( mbOut_vec[3] - cIOut.yaw() ) + square( mbOut_vec[4] - cIOut.pitch() ) + square( mbOut_vec[5] - cIOut.roll() ) +
182  square( mbOut_vec[6] - scale ));
183 
184  // Is the best set of points so far?
185  if( err < min_err && cSet.size() >= max_size )
186  {
187  min_err = err;
188  max_size = cSet.size();
189  results.transformation = cIOutQuat;
190  results.scale = scale;
191  results.inliers_idx = cSet;
192  } // end if SCALE ERROR
193  //printf(" - Consensus set size: %u - Error: %.6f\n", (unsigned int)cSet.size(), err );
194  } // end if cSet.size() > d
195  else
196  {
197  //printf(" - Consensus set size: %u - Not big enough!\n", (unsigned int)cSet.size() );
198  }
199  } // end 'iterations' for
200 
201  if( max_size == 0 )
202  {
203  std::cerr << "[se3_l2_robust] maximum size is == 0!\n";
204  return false;
205  }
206 
207  MRPT_END
208 
209  return true;
210 
211 } // end se3_l2_robust()
212 
A namespace of pseudo-random numbers genrators of diferent distributions.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
void permuteVector(const VEC &in_vector, VEC &out_result)
Returns a random permutation of a vector: all the elements of the input vector are in the output but ...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
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
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:5717
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
GLenum GLsizei n
Definition: glext.h:4618
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
STL namespace.
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
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define MRPT_END
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 scale
The estimated scale of the rigid transformation (should be very close to 1.0)
Definition: se3.h:98
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
void linspace(T first, T last, size_t count, VECTOR &out_vector)
Generates an equidistant sequence of numbers given the first one, the last one and the desired number...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
std::vector< int32_t > vector_int
Definition: types_simple.h:23
GLuint res
Definition: glext.h:6298
mrpt::poses::CPose3DQuat transformation
The best transformation found.
Definition: se3.h:97
#define ASSERTMSG_(f, __ERROR_MSG)
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLenum const GLfloat * params
Definition: glext.h:3514
For each individual-compatibility (IC) test, the indices of the candidate match between elements in b...
Output placeholder for se3_l2_robust()
Definition: se3.h:95



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019