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


Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019