icp.h
00001 /* 00002 Copyright (c) 2004, Tim Bailey 00003 All rights reserved. 00004 00005 Redistribution and use in source and binary forms, with or without 00006 modification, are permitted provided that the following conditions are met: 00007 00008 * Redistributions of source code must retain the above copyright notice, 00009 this list of conditions and the following disclaimer. 00010 * Redistributions in binary form must reproduce the above copyright notice, 00011 this list of conditions and the following disclaimer in the documentation 00012 and/or other materials provided with the distribution. 00013 * Neither the name of the Player Project nor the names of its contributors 00014 may be used to endorse or promote products derived from this software 00015 without specific prior written permission. 00016 00017 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 00021 ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 00024 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00027 */ 00028 00029 /* Iterated closest point (ICP) algorithm. 00030 * 00031 * This is a very simple implementation of ICP, with one simple extension where the 00032 * association may be chosen as an interpolation between two nearest neighbours rather 00033 * than just point-to-point with the nearest neighbour. 00034 * 00035 * A lot of extensions to this basic algorithm are possible. For example, 00036 * 1. Record at each iteration the set of NN associations for each observation. 00037 * If these do not change, then ICP has converged completely. 00038 * 00039 * 2. Various speed ups given in the following papers: 00040 * 00041 * (a) P.J. Besl and N.D. McKay. A method for registration of 3-D shapes. IEEE 00042 * Transactions on Pattern Analysis and Machine Intelligence, 14(2):239256, 1992. 00043 * 00044 * (b) F. Lu and E. Milios. Robot pose estimation in unknown environments by matching 00045 * 2D range scans. Journal of Intelligent and Robotic Systems, 18:249275, 1997. 00046 * 00047 * (c) S. Rusinkiewicz and M. Levoy. Efficient variants of the ICP algorithm. In Third 00048 * International Conference on 3D Digital Imaging and Modeling, pages 145152, 2001. 00049 * 00050 * 3. Methods for estimating the error introduced by point-wise correspondence in 00051 * the paper (b) above and also: 00052 * 00053 * S.T. P?ster, K.L. Kriechbaum, S.I. Roumeliotis, and J.W. Burdick. Weighted range 00054 * sensor matching algorithms for mobile robot displacement estimation. In IEEE 00055 * International Conference on Robotics and Automation, 2002. 00056 * 00057 * Tim Bailey 2004. 00058 */ 00059 #include <vector> 00060 #include "geometry2D.h" 00061 #include "nn.h" 00062 #include <memory> 00063 using namespace std; 00064 namespace Geom2D { 00065 00066 class ICP { 00067 public: 00068 ICP(); 00069 ~ICP(); 00070 Pose align(std::vector<Point> , std::vector<Point>,Pose , double , int , bool ); 00071 const std::vector<Point> get_ref_points() { return b; } 00072 const std::vector<Point> get_obs_points() { return a; } 00073 bool warning_misalign; 00074 private: 00075 vector<Point> ref; 00076 SweepSearch * nn; 00077 vector<Point> a; 00078 vector<Point> b; 00079 vector<int> index; 00080 }; 00081 00082 } // namespace Geom2D