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


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST