Main MRPT website > C++ reference for MRPT 1.5.6
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-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 // ------------------------------------------------------
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::utils;
27 using namespace mrpt::math;
28 using namespace mrpt::random;
29 using namespace mrpt::maps;
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 tests even if the algorithm is ok.
55 {
56  randomGenerator.randomize(); // randomize with time
57  // --------------------------------
58  // Load feature map:
59  // --------------------------------
60  CSimplePointsMap the_map;
61  // Generate random MAP:
62  the_map.resize(NUM_MAP_FEATS);
63  for (size_t i=0;i<NUM_MAP_FEATS;i++)
64  {
65  the_map.setPoint(i,
68  );
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::randomGenerator.drawUniform(-10,10+MAP_SIZE_X),
79  mrpt::random::randomGenerator.drawUniform(-10,10+MAP_SIZE_Y),
80  mrpt::random::randomGenerator.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;
99  }
100 
101  // ----------------------------------------------------
102  // Generate list of individual-compatible pairings
103  // ----------------------------------------------------
104  TMatchingPairList all_correspondences;
105 
106  all_correspondences.reserve(nMapPts*nObs);
107 
108  // ALL possibilities:
109  for (size_t j=0;j<nObs;j++)
110  {
111  TMatchingPair match;
112  match.other_idx = j;
113  match.other_x = observations[j].x;
114  match.other_y = observations[j].y;
115 
116  for (size_t i=0;i<nMapPts;i++) {
117  match.this_idx = i;
118  the_map.getPoint(i, match.this_x, match.this_y );
119  all_correspondences.push_back(match);
120  }
121  }
122 
123  // ----------------------------------------------------
124  // Run RANSAC-based D-A
125  // ----------------------------------------------------
128 
129  params.ransac_minSetSize = RANSAC_MINIMUM_INLIERS; // ransac_minSetSize (to add the solution to the SOG)
130  params.ransac_maxSetSize = all_correspondences.size(); // ransac_maxSetSize: Test with all data points
132  params.ransac_nSimulations = 0; // 0=auto
133  params.ransac_fuseByCorrsMatch = true;
134  params.ransac_fuseMaxDiffXY = 0.01f;
135  params.ransac_fuseMaxDiffPhi = DEG2RAD(0.1);
136  params.ransac_algorithmForLandmarks = true;
137  params.probability_find_good_model = 0.999999;
138  params.ransac_min_nSimulations = MINIMUM_RANSAC_ITERS; // (a lower limit to the auto-detected value of ransac_nSimulations)
139  params.verbose = false;
140 
141  // Run ransac data-association:
142  mrpt::tfest::se2_l2_robust(all_correspondences, normalizationStd, params, results);
143 
144  //mrpt::poses::CPosePDFSOG & best_poses = results.transformation;
145  TMatchingPairList & out_best_pairings = results.largestSubSet;
146 
147  // Reconstruct the SE(2) transformation for these pairings:
148  mrpt::poses::CPosePDFGaussian solution_pose;
149  mrpt::tfest::se2_l2(out_best_pairings, solution_pose);
150 
151  // Normalized covariance: scale!
152  solution_pose.cov *= square(normalizationStd);
153 
154  if (!(solution_pose.mean.distanceTo(GT_pose) < 0.9 && std::abs(solution_pose.mean.phi()-GT_pose.phi())<DEG2RAD(10)))
155  {
156  std::cerr
157  << "Solution pose: " << solution_pose.mean << endl
158  << "Ground truth pose: " << GT_pose << endl;
159  return false;
160  }
161  return true;
162 }
163 
164 
165 TEST(tfest, ransac_data_assoc)
166 {
167  // Run randomized experiments:
168  bool any_ok = false;
169  for (int i=0;i<3;i++)
170  if (ransac_data_assoc_run())
171  any_ok = true;
172 
173  EXPECT_TRUE(any_ok);
174 }
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
CPose2D mean
The mean value.
const float normalizationStd
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
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.
Parameters for se2_l2_robust().
Definition: se2.h:62
mrpt::utils::TMatchingPairList largestSubSet
the largest consensus sub-set
Definition: se2.h:112
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
const size_t MINIMUM_RANSAC_ITERS
bool ransac_fuseByCorrsMatch
(Default = true) If true, the weight of Gaussian modes will be increased when an exact match in the s...
Definition: se2.h:71
unsigned int ransac_nSimulations
(Default = 0) If set to 0, an adaptive algorithm is used to determine the number of iterations...
Definition: se2.h:67
unsigned int ransac_minSetSize
(Default=3)
Definition: se2.h:64
const float ransac_mahalanobisDistanceThreshold
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
#define M_PI
Definition: bits.h:78
bool TFEST_IMPEXP se2_l2_robust(const mrpt::utils::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 ...
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.
Definition: bits.h:52
bool verbose
(Default=false)
Definition: se2.h:78
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
unsigned int ransac_maxSetSize
(Default = 20)
Definition: se2.h:65
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
A list of TMatchingPair.
Definition: TMatchingPair.h:78
bool ransac_algorithmForLandmarks
(Default = true) Use Mahalanobis distance (true) or Euclidean dist (false)
Definition: se2.h:74
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
virtual void resize(size_t newLength) MRPT_OVERRIDE
Resizes all point buffers so they can hold the given number of points: newly created points are set t...
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:150
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:219
double DEG2RAD(const double x)
Degrees to radians.
Definition: bits.h:82
double ransac_mahalanobisDistanceThreshold
(Default = 3.0)
Definition: se2.h:66
GLint * first
Definition: glew.h:1433
double ransac_fuseMaxDiffPhi
(Default=0.1degree) (In radians)
Definition: se2.h:73
const size_t NUM_OBSERVATIONS_TO_SIMUL
bool ransac_data_assoc_run()
unsigned int ransac_min_nSimulations
(Default = 1500) See parameter probability_find_good_model
Definition: se2.h:76
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:178
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.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
const double MAP_SIZE_X
size_t size() const
Returns the number of stored points in the map.
TEST(tfest, ransac_data_assoc)
#define ASSERT_(f)
GLfloat * params
Definition: glew.h:1436
bool TFEST_IMPEXP se2_l2(const mrpt::utils::TMatchingPairList &in_correspondences, mrpt::math::TPose2D &out_transformation, mrpt::math::CMatrixDouble33 *out_estimateCovariance=NULL)
Least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames...
Definition: se2_l2.cpp:45
const double MAP_SIZE_Y
Output placeholder for se2_l2_robust()
Definition: se2.h:109
const size_t NUM_MAP_FEATS
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
double ransac_fuseMaxDiffXY
(Default = 0.01)
Definition: se2.h:72
double probability_find_good_model
(Default = 0.999) See parameter ransac_nSimulations. When using probability_find_good_model, the minimum number of iterations can be set with ransac_min_nSimulations
Definition: se2.h:75
const size_t RANSAC_MINIMUM_INLIERS



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