00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2009-2012, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 00041 #ifndef PCL_APPS_IN_HAND_SCANNER_ICP_H 00042 #define PCL_APPS_IN_HAND_SCANNER_ICP_H 00043 00044 #include <pcl/pcl_exports.h> 00045 #include <pcl/apps/in_hand_scanner/boost.h> 00046 #include <pcl/apps/in_hand_scanner/eigen.h> 00047 #include <pcl/apps/in_hand_scanner/common_types.h> 00048 00050 // Forward declarations 00052 00053 namespace pcl 00054 { 00055 template <typename PointT> 00056 class KdTree; 00057 } // End namespace pcl 00058 00060 // ICP 00062 00063 namespace pcl 00064 { 00065 namespace ihs 00066 { 00071 class PCL_EXPORTS ICP 00072 { 00073 public: 00074 00075 typedef pcl::PointXYZRGBNormal PointXYZRGBNormal; 00076 typedef pcl::PointCloud <PointXYZRGBNormal> CloudXYZRGBNormal; 00077 typedef CloudXYZRGBNormal::Ptr CloudXYZRGBNormalPtr; 00078 typedef CloudXYZRGBNormal::ConstPtr CloudXYZRGBNormalConstPtr; 00079 00080 typedef pcl::ihs::Mesh Mesh; 00081 typedef pcl::ihs::MeshPtr MeshPtr; 00082 typedef pcl::ihs::MeshConstPtr MeshConstPtr; 00083 00085 ICP (); 00086 00091 void 00092 setEpsilon (const float epsilon); 00093 00094 float 00095 getEpsilon () const; 00102 void 00103 setMaxIterations (const unsigned int max_iter); 00104 00105 unsigned int 00106 getMaxIterations () const; 00113 void 00114 setMinOverlap (const float overlap); 00115 00116 float 00117 getMinOverlap () const; 00124 void 00125 setMaxFitness (const float fitness); 00126 00127 float 00128 getMaxFitness () const; 00135 void 00136 setCorrespondenceRejectionFactor (const float factor); 00137 00138 float 00139 getCorrespondenceRejectionFactor () const; 00146 void 00147 setMaxAngle (const float angle); 00148 00149 float 00150 getMaxAngle () const; 00160 bool 00161 findTransformation (const MeshConstPtr& mesh_model, 00162 const CloudXYZRGBNormalConstPtr& cloud_data, 00163 const Eigen::Matrix4f& T_init, 00164 Eigen::Matrix4f& T_final); 00165 00166 private: 00167 00168 typedef pcl::PointNormal PointNormal; 00169 typedef pcl::PointCloud <PointNormal> CloudNormal; 00170 typedef CloudNormal::Ptr CloudNormalPtr; 00171 typedef CloudNormal::ConstPtr CloudNormalConstPtr; 00172 00173 typedef pcl::KdTree <PointNormal> KdTree; 00174 typedef boost::shared_ptr <KdTree> KdTreePtr; 00175 typedef boost::shared_ptr <const KdTree> KdTreeConstPtr; 00176 00182 CloudNormalConstPtr 00183 selectModelPoints (const MeshConstPtr& mesh_model, 00184 const Eigen::Matrix4f& T_inv) const; 00185 00190 CloudNormalConstPtr 00191 selectDataPoints (const CloudXYZRGBNormalConstPtr& cloud_data) const; 00192 00199 bool 00200 minimizePointPlane (const CloudNormal& cloud_source, 00201 const CloudNormal& cloud_target, 00202 Eigen::Matrix4f& T) const; 00203 00205 // Members 00207 00208 KdTreePtr kd_tree_; 00209 00210 // Convergence 00211 float epsilon_; // in cm^2 00212 00213 // Registration failure 00214 unsigned int max_iterations_; 00215 float min_overlap_; // [0 1] 00216 float max_fitness_; // in cm^2 00217 00218 // Correspondence rejection 00219 float factor_; 00220 float max_angle_; // in degrees 00221 }; 00222 } // End namespace ihs 00223 } // End namespace pcl 00224 00225 #endif // PCL_APPS_IN_HAND_SCANNER_ICP_H