00001 /* Iterated closest point (ICP) algorithm. 00002 * 00003 * This is a very simple implementation of ICP, with one simple extension where the 00004 * association may be chosen as an interpolation between two nearest neighbours rather 00005 * than just point-to-point with the nearest neighbour. 00006 * 00007 * A lot of extensions to this basic algorithm are possible. For example, 00008 * 1. Record at each iteration the set of NN associations for each observation. 00009 * If these do not change, then ICP has converged completely. 00010 * 00011 * 2. Various speed ups given in the following papers: 00012 * 00013 * (a) P.J. Besl and N.D. McKay. A method for registration of 3-D shapes. IEEE 00014 * Transactions on Pattern Analysis and Machine Intelligence, 14(2):239256, 1992. 00015 * 00016 * (b) F. Lu and E. Milios. Robot pose estimation in unknown environments by matching 00017 * 2D range scans. Journal of Intelligent and Robotic Systems, 18:249275, 1997. 00018 * 00019 * (c) S. Rusinkiewicz and M. Levoy. Efficient variants of the ICP algorithm. In Third 00020 * International Conference on 3D Digital Imaging and Modeling, pages 145152, 2001. 00021 * 00022 * 3. Methods for estimating the error introduced by point-wise correspondence in 00023 * the paper (b) above and also: 00024 * 00025 * S.T. P?ster, K.L. Kriechbaum, S.I. Roumeliotis, and J.W. Burdick. Weighted range 00026 * sensor matching algorithms for mobile robot displacement estimation. In IEEE 00027 * International Conference on Robotics and Automation, 2002. 00028 * 00029 * Tim Bailey 2004. 00030 */ 00031 #include <vector> 00032 #include "geometry2D.h" 00033 #include "nn.h" 00034 #include <memory> 00035 using namespace std; 00036 namespace Geom2D { 00037 00038 class ICP { 00039 public: 00040 ICP(); 00041 ~ICP(); 00042 Pose align(std::vector<Point> , std::vector<Point>,Pose , double , int , bool ); 00043 const std::vector<Point> get_ref_points() { return b; } 00044 const std::vector<Point> get_obs_points() { return a; } 00045 private: 00046 vector<Point> ref; 00047 SweepSearch * nn; 00048 vector<Point> a; 00049 vector<Point> b; 00050 vector<int> index; 00051 }; 00052 00053 } // namespace Geom2D