gicp.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: gicp.h 3756 2011-12-31 23:54:41Z rusu $
 *
 */

#ifndef PCL_GICP_H_
#define PCL_GICP_H_

#include "pcl/registration/icp.h"
#include "pcl/registration/bfgs.h"

namespace pcl
{
  template <typename PointSource, typename PointTarget>
  class GeneralizedIterativeClosestPoint : public IterativeClosestPoint<PointSource, PointTarget>
  {
    using IterativeClosestPoint<PointSource, PointTarget>::reg_name_;
    using IterativeClosestPoint<PointSource, PointTarget>::getClassName;
    using IterativeClosestPoint<PointSource, PointTarget>::indices_;
    using IterativeClosestPoint<PointSource, PointTarget>::target_;
    using IterativeClosestPoint<PointSource, PointTarget>::input_;
    using IterativeClosestPoint<PointSource, PointTarget>::tree_;
    using IterativeClosestPoint<PointSource, PointTarget>::nr_iterations_;
    using IterativeClosestPoint<PointSource, PointTarget>::max_iterations_;
    using IterativeClosestPoint<PointSource, PointTarget>::previous_transformation_;
    using IterativeClosestPoint<PointSource, PointTarget>::final_transformation_;
    using IterativeClosestPoint<PointSource, PointTarget>::transformation_;
    using IterativeClosestPoint<PointSource, PointTarget>::transformation_epsilon_;
    using IterativeClosestPoint<PointSource, PointTarget>::converged_;
    using IterativeClosestPoint<PointSource, PointTarget>::corr_dist_threshold_;
    using IterativeClosestPoint<PointSource, PointTarget>::inlier_threshold_;
    using IterativeClosestPoint<PointSource, PointTarget>::min_number_correspondences_;
    using IterativeClosestPoint<PointSource, PointTarget>::update_visualizer_;

    typedef pcl::PointCloud<PointSource> PointCloudSource;
    typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
    typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;

    typedef pcl::PointCloud<PointTarget> PointCloudTarget;
    typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
    typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;

    typedef PointIndices::Ptr PointIndicesPtr;
    typedef PointIndices::ConstPtr PointIndicesConstPtr;

    typedef typename pcl::KdTree<PointSource> InputKdTree;
    typedef typename pcl::KdTree<PointSource>::Ptr InputKdTreePtr;
    
    typedef Eigen::Matrix<double, 6, 1> Vector6d;

    public:
      GeneralizedIterativeClosestPoint () 
        : k_correspondences_(20), 
          gicp_epsilon_(0.001), rotation_epsilon_(2e-3),
          input_covariances_(0), target_covariances_(0), mahalanobis_(0),
          max_inner_iterations_(20)
      {
        min_number_correspondences_ = 4;
        reg_name_ = "GeneralizedIterativeClosestPoint";
        max_iterations_ = 200;
        transformation_epsilon_ = 5e-4;
        corr_dist_threshold_ = 5.;
        rigid_transformation_estimation_ = 
          boost::bind (&GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationBFGS, 
                       this, _1, _2, _3, _4, _5); 
        input_tree_.reset (new pcl::KdTreeFLANN<PointSource>);
      }

      inline void
      setInputCloud (const PointCloudSourceConstPtr &cloud)
      {
        if (cloud->points.empty ())
        {
          PCL_ERROR ("[pcl::%s::setInputInput] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
          return;
        }
        PointCloudSource input = *cloud;
        // Set all the point.data[3] values to 1 to aid the rigid transformation
        for (size_t i = 0; i < input.size (); ++i)
          input[i].data[3] = 1.0;
        
        input_ = input.makeShared ();
        input_tree_->setInputCloud (input_);
        input_covariances_.reserve (input_->size ());
      }

      inline void 
      setInputTarget (const PointCloudTargetConstPtr &target)
      {
        pcl::Registration<PointSource, PointTarget>::setInputTarget(target);
        target_covariances_.reserve (target_->size ());
      }

      void
      estimateRigidTransformationBFGS (const PointCloudSource &cloud_src,
                                       const std::vector<int> &indices_src,
                                       const PointCloudTarget &cloud_tgt,
                                       const std::vector<int> &indices_tgt,
                                       Eigen::Matrix4f &transformation_matrix);
      
      inline const Eigen::Matrix3d& mahalanobis(size_t index) const
      {
        assert(index < mahalanobis_.size());
        return mahalanobis_[index];
      }

      void
      computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const;

      inline void 
      setRotationEpsilon (double epsilon) { rotation_epsilon_ = epsilon; }

      inline double 
      getRotationEpsilon () { return (rotation_epsilon_); }

      void
      setCorrespondenceRandomness (int k) { k_correspondences_ = k; }

      void
      getCorrespondenceRandomness () { return (k_correspondences_); }

    private:

      int k_correspondences_;

      double gicp_epsilon_;

      double rotation_epsilon_;

      Eigen::Matrix4f base_transformation_;

      boost::mutex tmp_mutex_;

      const PointCloudSource *tmp_src_;

      const PointCloudTarget  *tmp_tgt_;

      const std::vector<int> *tmp_idx_src_;

      const std::vector<int> *tmp_idx_tgt_;

      InputKdTreePtr input_tree_;
      
      std::vector<Eigen::Matrix3d> input_covariances_;

      std::vector<Eigen::Matrix3d> target_covariances_;

      std::vector<Eigen::Matrix3d> mahalanobis_;
      
      int max_inner_iterations_;

      template<typename PointT>
      void computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud, 
                              const typename pcl::KdTree<PointT>::Ptr tree,
                              std::vector<Eigen::Matrix3d>& cloud_covariances);

      inline double 
      matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const
      {
        double r = 0.;
        size_t n = mat1.rows();
        // tr(mat1^t.mat2)
        for(size_t i = 0; i < n; i++)
          for(size_t j = 0; j < n; j++)
            r += mat1 (j, i) * mat2 (i,j);
        return r;
      }

      void 
      computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess);

      inline bool 
      searchForNeighbors (const PointSource &query, std::vector<int>& index, std::vector<float>& distance)
      {
        int k = tree_->nearestKSearch (query, 1, index, distance);
        if (k == 0)
          return (false);
        return (true);
      }

      void applyState(Eigen::Matrix4f &t, const Vector6d& x) const;
      
      struct OptimizationFunctorWithIndices : public BFGSDummyFunctor<double,6>
      {
        OptimizationFunctorWithIndices (const GeneralizedIterativeClosestPoint* gicp)
          : BFGSDummyFunctor<double,6> (), gicp_(gicp) {}
        double operator() (const Vector6d& x);
        void  df(const Vector6d &x, Vector6d &df);
        void fdf(const Vector6d &x, double &f, Vector6d &df);

        const GeneralizedIterativeClosestPoint *gicp_;
      };
      
    protected:
      boost::function<void(const pcl::PointCloud<PointSource> &cloud_src,
                           const std::vector<int> &src_indices,
                           const pcl::PointCloud<PointTarget> &cloud_tgt,
                           const std::vector<int> &tgt_indices,
                           Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_;
  };
}

#include "pcl/registration/impl/gicp.hpp"

#endif  //#ifndef PCL_GICP_H_