MRPT  1.9.9
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-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 "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/core/round.h>
18 #include <mrpt/math/utils.h> // linspace()
19 #include <numeric>
20 #include <iostream>
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  // -------------------------------------------
43  Eigen::Matrix<double, 7, 1> th;
44  th[0] = // X (meters)
45  th[1] = // Y (meters)
46  th[2] = params.ransac_threshold_lin; // Z (meters)
47  th[3] = // YAW (degrees)
48  th[4] = // PITCH (degrees)
49  th[5] = params.ransac_threshold_ang; // ROLL (degrees)
50  th[6] = params.ransac_threshold_scale; // SCALE
51 
52  // -------------------------------------------
53  // RANSAC parameters
54  // -------------------------------------------
55  double min_err =
56  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 =
61  params.ransac_minSetSize; // Minimum number of points to fit the model
62  const size_t d = mrpt::round(
63  N * params.ransac_maxSetSizePct); // Minimum number of points to be
64  // considered a good set
65  const size_t max_it =
66  params.ransac_nmaxSimulations; // Maximum number of iterations
67 
68  ASSERTMSG_(
69  d >= n,
70  "Minimum number of points to be considered a good set is < Minimum "
71  "number of points to fit the model");
72 
73  // -------------------------------------------
74  // MAIN loop
75  // -------------------------------------------
76  for (size_t iterations = 0; iterations < max_it; iterations++)
77  {
78  // printf("Iteration %2u of %u", iterations+1, max_it );
79 
80  // Generate maybe inliers
81  std::vector<uint32_t> rub, mbSet, cSet;
82  mrpt::math::linspace((int)0, (int)N - 1, (int)N, rub);
83  getRandomGenerator().permuteVector(rub, mbSet);
84 
85  // Compute first inliers output
86  TMatchingPairList mbInliers;
87  mbInliers.reserve(n);
88  for (size_t i = 0; mbInliers.size() < n && i < N; i++)
89  {
90  const size_t idx = mbSet[i];
91 
92  // User-provided filter:
93  if (params.user_individual_compat_callback)
94  {
96  pm.idx_this = in_correspondences[idx].this_idx;
97  pm.idx_other = in_correspondences[idx].other_idx;
98  if (!params.user_individual_compat_callback(pm))
99  continue; // Skip this one!
100  }
101 
102  mbInliers.push_back(in_correspondences[idx]);
103  cSet.push_back(idx);
104  }
105 
106  // Check minimum number:
107  if (cSet.size() < n)
108  {
109  if (params.verbose)
110  std::cerr << "[tfest::se3_l2_robust] Iter " << iterations
111  << ": It was not possible to find the min no of "
112  "(compatible) matching pairs.\n";
113  continue; // Try again
114  }
115 
116  CPose3DQuat mbOutQuat;
117  bool res = mrpt::tfest::se3_l2(
118  mbInliers, mbOutQuat, scale, params.forceScaleToUnity);
119  if (!res)
120  {
121  std::cerr << "[tfest::se3_l2_robust] tfest::se3_l2() returned "
122  "false for tentative subset during RANSAC "
123  "iteration!\n";
124  continue;
125  }
126 
127  // Maybe inliers Output
128  const CPose3D mbOut = CPose3D(mbOutQuat);
129  CVectorFloat mbOut_vec(7);
130  mbOut_vec[0] = mbOut.x();
131  mbOut_vec[1] = mbOut.y();
132  mbOut_vec[2] = mbOut.z();
133 
134  mbOut_vec[3] = mbOut.yaw();
135  mbOut_vec[4] = mbOut.pitch();
136  mbOut_vec[5] = mbOut.roll();
137 
138  mbOut_vec[6] = scale;
139 
140  // Inner loop: for each point NOT in the maybe inliers
141  for (size_t k = n; k < N; k++)
142  {
143  const size_t idx = mbSet[k];
144 
145  // User-provided filter:
146  if (params.user_individual_compat_callback)
147  {
149  pm.idx_this = in_correspondences[idx].this_idx;
150  pm.idx_other = in_correspondences[idx].other_idx;
151  if (!params.user_individual_compat_callback(pm))
152  continue; // Skip this one!
153  }
154 
155  // Consensus set: Maybe inliers + new point
156  CPose3DQuat csOutQuat;
157  mbInliers.push_back(in_correspondences[idx]); // Insert
159  mbInliers, csOutQuat, scale, params.forceScaleToUnity);
160  mbInliers.erase(mbInliers.end() - 1); // Erase
161 
162  if (!res)
163  {
164  std::cerr << "[tfest::se3_l2_robust] tfest::se3_l2() returned "
165  "false for tentative subset during RANSAC "
166  "iteration!\n";
167  continue;
168  }
169 
170  // Is this point a supporter of the initial inlier group?
171  const CPose3D csOut = CPose3D(csOutQuat);
172 
173  if (fabs(mbOut_vec[0] - csOut.x()) < th[0] &&
174  fabs(mbOut_vec[1] - csOut.y()) < th[1] &&
175  fabs(mbOut_vec[2] - csOut.z()) < th[2] &&
176  fabs(mbOut_vec[3] - csOut.yaw()) < th[3] &&
177  fabs(mbOut_vec[4] - csOut.pitch()) < th[4] &&
178  fabs(mbOut_vec[5] - csOut.roll()) < th[5] &&
179  fabs(mbOut_vec[6] - scale) < th[6])
180  {
181  // Inlier detected -> add to the inlier list
182  cSet.push_back(idx);
183  } // end if INLIERS
184  else
185  {
186  // cout << " It " << iterations << " - RANSAC Outlier Detected:
187  // " << k << endl;
188  }
189  } // end 'inner' for
190 
191  // Test cSet size
192  if (cSet.size() >= d)
193  {
194  // Good set of points found
195  TMatchingPairList cSetInliers;
196  cSetInliers.resize(cSet.size());
197  for (unsigned int m = 0; m < cSet.size(); m++)
198  cSetInliers[m] = in_correspondences[cSet[m]];
199 
200  // Compute output: Consensus Set + Initial Inliers Guess
201  CPose3DQuat cIOutQuat;
203  cSetInliers, cIOutQuat, scale,
204  params.forceScaleToUnity); // Compute output
205  ASSERTMSG_(
206  res,
207  "tfest::se3_l2() returned false for tentative subset during "
208  "RANSAC iteration!");
209 
210  // Compute error for consensus_set
211  const CPose3D cIOut = CPose3D(cIOutQuat);
212  const double err = std::sqrt(
213  square(mbOut_vec[0] - cIOut.x()) +
214  square(mbOut_vec[1] - cIOut.y()) +
215  square(mbOut_vec[2] - cIOut.z()) +
216  square(mbOut_vec[3] - cIOut.yaw()) +
217  square(mbOut_vec[4] - cIOut.pitch()) +
218  square(mbOut_vec[5] - cIOut.roll()) +
219  square(mbOut_vec[6] - scale));
220 
221  // Is the best set of points so far?
222  if (err < min_err && cSet.size() >= max_size)
223  {
224  min_err = err;
225  max_size = cSet.size();
226  results.transformation = cIOutQuat;
227  results.scale = scale;
228  results.inliers_idx = cSet;
229  } // end if SCALE ERROR
230  // printf(" - Consensus set size: %u - Error: %.6f\n", (unsigned
231  // int)cSet.size(), err );
232  } // end if cSet.size() > d
233  else
234  {
235  // printf(" - Consensus set size: %u - Not big enough!\n", (unsigned
236  // int)cSet.size() );
237  }
238  } // end 'iterations' for
239 
240  if (max_size == 0)
241  {
242  std::cerr << "[se3_l2_robust] maximum size is == 0!\n";
243  return false;
244  }
245 
246  MRPT_END
247 
248  return true;
249 
250 } // end se3_l2_robust()
A namespace of pseudo-random numbers generators of diferent distributions.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
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 ...
#define MRPT_START
Definition: exceptions.h:262
Parameters for se3_l2_robust().
Definition: se3.h:63
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:6502
GLenum GLsizei n
Definition: glext.h:5074
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:532
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:526
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:81
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:46
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:538
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:86
#define MRPT_END
Definition: exceptions.h:266
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
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:7268
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:3534
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020