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) 2010-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 #ifndef PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_H_ 00041 #define PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_H_ 00042 00043 #include <pcl/point_representation.h> 00044 #include <pcl/search/kdtree.h> 00045 #include <pcl/kdtree/kdtree.h> 00046 #include <pcl/kdtree/kdtree_flann.h> 00047 #include <pcl/registration/transformation_validation.h> 00048 00049 namespace pcl 00050 { 00051 namespace registration 00052 { 00053 /** \brief TransformationValidationEuclidean computes an L2SQR norm between a source and target 00054 * dataset. 00055 * 00056 * To prevent points with bad correspondences to contribute to the overall score, the class also 00057 * accepts a maximum_range parameter given via \ref setMaxRange that is used as a cutoff value for 00058 * nearest neighbor distance comparisons. 00059 * 00060 * The output score is normalized with respect to the number of valid correspondences found. 00061 * 00062 * Usage example: 00063 * \code 00064 * pcl::TransformationValidationEuclidean<pcl::PointXYZ, pcl::PointXYZ> tve; 00065 * tve.setMaxRange (0.01); // 1cm 00066 * double score = tve.validateTransformation (source, target, transformation); 00067 * \endcode 00068 * 00069 * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float. 00070 * \author Radu B. Rusu 00071 * \ingroup registration 00072 */ 00073 template <typename PointSource, typename PointTarget, typename Scalar = float> 00074 class TransformationValidationEuclidean 00075 { 00076 public: 00077 typedef typename TransformationValidation<PointSource, PointTarget, Scalar>::Matrix4 Matrix4; 00078 00079 typedef boost::shared_ptr<TransformationValidation<PointSource, PointTarget, Scalar> > Ptr; 00080 typedef boost::shared_ptr<const TransformationValidation<PointSource, PointTarget, Scalar> > ConstPtr; 00081 00082 typedef typename pcl::search::KdTree<PointTarget> KdTree; 00083 typedef typename pcl::search::KdTree<PointTarget>::Ptr KdTreePtr; 00084 00085 typedef typename KdTree::PointRepresentationConstPtr PointRepresentationConstPtr; 00086 00087 typedef typename TransformationValidation<PointSource, PointTarget>::PointCloudSourceConstPtr PointCloudSourceConstPtr; 00088 typedef typename TransformationValidation<PointSource, PointTarget>::PointCloudTargetConstPtr PointCloudTargetConstPtr; 00089 00090 /** \brief Constructor. 00091 * Sets the \a max_range parameter to double::max, \a threshold_ to NaN 00092 * and initializes the internal search \a tree to a FLANN kd-tree. 00093 */ 00094 TransformationValidationEuclidean () : 00095 max_range_ (std::numeric_limits<double>::max ()), 00096 threshold_ (std::numeric_limits<double>::quiet_NaN ()), 00097 tree_ (new pcl::search::KdTree<PointTarget>), 00098 force_no_recompute_ (false) 00099 { 00100 } 00101 00102 virtual ~TransformationValidationEuclidean () {}; 00103 00104 /** \brief Set the maximum allowable distance between a point and its correspondence in the 00105 * target in order for a correspondence to be considered \a valid. Default: double::max. 00106 * \param[in] max_range the new maximum allowable distance 00107 */ 00108 inline void 00109 setMaxRange (double max_range) 00110 { 00111 max_range_ = max_range; 00112 } 00113 00114 /** \brief Get the maximum allowable distance between a point and its 00115 * correspondence, as set by the user. 00116 */ 00117 inline double 00118 getMaxRange () 00119 { 00120 return (max_range_); 00121 } 00122 00123 00124 /** \brief Provide a pointer to the search object used to find correspondences in 00125 * the target cloud. 00126 * \param[in] tree a pointer to the spatial search object. 00127 * \param[in] force_no_recompute If set to true, this tree will NEVER be 00128 * recomputed, regardless of calls to setInputTarget. Only use if you are 00129 * confident that the tree will be set correctly. 00130 */ 00131 inline void 00132 setSearchMethodTarget (const KdTreePtr &tree, 00133 bool force_no_recompute = false) 00134 { 00135 tree_ = tree; 00136 if (force_no_recompute) 00137 { 00138 force_no_recompute_ = true; 00139 } 00140 } 00141 00142 /** \brief Set a threshold for which a specific transformation is considered valid. 00143 * 00144 * \note Since we're using MSE (Mean Squared Error) as a metric, the threshold 00145 * represents the mean Euclidean distance threshold over all nearest neighbors 00146 * up to max_range. 00147 * 00148 * \param[in] threshold the threshold for which a transformation is vali 00149 */ 00150 inline void 00151 setThreshold (double threshold) 00152 { 00153 threshold_ = threshold; 00154 } 00155 00156 /** \brief Get the threshold for which a specific transformation is valid. */ 00157 inline double 00158 getThreshold () 00159 { 00160 return (threshold_); 00161 } 00162 00163 /** \brief Validate the given transformation with respect to the input cloud data, and return a score. 00164 * 00165 * \param[in] cloud_src the source point cloud dataset 00166 * \param[in] cloud_tgt the target point cloud dataset 00167 * \param[out] transformation_matrix the resultant transformation matrix 00168 * 00169 * \return the score or confidence measure for the given 00170 * transformation_matrix with respect to the input data 00171 */ 00172 double 00173 validateTransformation ( 00174 const PointCloudSourceConstPtr &cloud_src, 00175 const PointCloudTargetConstPtr &cloud_tgt, 00176 const Matrix4 &transformation_matrix) const; 00177 00178 /** \brief Comparator function for deciding which score is better after running the 00179 * validation on multiple transforms. 00180 * 00181 * \param[in] score1 the first value 00182 * \param[in] score2 the second value 00183 * 00184 * \return true if score1 is better than score2 00185 */ 00186 virtual bool 00187 operator() (const double &score1, const double &score2) const 00188 { 00189 return (score1 < score2); 00190 } 00191 00192 /** \brief Check if the score is valid for a specific transformation. 00193 * 00194 * \param[in] cloud_src the source point cloud dataset 00195 * \param[in] cloud_tgt the target point cloud dataset 00196 * \param[out] transformation_matrix the transformation matrix 00197 * 00198 * \return true if the transformation is valid, false otherwise. 00199 */ 00200 virtual bool 00201 isValid ( 00202 const PointCloudSourceConstPtr &cloud_src, 00203 const PointCloudTargetConstPtr &cloud_tgt, 00204 const Matrix4 &transformation_matrix) const 00205 { 00206 if (pcl_isnan (threshold_)) 00207 { 00208 PCL_ERROR ("[pcl::TransformationValidationEuclidean::isValid] Threshold not set! Please use setThreshold () before continuing."); 00209 return (false); 00210 } 00211 00212 return (validateTransformation (cloud_src, cloud_tgt, transformation_matrix) < threshold_); 00213 } 00214 00215 protected: 00216 /** \brief The maximum allowable distance between a point and its correspondence in the target 00217 * in order for a correspondence to be considered \a valid. Default: double::max. 00218 */ 00219 double max_range_; 00220 00221 /** \brief The threshold for which a specific transformation is valid. 00222 * Set to NaN by default, as we must require the user to set it. 00223 */ 00224 double threshold_; 00225 00226 /** \brief A pointer to the spatial search object. */ 00227 KdTreePtr tree_; 00228 00229 /** \brief A flag which, if set, means the tree operating on the target cloud 00230 * will never be recomputed*/ 00231 bool force_no_recompute_; 00232 00233 00234 /** \brief Internal point representation uses only 3D coordinates for L2 */ 00235 class MyPointRepresentation: public pcl::PointRepresentation<PointTarget> 00236 { 00237 using pcl::PointRepresentation<PointTarget>::nr_dimensions_; 00238 using pcl::PointRepresentation<PointTarget>::trivial_; 00239 public: 00240 typedef boost::shared_ptr<MyPointRepresentation> Ptr; 00241 typedef boost::shared_ptr<const MyPointRepresentation> ConstPtr; 00242 00243 MyPointRepresentation () 00244 { 00245 nr_dimensions_ = 3; 00246 trivial_ = true; 00247 } 00248 00249 /** \brief Empty destructor */ 00250 virtual ~MyPointRepresentation () {} 00251 00252 virtual void 00253 copyToFloatArray (const PointTarget &p, float * out) const 00254 { 00255 out[0] = p.x; 00256 out[1] = p.y; 00257 out[2] = p.z; 00258 } 00259 }; 00260 00261 public: 00262 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00263 }; 00264 } 00265 } 00266 00267 #include <pcl/registration/impl/transformation_validation_euclidean.hpp> 00268 00269 #endif // PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_H_ 00270