Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2011, 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_NDT_2D_H_ 00042 #define PCL_NDT_2D_H_ 00043 00044 #include <pcl/registration/registration.h> 00045 00046 namespace pcl 00047 { 00048 /** \brief @b NormalDistributionsTransform2D provides an implementation of the 00049 * Normal Distributions Transform algorithm for scan matching. 00050 * 00051 * This implementation is intended to match the definition: 00052 * Peter Biber and Wolfgang Straßer. The normal distributions transform: A 00053 * new approach to laser scan matching. In Proceedings of the IEEE In- 00054 * ternational Conference on Intelligent Robots and Systems (IROS), pages 00055 * 2743–2748, Las Vegas, USA, October 2003. 00056 * 00057 * \author James Crosby 00058 */ 00059 template <typename PointSource, typename PointTarget> 00060 class NormalDistributionsTransform2D : public Registration<PointSource, PointTarget> 00061 { 00062 typedef typename Registration<PointSource, PointTarget>::PointCloudSource PointCloudSource; 00063 typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 00064 typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 00065 00066 typedef typename Registration<PointSource, PointTarget>::PointCloudTarget PointCloudTarget; 00067 00068 typedef PointIndices::Ptr PointIndicesPtr; 00069 typedef PointIndices::ConstPtr PointIndicesConstPtr; 00070 00071 public: 00072 00073 typedef boost::shared_ptr< NormalDistributionsTransform2D<PointSource, PointTarget> > Ptr; 00074 typedef boost::shared_ptr< const NormalDistributionsTransform2D<PointSource, PointTarget> > ConstPtr; 00075 00076 /** \brief Empty constructor. */ 00077 NormalDistributionsTransform2D () 00078 : Registration<PointSource,PointTarget> (), 00079 grid_centre_ (0,0), grid_step_ (1,1), grid_extent_ (20,20), newton_lambda_ (1,1,1) 00080 { 00081 reg_name_ = "NormalDistributionsTransform2D"; 00082 } 00083 00084 /** \brief Empty destructor */ 00085 virtual ~NormalDistributionsTransform2D () {} 00086 00087 /** \brief centre of the ndt grid (target coordinate system) 00088 * \param centre value to set 00089 */ 00090 virtual void 00091 setGridCentre (const Eigen::Vector2f& centre) { grid_centre_ = centre; } 00092 00093 /** \brief Grid spacing (step) of the NDT grid 00094 * \param[in] step value to set 00095 */ 00096 virtual void 00097 setGridStep (const Eigen::Vector2f& step) { grid_step_ = step; } 00098 00099 /** \brief NDT Grid extent (in either direction from the grid centre) 00100 * \param[in] extent value to set 00101 */ 00102 virtual void 00103 setGridExtent (const Eigen::Vector2f& extent) { grid_extent_ = extent; } 00104 00105 /** \brief NDT Newton optimisation step size parameter 00106 * \param[in] lambda step size: 1 is simple newton optimisation, smaller values may improve convergence 00107 */ 00108 virtual void 00109 setOptimizationStepSize (const double& lambda) { newton_lambda_ = Eigen::Vector3d (lambda, lambda, lambda); } 00110 00111 /** \brief NDT Newton optimisation step size parameter 00112 * \param[in] lambda step size: (1,1,1) is simple newton optimisation, 00113 * smaller values may improve convergence, or elements may be set to 00114 * zero to prevent optimisation over some parameters 00115 * 00116 * This overload allows control of updates to the individual (x, y, 00117 * theta) free parameters in the optimisation. If, for example, theta is 00118 * believed to be close to the correct value a small value of lambda[2] 00119 * should be used. 00120 */ 00121 virtual void 00122 setOptimizationStepSize (const Eigen::Vector3d& lambda) { newton_lambda_ = lambda; } 00123 00124 protected: 00125 /** \brief Rigid transformation computation method with initial guess. 00126 * \param[out] output the transformed input point cloud dataset using the rigid transformation found 00127 * \param[in] guess the initial guess of the transformation to compute 00128 */ 00129 virtual void 00130 computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess); 00131 00132 using Registration<PointSource, PointTarget>::reg_name_; 00133 using Registration<PointSource, PointTarget>::target_; 00134 using Registration<PointSource, PointTarget>::converged_; 00135 using Registration<PointSource, PointTarget>::nr_iterations_; 00136 using Registration<PointSource, PointTarget>::max_iterations_; 00137 using Registration<PointSource, PointTarget>::transformation_epsilon_; 00138 using Registration<PointSource, PointTarget>::transformation_; 00139 using Registration<PointSource, PointTarget>::previous_transformation_; 00140 using Registration<PointSource, PointTarget>::final_transformation_; 00141 using Registration<PointSource, PointTarget>::update_visualizer_; 00142 using Registration<PointSource, PointTarget>::indices_; 00143 00144 Eigen::Vector2f grid_centre_; 00145 Eigen::Vector2f grid_step_; 00146 Eigen::Vector2f grid_extent_; 00147 Eigen::Vector3d newton_lambda_; 00148 public: 00149 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00150 }; 00151 00152 } // namespace pcl 00153 00154 #include <pcl/registration/impl/ndt_2d.hpp> 00155 00156 #endif // ndef PCL_NDT_2D_H_ 00157