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 

Last updated 12 September 2005 21:38:45