icp.h

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 

Last updated 12 September 2005 21:38:45