Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/transformation_validation_euclidean.h
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