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>
13 #include <mrpt/poses/CPosePDF.h>
14 #include <mrpt/poses/CPose3DPDF.h>
15 
19 #include <mrpt/opengl/CSphere.h>
20 #include <mrpt/opengl/CDisk.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:
191  scan1.aperture = M_PIf;
192  scan1.rightToLeft = true;
193 
194  ASSERT_(sizeof(SCAN_RANGES_1) == sizeof(float) * SCANS_SIZE);
196 
197  CObservation2DRangeScan scan2 = scan1;
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 
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 
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:
298  mrpt::make_aligned_shared<CAngularObservationMesh>();
300  mrpt::make_aligned_shared<CAngularObservationMesh>();
301 
302  CAngularObservationMesh::trace2DSetOfRays(
303  scene1, viewpoint1, aom1,
304  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
306  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
307  M_PI, HOW_MANY_YAWS));
308  CAngularObservationMesh::trace2DSetOfRays(
309  scene1, viewpoint2, aom2,
310  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
312  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
313  M_PI, HOW_MANY_YAWS));
314 
315  // Put the viewpoints origins:
316  {
318  origin1->setPose(viewpoint1);
319  origin1->setScale(0.6f);
320  scene1->insert(origin1);
321  scene2->insert(origin1);
322  }
323  {
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;
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 }
virtual void SetUp()
void changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
Definition: CPointsMap.cpp:574
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:211
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
TICPAlgorithm ICP_algorithm
The algorithm to use (default: icpClassic).
Definition: CICP.h:84
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.
mrpt::img::TColorf color
Color of points.
Definition: CPointsMap.h:312
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.
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:65
double DEG2RAD(const double x)
Degrees to radians.
virtual void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CPointsMap.cpp:803
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
void generateObjects(CSetOfObjects::Ptr &world)
TEST_F(ICPTests, AlignScans_icpClassic)
void align2scans(const TICPAlgorithm icp_method)
double smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough...
Definition: CICP.h:120
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:180
#define SCANS_SIZE
float SCAN_RANGES_2[]
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This base provides a set of functions for maths stuff.
CPose3D viewpoint2(0.5, -0.2, 2.6, DEG2RAD(-5), DEG2RAD(100), DEG2RAD(-7))
unsigned int maxIterations
Maximum number of iterations to run.
Definition: CICP.h:101
This namespace contains representation of robot actions and observations.
#define M_PIf
Definition: common.h:61
bool doRANSAC
Perform a RANSAC step, mrpt::tfest::se2_l2_robust(), after the ICP convergence, to obtain a better es...
Definition: CICP.h:133
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options mrpt_slam_grp.
Definition: CICP.h:20
float SCAN_RANGES_1[]
const size_t HOW_MANY_YAWS
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
static void generateObjects(CSetOfObjects::Ptr &world)
TRenderOptions renderOptions
Definition: CPointsMap.h:316
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
double ALFA
The scale factor for thresholds everytime convergence is achieved.
Definition: CICP.h:117
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
CPose3D SCAN2_POSE_ERROR(0.15, -0.07, 0.10, -0.03, 0.1, 0.1)
A RGB color - floats in the range [0,1].
Definition: TColor.h:77
The ICP algorithm return information.
Definition: CICP.h:190
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
void loadFromVectors(size_t nRays, const float *scanRanges, const char *scanValidity)
char SCAN_VALID_1[]
char SCAN_VALID_2[]
double thresholdDist
Initial threshold distance for two points to become a correspondence.
Definition: CICP.h:114
virtual void TearDown()
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL)
Insert the observation information into this map.
Definition: CMetricMap.cpp:95
const size_t HOW_MANY_PITCHS
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
CPose3D viewpoint1(-0.3, 0.7, 3, DEG2RAD(5), DEG2RAD(80), DEG2RAD(3))



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