Main MRPT website > C++ reference for MRPT 1.9.9
CICP_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 #include <mrpt/slam/CICP.h>
11 #include <mrpt/maps/CSimplePointsMap.h>
12 #include <mrpt/opengl/CAngularObservationMesh.h>
13 #include <mrpt/poses/CPosePDF.h>
14 #include <mrpt/poses/CPose3DPDF.h>
15 
16 #include <mrpt/opengl/COpenGLScene.h>
17 #include <mrpt/opengl/CGridPlaneXY.h>
18 #include <mrpt/opengl/CSetOfObjects.h>
19 #include <mrpt/opengl/CSphere.h>
20 #include <mrpt/opengl/CDisk.h>
21 #include <mrpt/opengl/stock_objects.h>
22 #include <gtest/gtest.h>
23 
24 using namespace mrpt;
25 using namespace mrpt::slam;
26 using namespace mrpt::maps;
27 using namespace mrpt::opengl;
28 using namespace mrpt::utils;
29 using namespace mrpt::poses;
30 using namespace mrpt::math;
31 using namespace mrpt::obs;
32 using namespace std;
33 
34 class ICPTests : public ::testing::Test
35 {
36  protected:
37  virtual void SetUp() {}
38  virtual void TearDown() {}
39  void align2scans(const TICPAlgorithm icp_method)
40  {
41  float SCAN_RANGES_1[] = {
42  0.910f, 0.900f, 0.910f, 0.900f, 0.900f, 0.890f, 0.890f,
43  0.880f, 0.890f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f,
44  0.880f, 0.870f, 0.880f, 0.870f, 0.870f, 0.870f, 0.880f,
45  0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f,
46  0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f,
47  0.880f, 0.890f, 0.880f, 0.880f, 0.880f, 0.890f, 0.880f,
48  0.890f, 0.890f, 0.880f, 0.890f, 0.890f, 0.880f, 0.890f,
49  0.890f, 0.890f, 0.890f, 0.890f, 0.890f, 0.900f, 0.900f,
50  0.900f, 0.900f, 0.900f, 0.910f, 0.910f, 0.910f, 0.910f,
51  0.920f, 0.920f, 0.920f, 0.920f, 0.920f, 0.930f, 0.930f,
52  0.930f, 0.930f, 0.940f, 0.940f, 0.950f, 0.950f, 0.950f,
53  0.950f, 0.960f, 0.960f, 0.970f, 0.970f, 0.970f, 0.980f,
54  0.980f, 0.990f, 1.000f, 1.000f, 1.000f, 1.010f, 1.010f,
55  1.020f, 1.030f, 1.030f, 1.030f, 1.040f, 1.050f, 1.060f,
56  1.050f, 1.060f, 1.070f, 1.070f, 1.080f, 1.080f, 1.090f,
57  1.100f, 1.110f, 1.120f, 1.120f, 1.130f, 1.140f, 1.140f,
58  1.160f, 1.170f, 1.180f, 1.180f, 1.190f, 1.200f, 1.220f,
59  1.220f, 1.230f, 1.230f, 1.240f, 1.250f, 1.270f, 1.280f,
60  1.290f, 1.300f, 1.320f, 1.320f, 1.350f, 1.360f, 1.370f,
61  1.390f, 1.410f, 1.410f, 1.420f, 1.430f, 1.450f, 1.470f,
62  1.490f, 1.500f, 1.520f, 1.530f, 1.560f, 1.580f, 1.600f,
63  1.620f, 1.650f, 1.670f, 1.700f, 1.730f, 1.750f, 1.780f,
64  1.800f, 1.830f, 1.850f, 1.880f, 1.910f, 1.940f, 1.980f,
65  2.010f, 2.060f, 2.090f, 2.130f, 2.180f, 2.220f, 2.250f,
66  2.300f, 2.350f, 2.410f, 2.460f, 2.520f, 2.570f, 2.640f,
67  2.700f, 2.780f, 2.850f, 2.930f, 3.010f, 3.100f, 3.200f,
68  3.300f, 3.390f, 3.500f, 3.620f, 3.770f, 3.920f, 4.070f,
69  4.230f, 4.430f, 4.610f, 4.820f, 5.040f, 5.290f, 5.520f,
70  8.970f, 8.960f, 8.950f, 8.930f, 8.940f, 8.930f, 9.050f,
71  9.970f, 9.960f, 10.110f, 13.960f, 18.870f, 19.290f, 81.910f,
72  20.890f, 48.750f, 48.840f, 48.840f, 19.970f, 19.980f, 19.990f,
73  15.410f, 20.010f, 19.740f, 17.650f, 17.400f, 14.360f, 12.860f,
74  11.260f, 11.230f, 8.550f, 8.630f, 9.120f, 9.120f, 8.670f,
75  8.570f, 7.230f, 7.080f, 7.040f, 6.980f, 6.970f, 5.260f,
76  5.030f, 4.830f, 4.620f, 4.440f, 4.390f, 4.410f, 4.410f,
77  4.410f, 4.430f, 4.440f, 4.460f, 4.460f, 4.490f, 4.510f,
78  4.540f, 3.970f, 3.820f, 3.730f, 3.640f, 3.550f, 3.460f,
79  3.400f, 3.320f, 3.300f, 3.320f, 3.320f, 3.340f, 2.790f,
80  2.640f, 2.600f, 2.570f, 2.540f, 2.530f, 2.510f, 2.490f,
81  2.490f, 2.480f, 2.470f, 2.460f, 2.460f, 2.460f, 2.450f,
82  2.450f, 2.450f, 2.460f, 2.460f, 2.470f, 2.480f, 2.490f,
83  2.490f, 2.520f, 2.510f, 2.550f, 2.570f, 2.610f, 2.640f,
84  2.980f, 3.040f, 3.010f, 2.980f, 2.940f, 2.920f, 2.890f,
85  2.870f, 2.830f, 2.810f, 2.780f, 2.760f, 2.740f, 2.720f,
86  2.690f, 2.670f, 2.650f, 2.630f, 2.620f, 2.610f, 2.590f,
87  2.560f, 2.550f, 2.530f, 2.510f, 2.500f, 2.480f, 2.460f,
88  2.450f, 2.430f, 2.420f, 2.400f, 2.390f, 2.380f, 2.360f,
89  2.350f, 2.340f, 2.330f, 2.310f, 2.300f, 2.290f, 2.280f,
90  2.270f, 2.260f, 2.250f, 2.240f, 2.230f, 2.230f, 2.220f,
91  2.210f, 2.200f, 2.190f, 2.180f, 2.170f, 1.320f, 1.140f,
92  1.130f, 1.130f, 1.120f, 1.120f, 1.110f, 1.110f, 1.110f,
93  1.110f, 1.100f, 1.110f, 1.100f};
94  char SCAN_VALID_1[] = {
95  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
96  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
97  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
98  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
99  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
100  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
101  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
102  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
103  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
104  1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
105  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
106  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
107  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
108  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
109  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
110  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
111 
112  float SCAN_RANGES_2[] = {
113  0.720f, 0.720f, 0.720f, 0.720f, 0.720f, 0.720f, 0.710f,
114  0.720f, 0.710f, 0.710f, 0.710f, 0.710f, 0.710f, 0.710f,
115  0.710f, 0.710f, 0.710f, 0.710f, 0.710f, 0.710f, 0.720f,
116  0.720f, 0.720f, 0.720f, 0.730f, 0.730f, 0.730f, 0.730f,
117  0.730f, 0.730f, 0.730f, 0.740f, 0.740f, 0.740f, 0.740f,
118  0.740f, 0.740f, 0.750f, 0.750f, 0.750f, 0.750f, 0.750f,
119  0.750f, 0.750f, 0.750f, 0.760f, 0.760f, 0.760f, 0.760f,
120  0.760f, 0.760f, 0.760f, 0.760f, 0.770f, 0.770f, 0.770f,
121  0.770f, 0.780f, 0.780f, 0.780f, 0.790f, 0.790f, 0.800f,
122  0.800f, 0.800f, 0.800f, 0.800f, 0.800f, 0.810f, 0.810f,
123  0.820f, 0.820f, 0.830f, 0.830f, 0.840f, 0.840f, 0.850f,
124  0.850f, 0.860f, 0.860f, 0.860f, 0.870f, 0.870f, 0.880f,
125  0.890f, 0.890f, 0.900f, 0.900f, 0.910f, 0.920f, 0.930f,
126  0.930f, 0.940f, 0.940f, 0.940f, 0.950f, 0.960f, 0.960f,
127  0.970f, 0.980f, 0.990f, 1.000f, 1.010f, 1.020f, 1.030f,
128  1.040f, 1.050f, 1.060f, 1.070f, 1.080f, 1.080f, 1.100f,
129  1.100f, 1.120f, 1.120f, 1.140f, 1.140f, 1.170f, 1.160f,
130  1.180f, 1.190f, 1.210f, 1.220f, 1.240f, 1.250f, 1.280f,
131  1.290f, 1.300f, 1.320f, 1.340f, 1.350f, 1.380f, 1.390f,
132  1.420f, 1.440f, 1.460f, 1.470f, 1.500f, 1.520f, 1.550f,
133  1.570f, 1.600f, 1.630f, 1.670f, 1.690f, 1.730f, 1.760f,
134  1.790f, 1.820f, 1.870f, 1.900f, 1.940f, 1.970f, 2.030f,
135  2.080f, 2.130f, 2.170f, 2.230f, 2.280f, 2.340f, 2.400f,
136  2.490f, 2.550f, 2.630f, 2.700f, 2.810f, 2.880f, 3.010f,
137  3.090f, 3.240f, 3.340f, 3.500f, 3.620f, 3.810f, 3.950f,
138  4.180f, 4.340f, 4.620f, 8.170f, 8.140f, 8.150f, 8.120f,
139  8.110f, 8.100f, 8.100f, 8.300f, 9.040f, 9.130f, 9.130f,
140  13.030f, 18.050f, 19.150f, 81.910f, 20.070f, 47.980f, 48.040f,
141  48.030f, 19.140f, 19.180f, 19.180f, 19.190f, 14.550f, 19.210f,
142  16.850f, 16.840f, 7.800f, 7.770f, 7.770f, 7.750f, 7.770f,
143  7.760f, 7.780f, 7.760f, 8.320f, 8.350f, 8.350f, 8.090f,
144  7.720f, 7.730f, 6.430f, 6.360f, 6.290f, 6.260f, 6.230f,
145  6.220f, 6.160f, 5.800f, 4.510f, 4.410f, 4.240f, 4.140f,
146  4.000f, 3.910f, 3.790f, 3.680f, 3.660f, 3.680f, 3.680f,
147  3.700f, 3.710f, 3.730f, 3.730f, 3.760f, 3.770f, 3.790f,
148  3.820f, 3.850f, 3.900f, 3.940f, 3.980f, 3.250f, 3.180f,
149  3.140f, 3.070f, 3.030f, 2.970f, 2.930f, 2.880f, 2.850f,
150  2.790f, 2.760f, 2.710f, 2.680f, 2.660f, 2.670f, 2.690f,
151  2.710f, 2.720f, 2.740f, 2.760f, 2.770f, 2.780f, 2.800f,
152  2.170f, 2.120f, 2.090f, 2.060f, 2.020f, 2.010f, 1.990f,
153  1.980f, 1.970f, 1.960f, 1.950f, 1.950f, 1.940f, 1.940f,
154  1.950f, 1.940f, 1.940f, 1.950f, 1.930f, 1.940f, 1.940f,
155  1.940f, 1.940f, 1.940f, 1.950f, 1.960f, 1.960f, 1.980f,
156  1.980f, 2.000f, 2.010f, 2.030f, 2.060f, 2.090f, 2.120f,
157  2.190f, 2.560f, 2.540f, 2.530f, 2.520f, 2.500f, 2.490f,
158  2.470f, 2.460f, 2.450f, 2.440f, 2.420f, 2.410f, 2.400f,
159  2.390f, 2.380f, 2.370f, 2.360f, 2.350f, 2.340f, 2.340f,
160  2.330f, 2.320f, 2.310f, 2.300f, 2.290f, 2.290f, 2.290f,
161  2.280f, 2.270f, 2.260f, 2.260f, 2.250f, 2.240f, 2.240f,
162  2.230f, 2.230f, 2.220f, 2.220f, 2.210f, 2.210f, 2.200f,
163  2.200f, 2.190f, 2.190f, 2.190f, 2.180f, 2.180f, 2.170f,
164  2.170f, 2.170f, 2.160f, 2.160f};
165  char SCAN_VALID_2[] = {
166  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
167  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
168  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
169  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
170  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
171  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
172  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
173  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
174  1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
175  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
176  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
177  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
178  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
179  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
180  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
181  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
182 
183 #define SCANS_SIZE (sizeof(SCAN_RANGES_1) / sizeof(SCAN_RANGES_1[0]))
184 
185  CSimplePointsMap m1, m2;
186  float runningTime;
187  CICP::TReturnInfo info;
188  CICP ICP;
189 
190  // Load scans:
191  CObservation2DRangeScan scan1;
192  scan1.aperture = M_PIf;
193  scan1.rightToLeft = true;
194 
195  ASSERT_(sizeof(SCAN_RANGES_1) == sizeof(float) * SCANS_SIZE);
196  scan1.loadFromVectors(SCANS_SIZE, SCAN_RANGES_1, SCAN_VALID_1);
197 
198  CObservation2DRangeScan scan2 = scan1;
199  scan2.loadFromVectors(SCANS_SIZE, SCAN_RANGES_2, SCAN_VALID_2);
200 
201  // Build the points maps from the scans:
202  m1.insertObservation(&scan1);
203  m2.insertObservation(&scan2);
204 
205  // -----------------------------------------------------
206  ICP.options.ICP_algorithm = icp_method;
207 
208  ICP.options.maxIterations = 100;
209  ICP.options.thresholdAng = DEG2RAD(10.0f);
210  ICP.options.thresholdDist = 0.75f;
211  ICP.options.ALFA = 0.5f;
212  ICP.options.smallestThresholdDist = 0.05f;
213  ICP.options.doRANSAC = false;
214  // ICP.options.dumpToConsole();
215  // -----------------------------------------------------
216  CPose2D initialPose(0.8f, 0.0f, (float)DEG2RAD(0.0f));
217 
218  CPosePDF::Ptr pdf =
219  ICP.Align(&m1, &m2, initialPose, &runningTime, (void*)&info);
220 
221  /*printf("ICP run in %.02fms, %d iterations (%.02fms/iter), %.01f%%
222  goodness\n -> ",
223  runningTime*1000,
224  info.nIterations,
225  runningTime*1000.0f/info.nIterations,
226  info.goodness*100 );*/
227 
228  // cout << "Mean of estimation: " << pdf->getEstimatedPose() << endl<<
229  // endl;
230  // Should be around: Mean of estimation: (0.820,0.084,8.73deg)
231 
232  const CPose2D good_pose(0.820, 0.084, DEG2RAD(8.73));
233 
234  EXPECT_NEAR(good_pose.distanceTo(pdf->getMeanVal()), 0, 0.02);
235  }
236 
237  static void generateObjects(CSetOfObjects::Ptr& world)
238  {
239  CSphere::Ptr sph = mrpt::make_aligned_shared<CSphere>(0.5);
240  sph->setLocation(0, 0, 0);
241  sph->setColor(1, 0, 0);
242  world->insert(sph);
243 
244  CDisk::Ptr pln = mrpt::make_aligned_shared<opengl::CDisk>();
245  pln->setDiskRadius(2);
246  pln->setPose(CPose3D(0, 0, 0, 0, DEG2RAD(5), DEG2RAD(5)));
247  pln->setColor(0.8, 0, 0);
248  world->insert(pln);
249 
250  {
251  CDisk::Ptr pln = mrpt::make_aligned_shared<opengl::CDisk>();
252  pln->setDiskRadius(2);
253  pln->setPose(
254  CPose3D(0, 0, 0, DEG2RAD(30), DEG2RAD(-20), DEG2RAD(-2)));
255  pln->setColor(0.9, 0, 0);
256  world->insert(pln);
257  }
258  }
259 };
260 
261 TEST_F(ICPTests, AlignScans_icpClassic) { align2scans(icpClassic); }
262 TEST_F(ICPTests, AlignScans_icpLevenbergMarquardt)
263 
264 {
265  align2scans(icpLevenbergMarquardt);
266 }
267 
268 TEST_F(ICPTests, RayTracingICP3D)
269 {
270  // Increase this values to get more precision. It will also increase run
271  // time.
272  const size_t HOW_MANY_YAWS = 150;
273  const size_t HOW_MANY_PITCHS = 150;
274 
275  // The scans of the 3D object, taken from 2 different places:
276  vector<CObservation2DRangeScan> sequence_scans1, sequence_scans2;
277 
278  // The two origins for the 3D scans
279  CPose3D viewpoint1(-0.3, 0.7, 3, DEG2RAD(5), DEG2RAD(80), DEG2RAD(3));
280  CPose3D viewpoint2(0.5, -0.2, 2.6, DEG2RAD(-5), DEG2RAD(100), DEG2RAD(-7));
281 
282  CPose3D SCAN2_POSE_ERROR(0.15, -0.07, 0.10, -0.03, 0.1, 0.1);
283 
284  // Create the reference objects:
285  COpenGLScene::Ptr scene1 = mrpt::make_aligned_shared<COpenGLScene>();
286  COpenGLScene::Ptr scene2 = mrpt::make_aligned_shared<COpenGLScene>();
287  COpenGLScene::Ptr scene3 = mrpt::make_aligned_shared<COpenGLScene>();
288 
289  opengl::CGridPlaneXY::Ptr plane1 =
290  mrpt::make_aligned_shared<CGridPlaneXY>(-20, 20, -20, 20, 0, 1);
291  plane1->setColor(0.3, 0.3, 0.3);
292  scene1->insert(plane1);
293  scene2->insert(plane1);
294  scene3->insert(plane1);
295 
296  CSetOfObjects::Ptr world = mrpt::make_aligned_shared<CSetOfObjects>();
297  generateObjects(world);
298  scene1->insert(world);
299 
300  // Perform the 3D scans:
301  CAngularObservationMesh::Ptr aom1 =
302  mrpt::make_aligned_shared<CAngularObservationMesh>();
303  CAngularObservationMesh::Ptr aom2 =
304  mrpt::make_aligned_shared<CAngularObservationMesh>();
305 
306  CAngularObservationMesh::trace2DSetOfRays(
307  scene1, viewpoint1, aom1,
308  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
309  M_PI, HOW_MANY_PITCHS),
310  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
311  M_PI, HOW_MANY_YAWS));
312  CAngularObservationMesh::trace2DSetOfRays(
313  scene1, viewpoint2, aom2,
314  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
315  M_PI, HOW_MANY_PITCHS),
316  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
317  M_PI, HOW_MANY_YAWS));
318 
319  // Put the viewpoints origins:
320  {
321  CSetOfObjects::Ptr origin1 = opengl::stock_objects::CornerXYZ();
322  origin1->setPose(viewpoint1);
323  origin1->setScale(0.6f);
324  scene1->insert(origin1);
325  scene2->insert(origin1);
326  }
327  {
328  CSetOfObjects::Ptr origin2 = opengl::stock_objects::CornerXYZ();
329  origin2->setPose(viewpoint2);
330  origin2->setScale(0.6f);
331  scene1->insert(origin2);
332  scene2->insert(origin2);
333  }
334 
335  // Show the scanned points:
336  CSimplePointsMap M1, M2;
337 
338  aom1->generatePointCloud(&M1);
339  aom2->generatePointCloud(&M2);
340 
341  // Create the wrongly-localized M2:
342  CSimplePointsMap M2_noisy;
343  M2_noisy = M2;
344  M2_noisy.changeCoordinatesReference(SCAN2_POSE_ERROR);
345 
346  CSetOfObjects::Ptr PTNS1 = mrpt::make_aligned_shared<CSetOfObjects>();
347  CSetOfObjects::Ptr PTNS2 = mrpt::make_aligned_shared<CSetOfObjects>();
348 
349  CPointsMap::COLOR_3DSCENE(mrpt::utils::TColorf(1, 0, 0));
350  M1.getAs3DObject(PTNS1);
351 
352  CPointsMap::COLOR_3DSCENE(mrpt::utils::TColorf(0, 0, 1));
353  M2_noisy.getAs3DObject(PTNS2);
354 
355  scene2->insert(PTNS1);
356  scene2->insert(PTNS2);
357 
358  // --------------------------------------
359  // Do the ICP-3D
360  // --------------------------------------
361  float run_time;
362  CICP icp;
363  CICP::TReturnInfo icp_info;
364 
365  icp.options.thresholdDist = 0.40f;
366  icp.options.thresholdAng = 0;
367 
368  CPose3DPDF::Ptr pdf = icp.Align3D(
369  &M2_noisy, // Map to align
370  &M1, // Reference map
371  CPose3D(), // Initial gross estimate
372  &run_time, &icp_info);
373 
374  CPose3D mean = pdf->getMeanVal();
375 
376  // Checks:
377  EXPECT_NEAR(
378  0, (mean.getAsVectorVal() - SCAN2_POSE_ERROR.getAsVectorVal())
379  .array()
380  .abs()
381  .mean(),
382  0.02)
383  << "ICP output: mean= " << mean << endl
384  << "Real displacement: " << SCAN2_POSE_ERROR << endl;
385 }
TEST_F
TEST_F(ICPTests, AlignScans_icpClassic)
Definition: CICP_unittest.cpp:261
SCANS_SIZE
#define SCANS_SIZE
M_PI
#define M_PI
Definition: bits.h:92
DEG2RAD
#define DEG2RAD
Definition: bits.h:112
ICPTests::SetUp
virtual void SetUp()
Definition: CICP_unittest.cpp:37
ICPTests::generateObjects
static void generateObjects(CSetOfObjects::Ptr &world)
Definition: CICP_unittest.cpp:237
ICPTests::TearDown
virtual void TearDown()
Definition: CICP_unittest.cpp:38
ICPTests::align2scans
void align2scans(const TICPAlgorithm icp_method)
Definition: CICP_unittest.cpp:39
mrpt::maps::CMetricMap::insertObservation
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL)
Insert the observation information into this map.
Definition: CMetricMap.cpp:95
mrpt::maps::CPointsMap::changeCoordinatesReference
void changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
Definition: CPointsMap.cpp:613
mrpt::maps::CPointsMap::getAs3DObject
virtual void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CPointsMap.cpp:801
mrpt::maps::CSimplePointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
Definition: CSimplePointsMap.h:32
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:56
mrpt::obs::CObservation2DRangeScan::aperture
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees).
Definition: CObservation2DRangeScan.h:121
mrpt::obs::CObservation2DRangeScan::loadFromVectors
void loadFromVectors(size_t nRays, const float *scanRanges, const char *scanValidity)
Definition: CObservation2DRangeScan.cpp:603
mrpt::obs::CObservation2DRangeScan::rightToLeft
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
Definition: CObservation2DRangeScan.h:123
mrpt::opengl::CAngularObservationMesh::Ptr
std::shared_ptr< CAngularObservationMesh > Ptr
Definition: CAngularObservationMesh.h:46
mrpt::opengl::CDisk::Ptr
std::shared_ptr< CDisk > Ptr
Definition: CDisk.h:35
mrpt::opengl::CGridPlaneXY::Ptr
std::shared_ptr< CGridPlaneXY > Ptr
Definition: CGridPlaneXY.h:34
mrpt::opengl::COpenGLScene::Ptr
std::shared_ptr< COpenGLScene > Ptr
Definition: COpenGLScene.h:62
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< CSetOfObjects > Ptr
Definition: CSetOfObjects.h:32
mrpt::opengl::CSphere::Ptr
std::shared_ptr< CSphere > Ptr
Definition: CSphere.h:33
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:41
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:89
mrpt::poses::CPose3DPDF::Ptr
std::shared_ptr< CPose3DPDF > Ptr
Definition: CPose3DPDF.h:45
mrpt::poses::CPoseOrPoint::distanceTo
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:206
mrpt::poses::CPoseOrPoint::getAsVectorVal
mrpt::math::CVectorDouble getAsVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:265
mrpt::poses::CPosePDF::Ptr
std::shared_ptr< CPosePDF > Ptr
Definition: CPosePDF.h:44
mrpt::slam::CICP::TConfigParams::maxIterations
unsigned int maxIterations
Maximum number of iterations to run.
Definition: CICP.h:109
mrpt::slam::CICP::TConfigParams::ICP_algorithm
TICPAlgorithm ICP_algorithm
The algorithm to use (default: icpClassic).
Definition: CICP.h:89
mrpt::slam::CICP::TConfigParams::smallestThresholdDist
float smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough.
Definition: CICP.h:127
mrpt::slam::CICP::TConfigParams::ALFA
float ALFA
The scale factor for threshold everytime convergence is achieved.
Definition: CICP.h:124
mrpt::slam::CICP::TConfigParams::doRANSAC
bool doRANSAC
Perform a RANSAC step, mrpt::tfest::se2_l2_robust(), after the ICP convergence, to obtain a better es...
Definition: CICP.h:140
mrpt::slam::CICP::TConfigParams::thresholdDist
float thresholdDist
Initial threshold distance for two points to become a correspondence.
Definition: CICP.h:122
mrpt::slam::CICP
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:68
mrpt::slam::CICP::options
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:185
mrpt::slam::CMetricMapsAlignmentAlgorithm::Align3D
mrpt::poses::CPose3DPDF::Ptr Align3D(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3D &grossEst, float *runningTime=nullptr, void *info=nullptr)
The method for aligning a pair of metric maps, aligning the full 6D pose.
Definition: CMetricMapsAlignmentAlgorithm.cpp:37
mrpt::slam::CMetricMapsAlignmentAlgorithm::Align
mrpt::poses::CPosePDF::Ptr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, float *runningTime=nullptr, void *info=nullptr)
The method for aligning a pair of metric maps, aligning only 2D + orientation.
Definition: CMetricMapsAlignmentAlgorithm.cpp:26
mean
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
Definition: eigen_plugins.h:431
ASSERT_
#define ASSERT_(f)
Definition: mrpt_macros.h:309
M_PIf
#define M_PIf
Definition: mrpt_macros.h:440
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:20
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:20
mrpt::opengl::stock_objects::CornerXYZ
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
Definition: StockObjects.cpp:211
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:16
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:18
mrpt::slam::TICPAlgorithm
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options mrpt_slam_grp
Definition: CICP.h:23
mrpt::slam::icpLevenbergMarquardt
@ icpLevenbergMarquardt
Definition: CICP.h:25
mrpt::slam::icpClassic
@ icpClassic
Definition: CICP.h:24
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values,...
Definition: CParticleFilter.h:224
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:18
stock_objects.h
mrpt::slam::CICP::TReturnInfo
The ICP algorithm return information.
Definition: CICP.h:196
mrpt::utils::TColorf
A RGB color - floats in the range [0,1].
Definition: TColor.h:79


Page generated by Doxygen 1.9.1 for MRPT 1.9.9 Git: 63ea9d1f1 Thu Nov 23 00:06:53 2017 +0100 at mar 26 may 2026 12:19:29 CEST