MRPT  1.9.9
test.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  * Execute the iterating closest point algorithm (ICP) on a hardcoded pair of
12  * laser data. The algorithm computes the transformation (translation and
13  * rotation) for aligning the 2 sets of laser scans and plots the
14  */
15 
16 #include <mrpt/maps/CSimplePointsMap.h>
17 #include <mrpt/obs/CObservation2DRangeScan.h>
18 #include <mrpt/slam/CICP.h>
19 #include <mrpt/poses/CPose2D.h>
20 #include <mrpt/poses/CPosePDF.h>
21 #include <mrpt/poses/CPosePDFGaussian.h>
22 #include <mrpt/gui.h>
23 #include <mrpt/math/utils.h>
24 
25 #include <iostream>
26 #include <fstream>
27 
28 using namespace mrpt;
29 using namespace mrpt::slam;
30 using namespace mrpt::maps;
31 using namespace mrpt::obs;
32 using namespace mrpt::math;
33 using namespace mrpt::poses;
34 using namespace std;
35 
36 bool skip_window = false;
37 int ICP_method = (int)icpClassic;
38 
39 #define SCANS_SIZE 361
40 
41 #if 0
42  // Hard matching in a corridor
43  float SCAN_RANGES_1[] = { 2.670f,2.670f,2.670f,2.670f,2.670f,2.660f,2.670f,2.670f,2.670f,2.670f,2.670f,2.670f,2.670f,2.670f,2.670f,2.670f,2.680f,2.680f,2.680f,2.680f,2.690f,2.690f,2.700f,2.690f,2.700f,2.700f,2.710f,2.710f,2.720f,2.720f,2.720f,2.730f,2.740f,2.730f,2.740f,2.750f,2.750f,2.760f,2.770f,2.780f,2.780f,2.790f,2.800f,2.800f,2.820f,2.820f,2.830f,2.840f,2.850f,2.860f,2.870f,2.880f,2.890f,2.900f,2.910f,2.920f,2.930f,2.940f,2.950f,2.970f,2.990f,2.990f,3.010f,3.020f,3.040f,3.050f,3.070f,3.080f,3.100f,3.120f,3.140f,3.150f,3.170f,3.180f,3.200f,3.220f,3.240f,3.250f,3.280f,3.300f,3.320f,3.340f,3.370f,3.390f,3.420f,3.440f,3.470f,3.490f,3.510f,3.540f,3.570f,3.590f,3.630f,3.650f,3.690f,3.710f,3.750f,3.770f,3.820f,3.850f,3.890f,3.920f,3.960f,3.990f,4.030f,4.060f,4.120f,4.160f,4.210f,4.230f,4.290f,4.310f,4.380f,4.420f,4.490f,4.530f,4.590f,4.640f,4.710f,4.760f,4.830f,4.890f,4.970f,5.010f,5.110f,5.170f,5.260f,5.330f,5.410f,5.490f,5.590f,5.670f,5.780f,5.860f,5.980f,6.070f,6.180f,6.300f,6.410f,6.530f,6.670f,6.790f,6.950f,7.100f,7.250f,7.400f,7.590f,7.750f,7.940f,8.140f,8.380f,8.580f,8.840f,9.060f,9.360f,9.610f,9.940f,10.210f,10.600f,10.930f,11.380f,11.750f,12.280f,12.730f,13.300f,13.850f,14.580f,15.210f,16.040f,16.820f,17.890f,18.840f,20.210f,21.470f,23.260f,81.870f,81.870f,81.910f,81.910f,81.910f,39.910f,39.940f,81.870f, 81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.870f,81.910f,81.870f,81.870f,21.120f,21.120f,21.180f,21.190f,21.250f,19.140f,21.310f,14.710f,14.170f,13.680f,13.130f,12.690f,12.230f,11.860f,11.450f,11.140f,10.800f,10.520f,10.180f,9.920f,9.650f,9.440f,9.170f,8.980f,8.740f,8.570f,8.360f,8.180f,7.990f,7.850f,7.660f,7.540f,7.380f,7.260f,7.110f,6.990f,6.860f,6.760f,6.620f,6.530f,6.410f,6.310f,6.200f,6.120f,6.020f,5.940f,5.840f,5.780f,5.680f,5.620f,5.520f,5.460f,5.380f,5.320f,5.250f,5.200f,5.130f,5.080f,5.010f,4.960f,4.900f,4.860f,4.790f,4.760f,4.700f,4.660f,4.610f,4.570f,4.520f,4.490f,4.430f,4.400f,4.350f,4.320f,4.280f,4.240f,4.200f,4.180f,4.140f,4.120f,4.070f,4.050f,4.010f,3.990f,3.950f,3.930f,3.900f,3.870f,3.850f,3.820f,3.790f,3.770f,3.740f,3.720f,3.700f,3.670f,3.650f,3.640f,3.610f,3.590f,3.570f,3.560f,3.530f,3.520f,3.500f,3.490f,3.460f,3.450f,3.430f,3.420f,3.400f,3.400f,3.380f,3.360f,3.360f,3.330f,3.320f,3.310f,3.300f,3.290f,3.270f,3.260f,3.250f,3.240f,3.230f,3.220f,3.210f,3.200f,3.200f,3.190f,3.180f,3.170f,3.160f,3.150f,3.150f,3.150f,3.140f,3.140f,3.120f,3.120f,3.110f,3.100f,3.100f,3.100f,3.100f,3.090f,3.080f,3.080f,3.080f,3.080f,3.070f,3.070f,3.070f,3.060f,3.070f,3.060f,3.060f };
44  char SCAN_VALID_1[] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};
45 
46  float SCAN_RANGES_2[] = {2.630f,2.630f,2.630f,2.630f,2.630f,2.630f,2.630f,2.630f,2.630f,2.630f,2.630f,2.630f,2.630f,2.630f,2.640f,2.640f,2.640f,2.640f,2.640f,2.640f,2.650f,2.650f,2.650f,2.650f,2.660f,2.660f,2.670f,2.670f,2.670f,2.670f,2.680f,2.680f,2.690f,2.690f,2.700f,2.710f,2.710f,2.710f,2.720f,2.730f,2.740f,2.750f,2.750f,2.760f,2.770f,2.780f,2.790f,2.800f,2.800f,2.820f,2.830f,2.840f,2.850f,2.860f,2.870f,2.880f,2.890f,2.900f,2.920f,2.930f,2.930f,2.950f,2.960f,2.980f,2.990f,3.000f,3.020f,3.040f,3.060f,3.070f,3.090f,3.110f,3.120f,3.140f,3.150f,3.170f,3.190f,3.200f,3.230f,3.250f,3.270f,3.290f,3.320f,3.330f,3.360f,3.380f,3.410f,3.430f,3.460f,3.490f,3.510f,3.540f,3.570f,3.590f,3.620f,3.650f,3.680f,3.720f,3.750f,3.750f,3.800f,3.850f,3.880f,3.930f,3.960f,3.990f,4.040f,4.060f,4.110f,4.160f,4.210f,4.250f,4.300f,4.330f,4.410f,4.450f,4.500f,4.550f,4.610f,4.660f,4.730f,4.780f,4.860f,4.930f,4.990f,5.060f,5.140f,5.210f,5.300f,5.380f,5.450f,5.540f,5.630f,5.730f,5.820f,5.920f,6.030f,6.130f,6.240f,6.360f,6.490f,6.620f,6.760f,6.900f,7.050f,7.210f,7.370f,7.540f,7.730f,7.920f,8.120f,8.330f,8.550f,8.780f,9.040f,9.300f,9.580f,9.880f,10.230f,10.580f,10.940f,11.340f,11.780f,12.240f,12.740f,13.300f,13.910f,14.580f,15.260f,16.030f,16.930f,17.900f,19.060f,20.300f,81.870f,23.500f,81.910f,81.910f,81.910f,81.910f,81.910f,38.760f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.910f,81.870f,81.910f,20.620f,20.000f,20.020f,20.050f,20.070f,17.870f,20.140f,17.950f,20.210f,13.560f,13.160f,12.700f,12.240f,11.850f,11.490f,11.120f,10.840f,10.510f,10.230f,9.940f,9.700f,9.440f,9.220f,8.990f,8.790f,8.570f,8.390f,8.210f,8.030f,7.860f,7.710f,7.560f,7.420f,7.270f,7.150f,7.020f,6.900f,6.770f,6.660f,6.550f,6.460f,6.340f,6.260f,6.150f,6.060f,5.980f,5.890f,5.800f,5.730f,5.650f,5.580f,5.500f,5.430f,5.360f,5.300f,5.230f,5.170f,5.100f,5.060f,5.000f,4.940f,4.880f,4.830f,4.770f,4.730f,4.690f,4.640f,4.600f,4.550f,4.520f,4.480f,4.420f,4.390f,4.340f,4.310f,4.280f,4.240f,4.200f,4.180f,4.150f,4.120f,4.070f,4.050f,4.010f,3.990f,3.960f,3.940f,3.910f,3.880f,3.850f,3.830f,3.800f,3.780f,3.760f,3.740f,3.710f,3.690f,3.660f,3.640f,3.620f,3.600f,3.580f,3.570f,3.550f,3.540f,3.520f,3.510f,3.490f,3.470f,3.450f,3.440f,3.430f,3.410f,3.400f,3.390f,3.380f,3.380f,3.350f,3.350f,3.340f,3.330f,3.290f,3.290f,3.270f,3.260f,3.250f,3.240f,3.240f,3.230f,3.220f,3.210f,3.200f,3.200f,3.180f,3.180f,3.180f,3.180f,3.160f,3.150f,3.150f,3.150f,3.130f,3.130f,3.120f,3.130f,3.120f,3.140f,3.130f,3.130f,3.110f,3.120f,3.120f,3.120f,3.100f,3.100f,3.090f};
47  char SCAN_VALID_2[] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1 };
48 #else
49 // A more usual case:
50 float SCAN_RANGES_1[] = {
51  0.910f, 0.900f, 0.910f, 0.900f, 0.900f, 0.890f, 0.890f, 0.880f,
52  0.890f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.870f,
53  0.880f, 0.870f, 0.870f, 0.870f, 0.880f, 0.880f, 0.880f, 0.880f,
54  0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f,
55  0.880f, 0.880f, 0.880f, 0.880f, 0.890f, 0.880f, 0.880f, 0.880f,
56  0.890f, 0.880f, 0.890f, 0.890f, 0.880f, 0.890f, 0.890f, 0.880f,
57  0.890f, 0.890f, 0.890f, 0.890f, 0.890f, 0.890f, 0.900f, 0.900f,
58  0.900f, 0.900f, 0.900f, 0.910f, 0.910f, 0.910f, 0.910f, 0.920f,
59  0.920f, 0.920f, 0.920f, 0.920f, 0.930f, 0.930f, 0.930f, 0.930f,
60  0.940f, 0.940f, 0.950f, 0.950f, 0.950f, 0.950f, 0.960f, 0.960f,
61  0.970f, 0.970f, 0.970f, 0.980f, 0.980f, 0.990f, 1.000f, 1.000f,
62  1.000f, 1.010f, 1.010f, 1.020f, 1.030f, 1.030f, 1.030f, 1.040f,
63  1.050f, 1.060f, 1.050f, 1.060f, 1.070f, 1.070f, 1.080f, 1.080f,
64  1.090f, 1.100f, 1.110f, 1.120f, 1.120f, 1.130f, 1.140f, 1.140f,
65  1.160f, 1.170f, 1.180f, 1.180f, 1.190f, 1.200f, 1.220f, 1.220f,
66  1.230f, 1.230f, 1.240f, 1.250f, 1.270f, 1.280f, 1.290f, 1.300f,
67  1.320f, 1.320f, 1.350f, 1.360f, 1.370f, 1.390f, 1.410f, 1.410f,
68  1.420f, 1.430f, 1.450f, 1.470f, 1.490f, 1.500f, 1.520f, 1.530f,
69  1.560f, 1.580f, 1.600f, 1.620f, 1.650f, 1.670f, 1.700f, 1.730f,
70  1.750f, 1.780f, 1.800f, 1.830f, 1.850f, 1.880f, 1.910f, 1.940f,
71  1.980f, 2.010f, 2.060f, 2.090f, 2.130f, 2.180f, 2.220f, 2.250f,
72  2.300f, 2.350f, 2.410f, 2.460f, 2.520f, 2.570f, 2.640f, 2.700f,
73  2.780f, 2.850f, 2.930f, 3.010f, 3.100f, 3.200f, 3.300f, 3.390f,
74  3.500f, 3.620f, 3.770f, 3.920f, 4.070f, 4.230f, 4.430f, 4.610f,
75  4.820f, 5.040f, 5.290f, 5.520f, 8.970f, 8.960f, 8.950f, 8.930f,
76  8.940f, 8.930f, 9.050f, 9.970f, 9.960f, 10.110f, 13.960f, 18.870f,
77  19.290f, 81.910f, 20.890f, 48.750f, 48.840f, 48.840f, 19.970f, 19.980f,
78  19.990f, 15.410f, 20.010f, 19.740f, 17.650f, 17.400f, 14.360f, 12.860f,
79  11.260f, 11.230f, 8.550f, 8.630f, 9.120f, 9.120f, 8.670f, 8.570f,
80  7.230f, 7.080f, 7.040f, 6.980f, 6.970f, 5.260f, 5.030f, 4.830f,
81  4.620f, 4.440f, 4.390f, 4.410f, 4.410f, 4.410f, 4.430f, 4.440f,
82  4.460f, 4.460f, 4.490f, 4.510f, 4.540f, 3.970f, 3.820f, 3.730f,
83  3.640f, 3.550f, 3.460f, 3.400f, 3.320f, 3.300f, 3.320f, 3.320f,
84  3.340f, 2.790f, 2.640f, 2.600f, 2.570f, 2.540f, 2.530f, 2.510f,
85  2.490f, 2.490f, 2.480f, 2.470f, 2.460f, 2.460f, 2.460f, 2.450f,
86  2.450f, 2.450f, 2.460f, 2.460f, 2.470f, 2.480f, 2.490f, 2.490f,
87  2.520f, 2.510f, 2.550f, 2.570f, 2.610f, 2.640f, 2.980f, 3.040f,
88  3.010f, 2.980f, 2.940f, 2.920f, 2.890f, 2.870f, 2.830f, 2.810f,
89  2.780f, 2.760f, 2.740f, 2.720f, 2.690f, 2.670f, 2.650f, 2.630f,
90  2.620f, 2.610f, 2.590f, 2.560f, 2.550f, 2.530f, 2.510f, 2.500f,
91  2.480f, 2.460f, 2.450f, 2.430f, 2.420f, 2.400f, 2.390f, 2.380f,
92  2.360f, 2.350f, 2.340f, 2.330f, 2.310f, 2.300f, 2.290f, 2.280f,
93  2.270f, 2.260f, 2.250f, 2.240f, 2.230f, 2.230f, 2.220f, 2.210f,
94  2.200f, 2.190f, 2.180f, 2.170f, 1.320f, 1.140f, 1.130f, 1.130f,
95  1.120f, 1.120f, 1.110f, 1.110f, 1.110f, 1.110f, 1.100f, 1.110f,
96  1.100f};
97 char SCAN_VALID_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, 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, 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, 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, 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, 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, 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, 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, 1, 1,
106  1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 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, 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, 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, 1, 1,
110  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
111  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
112  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
113 
114 float SCAN_RANGES_2[] = {
115  0.720f, 0.720f, 0.720f, 0.720f, 0.720f, 0.720f, 0.710f, 0.720f,
116  0.710f, 0.710f, 0.710f, 0.710f, 0.710f, 0.710f, 0.710f, 0.710f,
117  0.710f, 0.710f, 0.710f, 0.710f, 0.720f, 0.720f, 0.720f, 0.720f,
118  0.730f, 0.730f, 0.730f, 0.730f, 0.730f, 0.730f, 0.730f, 0.740f,
119  0.740f, 0.740f, 0.740f, 0.740f, 0.740f, 0.750f, 0.750f, 0.750f,
120  0.750f, 0.750f, 0.750f, 0.750f, 0.750f, 0.760f, 0.760f, 0.760f,
121  0.760f, 0.760f, 0.760f, 0.760f, 0.760f, 0.770f, 0.770f, 0.770f,
122  0.770f, 0.780f, 0.780f, 0.780f, 0.790f, 0.790f, 0.800f, 0.800f,
123  0.800f, 0.800f, 0.800f, 0.800f, 0.810f, 0.810f, 0.820f, 0.820f,
124  0.830f, 0.830f, 0.840f, 0.840f, 0.850f, 0.850f, 0.860f, 0.860f,
125  0.860f, 0.870f, 0.870f, 0.880f, 0.890f, 0.890f, 0.900f, 0.900f,
126  0.910f, 0.920f, 0.930f, 0.930f, 0.940f, 0.940f, 0.940f, 0.950f,
127  0.960f, 0.960f, 0.970f, 0.980f, 0.990f, 1.000f, 1.010f, 1.020f,
128  1.030f, 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, 1.180f,
130  1.190f, 1.210f, 1.220f, 1.240f, 1.250f, 1.280f, 1.290f, 1.300f,
131  1.320f, 1.340f, 1.350f, 1.380f, 1.390f, 1.420f, 1.440f, 1.460f,
132  1.470f, 1.500f, 1.520f, 1.550f, 1.570f, 1.600f, 1.630f, 1.670f,
133  1.690f, 1.730f, 1.760f, 1.790f, 1.820f, 1.870f, 1.900f, 1.940f,
134  1.970f, 2.030f, 2.080f, 2.130f, 2.170f, 2.230f, 2.280f, 2.340f,
135  2.400f, 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, 4.180f,
137  4.340f, 4.620f, 8.170f, 8.140f, 8.150f, 8.120f, 8.110f, 8.100f,
138  8.100f, 8.300f, 9.040f, 9.130f, 9.130f, 13.030f, 18.050f, 19.150f,
139  81.910f, 20.070f, 47.980f, 48.040f, 48.030f, 19.140f, 19.180f, 19.180f,
140  19.190f, 14.550f, 19.210f, 16.850f, 16.840f, 7.800f, 7.770f, 7.770f,
141  7.750f, 7.770f, 7.760f, 7.780f, 7.760f, 8.320f, 8.350f, 8.350f,
142  8.090f, 7.720f, 7.730f, 6.430f, 6.360f, 6.290f, 6.260f, 6.230f,
143  6.220f, 6.160f, 5.800f, 4.510f, 4.410f, 4.240f, 4.140f, 4.000f,
144  3.910f, 3.790f, 3.680f, 3.660f, 3.680f, 3.680f, 3.700f, 3.710f,
145  3.730f, 3.730f, 3.760f, 3.770f, 3.790f, 3.820f, 3.850f, 3.900f,
146  3.940f, 3.980f, 3.250f, 3.180f, 3.140f, 3.070f, 3.030f, 2.970f,
147  2.930f, 2.880f, 2.850f, 2.790f, 2.760f, 2.710f, 2.680f, 2.660f,
148  2.670f, 2.690f, 2.710f, 2.720f, 2.740f, 2.760f, 2.770f, 2.780f,
149  2.800f, 2.170f, 2.120f, 2.090f, 2.060f, 2.020f, 2.010f, 1.990f,
150  1.980f, 1.970f, 1.960f, 1.950f, 1.950f, 1.940f, 1.940f, 1.950f,
151  1.940f, 1.940f, 1.950f, 1.930f, 1.940f, 1.940f, 1.940f, 1.940f,
152  1.940f, 1.950f, 1.960f, 1.960f, 1.980f, 1.980f, 2.000f, 2.010f,
153  2.030f, 2.060f, 2.090f, 2.120f, 2.190f, 2.560f, 2.540f, 2.530f,
154  2.520f, 2.500f, 2.490f, 2.470f, 2.460f, 2.450f, 2.440f, 2.420f,
155  2.410f, 2.400f, 2.390f, 2.380f, 2.370f, 2.360f, 2.350f, 2.340f,
156  2.340f, 2.330f, 2.320f, 2.310f, 2.300f, 2.290f, 2.290f, 2.290f,
157  2.280f, 2.270f, 2.260f, 2.260f, 2.250f, 2.240f, 2.240f, 2.230f,
158  2.230f, 2.220f, 2.220f, 2.210f, 2.210f, 2.200f, 2.200f, 2.190f,
159  2.190f, 2.190f, 2.180f, 2.180f, 2.170f, 2.170f, 2.170f, 2.160f,
160  2.160f};
161 char SCAN_VALID_2[] = {
162  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
163  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
164  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
165  1, 1, 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, 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, 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, 1, 1,
169  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 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, 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, 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, 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, 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, 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, 1, 1,
176  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
177 
178 #endif
179 
180 // ------------------------------------------------------
181 // TestICP
182 // ------------------------------------------------------
183 void TestICP()
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  ASSERT_(sizeof(SCAN_RANGES_1) == sizeof(float) * SCANS_SIZE);
195 
196  /**
197  * Copy the float scan laser arrays to the corresponding members of the
198  * CObservation2DRangeScan instances
199  */
200  scan1.loadFromVectors(SCANS_SIZE, SCAN_RANGES_1, SCAN_VALID_1);
201  CObservation2DRangeScan scan2 = scan1;
202  scan2.loadFromVectors(SCANS_SIZE, SCAN_RANGES_2, SCAN_VALID_2);
203 
204  // Build the points maps from the scans:
205  m1.insertObservation(&scan1);
206  m2.insertObservation(&scan2);
207 
208 #if MRPT_HAS_PCL
209  cout << "Saving map1.pcd and map2.pcd in PCL format...\n";
210  m1.savePCDFile("map1.pcd", false);
211  m2.savePCDFile("map2.pcd", false);
212 #endif
213 
214  // -----------------------------------------------------
215 
216  /**
217  * user-set parameters for the icp algorithm.
218  * For a full list of the available options check the online tutorials
219  * page:
220  * http://www.mrpt.org/Iterative_Closest_Point_(ICP)_and_other_matching_algorithms
221  */
222 
223  // select which algorithm version to use
224  // ICP.options.ICP_algorithm = icpLevenbergMarquardt;
225  // ICP.options.ICP_algorithm = icpClassic;
226  ICP.options.ICP_algorithm = (TICPAlgorithm)ICP_method;
227 
228  // configuration options for the icp algorithm
229  ICP.options.maxIterations = 100;
230  ICP.options.thresholdAng = DEG2RAD(10.0f);
231  ICP.options.thresholdDist = 0.75f;
232  ICP.options.ALFA = 0.5f;
233  ICP.options.smallestThresholdDist = 0.05f;
234  ICP.options.doRANSAC = false;
235 
236  ICP.options.dumpToConsole();
237  // -----------------------------------------------------
238 
239  /**
240  * Scans alignment procedure.
241  * Given an initial guess (initialPose) and the maps to be aligned, the
242  * algorithm returns the probability density function (pdf) of the alignment
243  * Additional arguments are provided to investigate the performance of the
244  * algorithm
245  */
246  CPose2D initialPose(0.8f, 0.0f, (float)DEG2RAD(0.0f));
247 
248  CPosePDF::Ptr pdf =
249  ICP.Align(&m1, &m2, initialPose, &runningTime, (void*)&info);
250 
251  printf(
252  "ICP run in %.02fms, %d iterations (%.02fms/iter), %.01f%% goodness\n "
253  "-> ",
254  runningTime * 1000, info.nIterations,
255  runningTime * 1000.0f / info.nIterations, info.goodness * 100);
256 
257  cout << "Mean of estimation: " << pdf->getMeanVal() << endl << endl;
258 
259  CPosePDFGaussian gPdf;
260  gPdf.copyFrom(*pdf);
261 
262  cout << "Covariance of estimation: " << endl << gPdf.cov << endl;
263 
264  cout << " std(x): " << sqrt(gPdf.cov(0, 0)) << endl;
265  cout << " std(y): " << sqrt(gPdf.cov(1, 1)) << endl;
266  cout << " std(phi): " << RAD2DEG(sqrt(gPdf.cov(2, 2))) << " (deg)" << endl;
267 
268  // cout << "Covariance of estimation (MATLAB format): " << endl <<
269  // gPdf.cov.inMatlabFormat() << endl;
270 
271  /**
272  * Save the results for potential postprocessing (in txt and in matlab
273  * format)
274  */
275  cout << "-> Saving reference map as scan1.txt" << endl;
276  m1.save2D_to_text_file("scan1.txt");
277 
278  cout << "-> Saving map to align as scan2.txt" << endl;
279  m2.save2D_to_text_file("scan2.txt");
280 
281  cout << "-> Saving transformed map to align as scan2_trans.txt" << endl;
282  CSimplePointsMap m2_trans = m2;
283  m2_trans.changeCoordinatesReference(gPdf.mean);
284  m2_trans.save2D_to_text_file("scan2_trans.txt");
285 
286  cout << "-> Saving MATLAB script for drawing 2D ellipsoid as view_ellip.m"
287  << endl;
288  CMatrixFloat COV22 = CMatrixFloat(CMatrixDouble(gPdf.cov));
289  COV22.setSize(2, 2);
290  CVectorFloat MEAN2D(2);
291  MEAN2D[0] = gPdf.mean.x();
292  MEAN2D[1] = gPdf.mean.y();
293  {
294  ofstream f("view_ellip.m");
295  f << math::MATLAB_plotCovariance2D(COV22, MEAN2D, 3.0f);
296  }
297 
298 // If we have 2D windows, use'em:
299 #if MRPT_HAS_WXWIDGETS
300  /**
301  * Plotting the icp results:
302  * Aligned maps + transformation uncertainty ellipsis
303  */
304  if (!skip_window)
305  {
306  gui::CDisplayWindowPlots win("ICP results");
307 
308  // Reference map:
309  vector<float> map1_xs, map1_ys, map1_zs;
310  m1.getAllPoints(map1_xs, map1_ys, map1_zs);
311  win.plot(map1_xs, map1_ys, "b.3", "map1");
312 
313  // Translated map:
314  vector<float> map2_xs, map2_ys, map2_zs;
315  m2_trans.getAllPoints(map2_xs, map2_ys, map2_zs);
316  win.plot(map2_xs, map2_ys, "r.3", "map2");
317 
318  // Uncertainty
319  win.plotEllipse(MEAN2D[0], MEAN2D[1], COV22, 3.0, "b2", "cov");
320 
321  win.axis(-1, 10, -6, 6);
322  win.axis_equal();
323 
324  cout << "Close the window to exit" << endl;
325  win.waitForKey();
326  }
327 #endif
328 }
329 
330 int main(int argc, char** argv)
331 {
332  try
333  {
334  skip_window = (argc > 2);
335  if (argc > 1)
336  {
337  ICP_method = atoi(argv[1]);
338  }
339 
340  TestICP();
341 
342  return 0;
343  }
344  catch (exception& e)
345  {
346  cout << "MRPT exception caught: " << e.what() << endl;
347  return -1;
348  }
349  catch (...)
350  {
351  printf("Another exception!!");
352  return -1;
353  }
354 }
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
mrpt::poses::CPosePDFGaussian::copyFrom
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
Definition: CPosePDFGaussian.cpp:98
CSimplePointsMap.h
mrpt::maps::CPointsMap::save2D_to_text_file
bool save2D_to_text_file(const std::string &file) const
Save to a text file.
Definition: CPointsMap.cpp:83
mrpt::math::MATLAB_plotCovariance2D
std::string MATLAB_plotCovariance2D(const CMatrixFloat &cov22, const CVectorFloat &mean, const float &stdCount, const std::string &style=std::string("b"), const size_t &nEllipsePoints=30)
Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2...
Definition: math.cpp:354
mrpt::poses::CPosePDFGaussian::mean
CPose2D mean
The mean value.
Definition: CPosePDFGaussian.h:44
mrpt::RAD2DEG
double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:48
mrpt::maps::CPointsMap::changeCoordinatesReference
void changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
Definition: CPointsMap.cpp:574
mrpt::gui::CDisplayWindowPlots
Create a GUI window and display plots with MATLAB-like interfaces and commands.
Definition: CDisplayWindowPlots.h:29
mrpt::slam::CICP::TConfigParams::ICP_algorithm
TICPAlgorithm ICP_algorithm
The algorithm to use (default: icpClassic).
Definition: CICP.h:84
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
mrpt::slam::CICP
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:65
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
CICP.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
std
STL namespace.
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:120
mrpt::slam::CICP::options
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:180
mrpt::poses::CPosePDFGaussian::cov
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Definition: CPosePDFGaussian.h:46
SCANS_SIZE
#define SCANS_SIZE
SCAN_RANGES_2
float SCAN_RANGES_2[]
Definition: vision_stereo_rectify/test.cpp:114
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::poses::CPosePDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFGaussian.h:29
std::shared_ptr
Definition: TTypeName.h:18
mrpt::maps::CPointsMap::getAllPoints
void getAllPoints(VECTOR &xs, VECTOR &ys, VECTOR &zs, size_t decimation=1) const
Returns a copy of the 2D/3D points as a std::vector of float coordinates.
Definition: CPointsMap.h:536
mrpt::math::CMatrixDouble
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
Definition: CMatrixTemplateNumeric.h:144
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
mrpt::slam::CICP::TConfigParams::maxIterations
unsigned int maxIterations
Maximum number of iterations to run.
Definition: CICP.h:101
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
M_PIf
#define M_PIf
Definition: common.h:61
mrpt::math::CMatrixFloat
CMatrixTemplateNumeric< float > CMatrixFloat
Declares a matrix of float numbers (non serializable).
Definition: CMatrixTemplateNumeric.h:138
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:133
mrpt::maps::CPointsMap::savePCDFile
virtual bool savePCDFile(const std::string &filename, bool save_as_binary) const
Save the point cloud as a PCL PCD file, in either ASCII or binary format (requires MRPT built against...
Definition: CPointsMap.cpp:1710
win
mrpt::gui::CDisplayWindow3D::Ptr win
Definition: vision_stereo_rectify/test.cpp:31
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
mrpt::slam::TICPAlgorithm
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options mrpt_slam_grp.
Definition: CICP.h:20
ICP_method
int ICP_method
Definition: vision_stereo_rectify/test.cpp:37
CObservation2DRangeScan.h
SCAN_RANGES_1
float SCAN_RANGES_1[]
Definition: vision_stereo_rectify/test.cpp:36
CPosePDFGaussian.h
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:56
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
mrpt::slam::CICP::TConfigParams::ALFA
double ALFA
The scale factor for thresholds everytime convergence is achieved.
Definition: CICP.h:117
mrpt::slam::CICP::TReturnInfo
The ICP algorithm return information.
Definition: CICP.h:190
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
mrpt::obs::CObservation2DRangeScan::loadFromVectors
void loadFromVectors(size_t nRays, const float *scanRanges, const char *scanValidity)
Definition: CObservation2DRangeScan.cpp:552
TestICP
void TestICP()
Definition: vision_stereo_rectify/test.cpp:183
CPosePDF.h
SCAN_VALID_1
char SCAN_VALID_1[]
Definition: vision_stereo_rectify/test.cpp:83
skip_window
bool skip_window
Definition: vision_stereo_rectify/test.cpp:36
SCAN_VALID_2
char SCAN_VALID_2[]
Definition: vision_stereo_rectify/test.cpp:161
mrpt::slam::CICP::TConfigParams::thresholdDist
double thresholdDist
Initial threshold distance for two points to become a correspondence.
Definition: CICP.h:114
CPose2D.h
mrpt::config::CLoadableOptions::dumpToConsole
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
Definition: CLoadableOptions.cpp:44
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::obs::CObservation2DRangeScan::rightToLeft
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
Definition: CObservation2DRangeScan.h:127


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