MRPT  1.9.9
se3_l2_ransac.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "tfest-precomp.h" // Precompiled headers
11 
12 #include <mrpt/tfest/se3.h>
13 
14 #include <mrpt/core/round.h>
15 #include <mrpt/math/utils.h> // linspace()
16 #include <mrpt/poses/CPose3D.h>
17 #include <mrpt/poses/CPose3DQuat.h>
18 #include <mrpt/random.h>
19 #include <iostream>
20 #include <numeric>
21 
22 using namespace mrpt;
23 using namespace mrpt::tfest;
24 using namespace mrpt::random;
25 using namespace mrpt::poses;
26 using namespace mrpt::math;
27 using namespace std;
28 
29 /*---------------------------------------------------------------
30  se3_l2_robust
31  ---------------------------------------------------------------*/
33  const mrpt::tfest::TMatchingPairList& in_correspondences,
35 {
37 
38  const size_t N = in_correspondences.size();
39 
40  // -------------------------------------------
41  // Thresholds
42  // -------------------------------------------
44  // x,y,z [m]
45  th[0] = th[1] = th[2] = params.ransac_threshold_lin;
46  // yaw,pitch, roll [deg]
47  th[3] = th[4] = th[5] = params.ransac_threshold_ang;
48  // scale:
49  th[6] = params.ransac_threshold_scale;
50 
51  // -------------------------------------------
52  // RANSAC parameters
53  // -------------------------------------------
54  double min_err =
55  std::numeric_limits<double>::max(); // Minimum error achieved so far
56  size_t max_size = 0; // Maximum size of the consensus set so far
57  double scale; // Output scale
58 
59  const size_t n =
60  params.ransac_minSetSize; // Minimum number of points to fit the model
61  const size_t d = mrpt::round(
62  N * params.ransac_maxSetSizePct); // Minimum number of points to be
63  // considered a good set
64  const size_t max_it =
65  params.ransac_nmaxSimulations; // Maximum number of iterations
66 
67  ASSERTMSG_(
68  d >= n,
69  "Minimum number of points to be considered a good set is < Minimum "
70  "number of points to fit the model");
71 
72  // -------------------------------------------
73  // MAIN loop
74  // -------------------------------------------
75  for (size_t iterations = 0; iterations < max_it; iterations++)
76  {
77  // printf("Iteration %2u of %u", iterations+1, max_it );
78 
79  // Generate maybe inliers
80  std::vector<uint32_t> rub, mbSet, cSet;
81  mrpt::math::linspace((int)0, (int)N - 1, (int)N, rub);
82  getRandomGenerator().permuteVector(rub, mbSet);
83 
84  // Compute first inliers output
85  TMatchingPairList mbInliers;
86  mbInliers.reserve(n);
87  for (size_t i = 0; mbInliers.size() < n && i < N; i++)
88  {
89  const size_t idx = mbSet[i];
90 
91  // User-provided filter:
92  if (params.user_individual_compat_callback)
93  {
95  pm.idx_this = in_correspondences[idx].this_idx;
96  pm.idx_other = in_correspondences[idx].other_idx;
97  if (!params.user_individual_compat_callback(pm))
98  continue; // Skip this one!
99  }
100 
101  mbInliers.push_back(in_correspondences[idx]);
102  cSet.push_back(idx);
103  }
104 
105  // Check minimum number:
106  if (cSet.size() < n)
107  {
108  if (params.verbose)
109  std::cerr << "[tfest::se3_l2_robust] Iter " << iterations
110  << ": It was not possible to find the min no of "
111  "(compatible) matching pairs.\n";
112  continue; // Try again
113  }
114 
115  CPose3DQuat mbOutQuat;
116  bool res = mrpt::tfest::se3_l2(
117  mbInliers, mbOutQuat, scale, params.forceScaleToUnity);
118  if (!res)
119  {
120  std::cerr << "[tfest::se3_l2_robust] tfest::se3_l2() returned "
121  "false for tentative subset during RANSAC "
122  "iteration!\n";
123  continue;
124  }
125 
126  // Maybe inliers Output
127  const CPose3D mbOut = CPose3D(mbOutQuat);
128  CVectorFloat mbOut_vec(7);
129  mbOut_vec[0] = mbOut.x();
130  mbOut_vec[1] = mbOut.y();
131  mbOut_vec[2] = mbOut.z();
132 
133  mbOut_vec[3] = mbOut.yaw();
134  mbOut_vec[4] = mbOut.pitch();
135  mbOut_vec[5] = mbOut.roll();
136 
137  mbOut_vec[6] = scale;
138 
139  // Inner loop: for each point NOT in the maybe inliers
140  for (size_t k = n; k < N; k++)
141  {
142  const size_t idx = mbSet[k];
143 
144  // User-provided filter:
145  if (params.user_individual_compat_callback)
146  {
148  pm.idx_this = in_correspondences[idx].this_idx;
149  pm.idx_other = in_correspondences[idx].other_idx;
150  if (!params.user_individual_compat_callback(pm))
151  continue; // Skip this one!
152  }
153 
154  // Consensus set: Maybe inliers + new point
155  CPose3DQuat csOutQuat;
156  mbInliers.push_back(in_correspondences[idx]); // Insert
158  mbInliers, csOutQuat, scale, params.forceScaleToUnity);
159  mbInliers.erase(mbInliers.end() - 1); // Erase
160 
161  if (!res)
162  {
163  std::cerr << "[tfest::se3_l2_robust] tfest::se3_l2() returned "
164  "false for tentative subset during RANSAC "
165  "iteration!\n";
166  continue;
167  }
168 
169  // Is this point a supporter of the initial inlier group?
170  const CPose3D csOut = CPose3D(csOutQuat);
171 
172  if (fabs(mbOut_vec[0] - csOut.x()) < th[0] &&
173  fabs(mbOut_vec[1] - csOut.y()) < th[1] &&
174  fabs(mbOut_vec[2] - csOut.z()) < th[2] &&
175  fabs(mbOut_vec[3] - csOut.yaw()) < th[3] &&
176  fabs(mbOut_vec[4] - csOut.pitch()) < th[4] &&
177  fabs(mbOut_vec[5] - csOut.roll()) < th[5] &&
178  fabs(mbOut_vec[6] - scale) < th[6])
179  {
180  // Inlier detected -> add to the inlier list
181  cSet.push_back(idx);
182  } // end if INLIERS
183  else
184  {
185  // cout << " It " << iterations << " - RANSAC Outlier Detected:
186  // " << k << endl;
187  }
188  } // end 'inner' for
189 
190  // Test cSet size
191  if (cSet.size() >= d)
192  {
193  // Good set of points found
194  TMatchingPairList cSetInliers;
195  cSetInliers.resize(cSet.size());
196  for (unsigned int m = 0; m < cSet.size(); m++)
197  cSetInliers[m] = in_correspondences[cSet[m]];
198 
199  // Compute output: Consensus Set + Initial Inliers Guess
200  CPose3DQuat cIOutQuat;
202  cSetInliers, cIOutQuat, scale,
203  params.forceScaleToUnity); // Compute output
204  ASSERTMSG_(
205  res,
206  "tfest::se3_l2() returned false for tentative subset during "
207  "RANSAC iteration!");
208 
209  // Compute error for consensus_set
210  const CPose3D cIOut = CPose3D(cIOutQuat);
211  const double err = std::sqrt(
212  square(mbOut_vec[0] - cIOut.x()) +
213  square(mbOut_vec[1] - cIOut.y()) +
214  square(mbOut_vec[2] - cIOut.z()) +
215  square(mbOut_vec[3] - cIOut.yaw()) +
216  square(mbOut_vec[4] - cIOut.pitch()) +
217  square(mbOut_vec[5] - cIOut.roll()) +
218  square(mbOut_vec[6] - scale));
219 
220  // Is the best set of points so far?
221  if (err < min_err && cSet.size() >= max_size)
222  {
223  min_err = err;
224  max_size = cSet.size();
225  results.transformation = cIOutQuat;
226  results.scale = scale;
227  results.inliers_idx = cSet;
228  } // end if SCALE ERROR
229  // printf(" - Consensus set size: %u - Error: %.6f\n", (unsigned
230  // int)cSet.size(), err );
231  } // end if cSet.size() > d
232  else
233  {
234  // printf(" - Consensus set size: %u - Not big enough!\n", (unsigned
235  // int)cSet.size() );
236  }
237  } // end 'iterations' for
238 
239  if (max_size == 0)
240  {
241  std::cerr << "[se3_l2_robust] maximum size is == 0!\n";
242  return false;
243  }
244 
245  MRPT_END
246 
247  return true;
248 
249 } // end se3_l2_robust()
A namespace of pseudo-random numbers generators of diferent distributions.
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 ...
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
Template for column vectors of dynamic size, compatible with Eigen.
Parameters for se3_l2_robust().
Definition: se3.h:63
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:6604
GLenum GLsizei n
Definition: glext.h:5136
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:548
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:542
STL namespace.
T square(const T x)
Inline function for the square of a number.
This base provides a set of functions for maths stuff.
map< string, CVectorDouble > results
A list of TMatchingPair.
Definition: TMatchingPair.h:70
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
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...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:554
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:84
#define MRPT_END
Definition: exceptions.h:245
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:218
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 ...
GLuint res
Definition: glext.h:7385
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLenum const GLfloat * params
Definition: glext.h:3538
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:101
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: eb3744a2b Tue Aug 20 09:56:51 2019 +0200 at mar ago 20 10:00:13 CEST 2019