/* +---------------------------------------------------------------------------+ | The Mobile Robot Programming Toolkit (MRPT) C++ library | | | | http://mrpt.sourceforge.net/ | | | | Copyright (C) 2005-2011 University of Malaga | | | | This software was written by the Machine Perception and Intelligent | | Robotics Lab, University of Malaga (Spain). | | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> | | | | This file is part of the MRPT project. | | | | MRPT is free software: you can redistribute it and/or modify | | it under the terms of the GNU General Public License as published by | | the Free Software Foundation, either version 3 of the License, or | | (at your option) any later version. | | | | MRPT is distributed in the hope that it will be useful, | | but WITHOUT ANY WARRANTY; without even the implied warranty of | | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | | GNU General Public License for more details. | | | | You should have received a copy of the GNU General Public License | | along with MRPT. If not, see <http://www.gnu.org/licenses/>. | | | +---------------------------------------------------------------------------+ */ #include <mrpt/mrpt.h> using namespace UTILS; using namespace MRML; using namespace std; // ------------------------------------------------------ // TestMatchingCovariance // ------------------------------------------------------ void TestMatchingCovariance() { CTicTac tictac; CSimplePointsMap m1,m2,m3; CPoint3D p; CMatrix cov,Z,M_psdInv,dataScans,dataOdo, aux; float ecm,corrsRatio; CMetricMap::TMatchingPairList corrs; m1.load2D_from_text_file("map1.txt"); CFileStream("map1.bin",fomWrite) << m1; m1.load2D_from_text_file("map2.txt"); CFileStream("map2.bin",fomWrite) << m1; m1.load2D_from_text_file("map3.txt"); CFileStream("map3.bin",fomWrite) << m1; m1.load2D_from_text_file("map4.txt"); CFileStream("map4.bin",fomWrite) << m1; m1.load2D_from_text_file("map5.txt"); CFileStream("map5.bin",fomWrite) << m1; // Load the point maps to align CFileStream("map1.bin",fomRead) >> m1; CFileStream("map4.bin",fomRead) >> m2; tictac.Tic(); for (int j=0;j<100;j++) { CPose2D otherMapPose(0,0,0); CPose2D dumm(0,0,0); m1.computeMatchingWith2D( &m2, otherMapPose, 0.25f /*Max dist. for corr.*/, DEG2RAD(5.0f), dumm, corrs, corrsRatio, &ecm, &cov, true); } printf("In %fms:\nCorrs ratio=%f\necm=%f\n",tictac.Tac()*10,corrsRatio,sqrt(ecm/ (corrs.size() ? corrs.size():1) )); } int main() { try { TestMatchingCovariance(); return 0; } catch (exception &e) { cout << "MRPT exception catched: " << e.what() << endl; return -1; } catch (...) { printf("Another exception!!"); return -1; } }