MRPT  1.9.9
se3_ransac_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 // ------------------------------------------------------
11 // This test file is a subset of a complete example. See:
12 // [MRPT]/samples/ransac-data-association/
13 // ------------------------------------------------------
14 
15 #include <mrpt/random.h>
16 #include <mrpt/poses/CPose2D.h>
18 #include <mrpt/poses/CPosePDFSOG.h>
19 #include <mrpt/math/geometry.h>
20 #include <mrpt/tfest/se2.h>
22 
23 #include <gtest/gtest.h>
24 
25 using namespace mrpt;
26 using namespace mrpt::math;
27 using namespace mrpt::random;
28 using namespace mrpt::maps;
29 using namespace mrpt::tfest;
30 using namespace mrpt::poses;
31 using namespace std;
32 
33 // ============= PARAMETERS ===================
34 const size_t NUM_OBSERVATIONS_TO_SIMUL = 15;
35 const size_t RANSAC_MINIMUM_INLIERS = 9; // Min. # of inliers to accept
36 
37 const float normalizationStd = 0.10f; // 1 sigma noise (meters)
39 const size_t MINIMUM_RANSAC_ITERS = 100000;
40 
41 const size_t NUM_MAP_FEATS = 50;
42 const double MAP_SIZE_X = 30;
43 const double MAP_SIZE_Y = 15;
44 // ==============================================
45 
46 struct TObs
47 {
48  size_t ID; // Ground truth ID
49  double x, y;
50 };
51 
52 // Return true if test succeds.
53 // Due to RANSAC being non-deterministic, there exists a small chance of failed
54 // tests even if the algorithm is ok.
56 {
57  getRandomGenerator().randomize(); // randomize with time
58  // --------------------------------
59  // Load feature map:
60  // --------------------------------
61  CSimplePointsMap the_map;
62  // Generate random MAP:
63  the_map.resize(NUM_MAP_FEATS);
64  for (size_t i = 0; i < NUM_MAP_FEATS; i++)
65  {
66  the_map.setPoint(
67  i, getRandomGenerator().drawUniform(0, MAP_SIZE_X),
68  getRandomGenerator().drawUniform(0, MAP_SIZE_Y));
69  }
70  const size_t nMapPts = the_map.size();
71  const size_t nObs = NUM_OBSERVATIONS_TO_SIMUL;
72 
73  // Read the observations themselves:
74  vector<TObs> observations;
75  observations.resize(nObs);
76 
77  const mrpt::poses::CPose2D GT_pose(
78  mrpt::random::getRandomGenerator().drawUniform(-10, 10 + MAP_SIZE_X),
79  mrpt::random::getRandomGenerator().drawUniform(-10, 10 + MAP_SIZE_Y),
80  mrpt::random::getRandomGenerator().drawUniform(-M_PI, M_PI));
81 
82  const mrpt::poses::CPose2D GT_pose_inv = -GT_pose;
83 
84  std::vector<std::pair<size_t, float>> idxs;
85  the_map.kdTreeRadiusSearch2D(GT_pose.x(), GT_pose.y(), 1000, idxs);
86  ASSERT_(idxs.size() >= nObs);
87 
88  for (size_t i = 0; i < nObs; i++)
89  {
90  double gx, gy;
91  the_map.getPoint(idxs[i].first, gx, gy);
92 
93  double lx, ly;
94  GT_pose_inv.composePoint(gx, gy, lx, ly);
95 
96  observations[i].ID = idxs[i].first;
97  observations[i].x = lx +
99  0, normalizationStd);
100  observations[i].y = ly +
102  0, normalizationStd);
103  }
104 
105  // ----------------------------------------------------
106  // Generate list of individual-compatible pairings
107  // ----------------------------------------------------
108  TMatchingPairList all_correspondences;
109 
110  all_correspondences.reserve(nMapPts * nObs);
111 
112  // ALL possibilities:
113  for (size_t j = 0; j < nObs; j++)
114  {
115  TMatchingPair match;
116  match.other_idx = j;
117  match.other_x = observations[j].x;
118  match.other_y = observations[j].y;
119 
120  for (size_t i = 0; i < nMapPts; i++)
121  {
122  match.this_idx = i;
123  the_map.getPoint(i, match.this_x, match.this_y);
124  all_correspondences.push_back(match);
125  }
126  }
127 
128  // ----------------------------------------------------
129  // Run RANSAC-based D-A
130  // ----------------------------------------------------
133 
134  params.ransac_minSetSize = RANSAC_MINIMUM_INLIERS; // ransac_minSetSize (to
135  // add the solution to
136  // the SOG)
137  params.ransac_maxSetSize =
138  all_correspondences
139  .size(); // ransac_maxSetSize: Test with all data points
140  params.ransac_mahalanobisDistanceThreshold =
142  params.ransac_nSimulations = 0; // 0=auto
143  params.ransac_fuseByCorrsMatch = true;
144  params.ransac_fuseMaxDiffXY = 0.01f;
145  params.ransac_fuseMaxDiffPhi = DEG2RAD(0.1);
146  params.ransac_algorithmForLandmarks = true;
147  params.probability_find_good_model = 0.999999;
148  params.ransac_min_nSimulations =
149  MINIMUM_RANSAC_ITERS; // (a lower limit to the auto-detected value of
150  // ransac_nSimulations)
151  params.verbose = false;
152 
153  // Run ransac data-association:
155  all_correspondences, normalizationStd, params, results);
156 
157  // mrpt::poses::CPosePDFSOG & best_poses = results.transformation;
158  TMatchingPairList& out_best_pairings = results.largestSubSet;
159 
160  // Reconstruct the SE(2) transformation for these pairings:
161  mrpt::poses::CPosePDFGaussian solution_pose;
162  mrpt::tfest::se2_l2(out_best_pairings, solution_pose);
163 
164  // Normalized covariance: scale!
165  solution_pose.cov *= square(normalizationStd);
166 
167  if (!(solution_pose.mean.distanceTo(GT_pose) < 0.9 &&
168  std::abs(solution_pose.mean.phi() - GT_pose.phi()) < DEG2RAD(10)))
169  {
170  std::cerr << "Solution pose: " << solution_pose.mean << endl
171  << "Ground truth pose: " << GT_pose << endl;
172  return false;
173  }
174  return true;
175 }
176 
177 TEST(tfest, ransac_data_assoc)
178 {
179  // Run randomized experiments:
180  bool any_ok = false;
181  for (int i = 0; i < 3; i++)
182  if (ransac_data_assoc_run()) any_ok = true;
183 
184  EXPECT_TRUE(any_ok);
185 }
A namespace of pseudo-random numbers generators of diferent distributions.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
CPose2D mean
The mean value.
const float normalizationStd
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:211
bool se2_l2(const mrpt::tfest::TMatchingPairList &in_correspondences, mrpt::math::TPose2D &out_transformation, mrpt::math::CMatrixDouble33 *out_estimateCovariance=nullptr)
Least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames...
Definition: se2_l2.cpp:45
GLint * first
Definition: glext.h:3827
Parameters for se2_l2_robust().
Definition: se2.h:73
double DEG2RAD(const double x)
Degrees to radians.
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
const size_t MINIMUM_RANSAC_ITERS
const float ransac_mahalanobisDistanceThreshold
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
void composePoint(double lx, double ly, double &gx, double &gy) const
An alternative, slightly more efficient way of doing with G and L being 2D points and P this 2D pose...
Definition: CPose2D.cpp:175
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
double drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This base provides a set of functions for maths stuff.
size_t kdTreeRadiusSearch2D(const num_t x0, const num_t y0, const num_t maxRadiusSqr, std::vector< std::pair< size_t, num_t >> &out_indices_dist) const
KD Tree-based search for all the points within a given radius of some 2D point.
bool se2_l2_robust(const mrpt::tfest::TMatchingPairList &in_correspondences, const double in_normalizationStd, const TSE2RobustParams &in_ransac_params, TSE2RobustResult &out_results)
Robust least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference ...
map< string, CVectorDouble > results
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
A list of TMatchingPair.
Definition: TMatchingPair.h:81
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
const size_t NUM_OBSERVATIONS_TO_SIMUL
bool ransac_data_assoc_run()
unsigned long getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
Definition: CPointsMap.cpp:211
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void setPoint(size_t index, float x, float y, float z)
Changes a given point from map, with Z defaulting to 0 if not provided.
Definition: CPointsMap.h:462
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
const double MAP_SIZE_X
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:80
TEST(tfest, ransac_data_assoc)
GLenum GLint x
Definition: glext.h:3538
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
const double MAP_SIZE_Y
size_t size() const
Returns the number of stored points in the map.
Definition: CPointsMap.h:408
virtual void resize(size_t newLength) override
Resizes all point buffers so they can hold the given number of points: newly created points are set t...
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLenum const GLfloat * params
Definition: glext.h:3534
Output placeholder for se2_l2_robust()
Definition: se2.h:144
const size_t NUM_MAP_FEATS
const size_t RANSAC_MINIMUM_INLIERS



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