Example: slam_icp_simple_example
C++ example source code:
/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include <mrpt/gui/CDisplayWindowPlots.h> #include <mrpt/maps/CSimplePointsMap.h> #include <mrpt/math/utils.h> #include <mrpt/obs/CObservation2DRangeScan.h> #include <mrpt/obs/stock_observations.h> #include <mrpt/poses/CPose2D.h> #include <mrpt/poses/CPosePDF.h> #include <mrpt/poses/CPosePDFGaussian.h> #include <mrpt/slam/CICP.h> #include <fstream> #include <iostream> using namespace mrpt; using namespace mrpt::slam; using namespace mrpt::maps; using namespace mrpt::obs; using namespace mrpt::math; using namespace mrpt::poses; using namespace std; bool skip_window = false; int ICP_method = (int)icpClassic; // ------------------------------------------------------ // TestICP // ------------------------------------------------------ void TestICP() { CSimplePointsMap m1, m2; CICP::TReturnInfo info; CICP ICP; // Load scans: CObservation2DRangeScan scan1; stock_observations::example2DRangeScan(scan1, 0); CObservation2DRangeScan scan2; stock_observations::example2DRangeScan(scan2, 1); // Build the points maps from the scans: m1.insertObservation(scan1); m2.insertObservation(scan2); // ----------------------------------------------------- // select which algorithm version to use // ICP.options.ICP_algorithm = icpLevenbergMarquardt; // ICP.options.ICP_algorithm = icpClassic; ICP.options.ICP_algorithm = (TICPAlgorithm)ICP_method; // configuration options for the icp algorithm ICP.options.maxIterations = 100; ICP.options.thresholdAng = DEG2RAD(10.0f); ICP.options.thresholdDist = 0.75f; ICP.options.ALFA = 0.5f; ICP.options.smallestThresholdDist = 0.05f; ICP.options.doRANSAC = false; ICP.options.dumpToConsole(); // ----------------------------------------------------- CPose2D initialPose(0.8f, 0.0f, (float)DEG2RAD(0.0f)); CPosePDF::Ptr pdf = ICP.Align(&m1, &m2, initialPose, info); printf( "ICP run in %.02fms, %d iterations (%.02fms/iter), %.01f%% goodness\n " "-> ", info.executionTime * 1000, info.nIterations, info.executionTime * 1000.0f / info.nIterations, info.goodness * 100); cout << "Mean of estimation: " << pdf->getMeanVal() << endl << endl; CPosePDFGaussian gPdf; gPdf.copyFrom(*pdf); cout << "Covariance of estimation: " << endl << gPdf.cov << endl; cout << " std(x): " << sqrt(gPdf.cov(0, 0)) << endl; cout << " std(y): " << sqrt(gPdf.cov(1, 1)) << endl; cout << " std(phi): " << RAD2DEG(sqrt(gPdf.cov(2, 2))) << " (deg)" << endl; // cout << "Covariance of estimation (MATLAB format): " << endl << // gPdf.cov.inMatlabFormat() << endl; cout << "-> Saving reference map as scan1.txt" << endl; m1.save2D_to_text_file("scan1.txt"); cout << "-> Saving map to align as scan2.txt" << endl; m2.save2D_to_text_file("scan2.txt"); cout << "-> Saving transformed map to align as scan2_trans.txt" << endl; CSimplePointsMap m2_trans = m2; m2_trans.changeCoordinatesReference(gPdf.mean); m2_trans.save2D_to_text_file("scan2_trans.txt"); cout << "-> Saving MATLAB script for drawing 2D ellipsoid as view_ellip.m" << endl; CMatrixFloat COV22 = CMatrixFloat(CMatrixDouble(gPdf.cov)); COV22.setSize(2, 2); CVectorFloat MEAN2D(2); MEAN2D[0] = gPdf.mean.x(); MEAN2D[1] = gPdf.mean.y(); { ofstream f("view_ellip.m"); f << math::MATLAB_plotCovariance2D(COV22, MEAN2D, 3.0f); } // If we have 2D windows, use'em: #if MRPT_HAS_WXWIDGETS if (!skip_window) { gui::CDisplayWindowPlots win("ICP results"); // Reference map: vector<float> map1_xs, map1_ys, map1_zs; m1.getAllPoints(map1_xs, map1_ys, map1_zs); win.plot(map1_xs, map1_ys, "b.3", "map1"); // Translated map: vector<float> map2_xs, map2_ys, map2_zs; m2_trans.getAllPoints(map2_xs, map2_ys, map2_zs); win.plot(map2_xs, map2_ys, "r.3", "map2"); // Uncertainty win.plotEllipse(MEAN2D[0], MEAN2D[1], COV22, 3.0, "b2", "cov"); win.axis(-1, 10, -6, 6); win.axis_equal(); cout << "Close the window to exit" << endl; win.waitForKey(); } #endif } int main(int argc, char** argv) { try { skip_window = (argc > 2); if (argc > 1) { ICP_method = atoi(argv[1]); } TestICP(); return 0; } catch (exception& e) { cout << "MRPT exception caught: " << e.what() << endl; return -1; } catch (...) { printf("Another exception!!"); return -1; } }