Point Cloud Library (PCL)  1.3.1
transformation_estimation_lm.hpp
Go to the documentation of this file.
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  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: transformation_estimation_lm.hpp 3041 2011-11-01 04:44:41Z rusu $
00037  *
00038  */
00039 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_
00040 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_
00041 
00042 #include "pcl/registration/warp_point_rigid.h"
00043 #include "pcl/registration/warp_point_rigid_6d.h"
00044 #include "pcl/registration/distances.h"
00045 #include <unsupported/Eigen/NonLinearOptimization>
00047 template <typename PointSource, typename PointTarget> void
00048 pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::estimateRigidTransformation (
00049     const pcl::PointCloud<PointSource> &cloud_src,
00050     const pcl::PointCloud<PointTarget> &cloud_tgt,
00051     Eigen::Matrix4f &transformation_matrix)
00052 {
00053 
00054   // <cloud_src,cloud_src> is the source dataset
00055   if (cloud_src.points.size () != cloud_tgt.points.size ())
00056   {
00057     PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] ");
00058     PCL_ERROR ("Number or points in source (%lu) differs than target (%lu)!\n", 
00059                (unsigned long)cloud_src.points.size (), (unsigned long)cloud_tgt.points.size ());
00060     return;
00061   }
00062   if (cloud_src.points.size () < 4)     // need at least 4 samples
00063   {
00064     PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] ");
00065     PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %lu points!\n", 
00066                (unsigned long)cloud_src.points.size ());
00067     return;
00068   }
00069 
00070   // If no warp function has been set, use the default (WarpPointRigid6D)
00071   if (!warp_point_)
00072     warp_point_.reset (new WarpPointRigid6D<PointSource, PointTarget>);
00073 
00074   int n_unknowns = warp_point_->getDimension ();
00075   int m = cloud_src.points.size ();
00076   Eigen::VectorXd x(n_unknowns);
00077   x.setZero ();
00078   
00079   // Set temporary pointers
00080   tmp_src_ = &cloud_src;
00081   tmp_tgt_ = &cloud_tgt;
00082 
00083   OptimizationFunctor functor (n_unknowns, m, this);
00084   Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
00085   Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor> > lm (num_diff);
00086   int info = lm.minimize (x);
00087 
00088   // Compute the norm of the residuals
00089   PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
00090   PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
00091   PCL_DEBUG ("Final solution: [%f", x[0]);
00092   for (int i = 1; i < n_unknowns; ++i) 
00093     PCL_DEBUG (" %f", x[i]);
00094   PCL_DEBUG ("]\n");
00095 
00096   // Return the correct transformation
00097   Eigen::VectorXf params = x.cast<float> ();
00098   warp_point_->setParam (params);
00099   transformation_matrix = warp_point_->getTransform ();
00100 
00101   tmp_src_ = NULL;
00102   tmp_tgt_ = NULL;
00103 }
00104 
00106 template <typename PointSource, typename PointTarget> void
00107 pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::estimateRigidTransformation (
00108     const pcl::PointCloud<PointSource> &cloud_src,
00109     const std::vector<int> &indices_src,
00110     const pcl::PointCloud<PointTarget> &cloud_tgt,
00111     Eigen::Matrix4f &transformation_matrix)
00112 {
00113   if (indices_src.size () != cloud_tgt.points.size ())
00114   {
00115     PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)cloud_tgt.points.size ());
00116     return;
00117   }
00118 
00119   // <cloud_src,cloud_src> is the source dataset
00120   transformation_matrix.setIdentity ();
00121 
00122   const int nr_correspondences = (int)cloud_tgt.points.size ();
00123   std::vector<int> indices_tgt;
00124   indices_tgt.resize(nr_correspondences);
00125   for (int i = 0; i < nr_correspondences; ++i)
00126     indices_tgt[i] = i;
00127 
00128   estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
00129 }
00130 
00132 template <typename PointSource, typename PointTarget> inline void
00133 pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::estimateRigidTransformation (
00134     const pcl::PointCloud<PointSource> &cloud_src,
00135     const std::vector<int> &indices_src,
00136     const pcl::PointCloud<PointTarget> &cloud_tgt,
00137     const std::vector<int> &indices_tgt,
00138     Eigen::Matrix4f &transformation_matrix)
00139 {
00140   if (indices_src.size () != indices_tgt.size ())
00141   {
00142     PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)indices_tgt.size ());
00143     return;
00144   }
00145 
00146   if (indices_src.size () < 4)     // need at least 4 samples
00147   {
00148     PCL_ERROR ("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
00149     PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %lu points!",
00150                (unsigned long)indices_src.size ());
00151     return;
00152   }
00153 
00154   // If no warp function has been set, use the default (WarpPointRigid6D)
00155   if (!warp_point_)
00156     warp_point_.reset (new WarpPointRigid6D<PointSource, PointTarget>);
00157 
00158   int n_unknowns = warp_point_->getDimension ();  // get dimension of unknown space
00159   int m = indices_src.size ();
00160   Eigen::VectorXd x(n_unknowns);
00161   x.setConstant (n_unknowns, 0);
00162 
00163   // Set temporary pointers
00164   tmp_src_ = &cloud_src;
00165   tmp_tgt_ = &cloud_tgt;
00166   tmp_idx_src_ = &indices_src;
00167   tmp_idx_tgt_ = &indices_tgt;
00168 
00169   OptimizationFunctorWithIndices functor (n_unknowns, m, this);
00170   Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff (functor);
00171   Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices> > lm (num_diff);
00172   int info = lm.minimize (x);
00173 
00174   // Compute the norm of the residuals
00175   PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
00176   PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
00177   PCL_DEBUG ("Final solution: [%f", x[0]);
00178   for (int i = 1; i < n_unknowns; ++i) 
00179     PCL_DEBUG (" %f", x[i]);
00180   PCL_DEBUG ("]\n");
00181 
00182   // Return the correct transformation
00183   Eigen::VectorXf params = x.cast<float> ();
00184   warp_point_->setParam (params);
00185   transformation_matrix = warp_point_->getTransform ();
00186 
00187   tmp_src_ = NULL;
00188   tmp_tgt_ = NULL;
00189   tmp_idx_src_ = tmp_idx_tgt_ = NULL;
00190 }
00191 
00193 template <typename PointSource, typename PointTarget> inline void
00194 pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::estimateRigidTransformation (
00195     const pcl::PointCloud<PointSource> &cloud_src,
00196     const pcl::PointCloud<PointTarget> &cloud_tgt,
00197     const pcl::Correspondences &correspondences,
00198     Eigen::Matrix4f &transformation_matrix)
00199 {
00200   const int nr_correspondences = (int)correspondences.size();
00201   std::vector<int> indices_src(nr_correspondences);
00202   std::vector<int> indices_tgt(nr_correspondences);
00203   for (int i = 0; i < nr_correspondences; ++i)
00204   {
00205     indices_src[i] = correspondences[i].index_query;
00206     indices_tgt[i] = correspondences[i].index_match;
00207   }
00208 
00209   estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
00210 }
00211 
00213 template <typename PointSource, typename PointTarget> int 
00214 pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::OptimizationFunctor::operator() (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
00215 {
00216   const PointCloud<PointSource> & src_points = *estimator_->tmp_src_;
00217   const PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_;
00218 
00219   // Initialize the warp function with the given parameters
00220   Eigen::VectorXf params = x.cast<float> ();
00221   estimator_->warp_point_->setParam (params);
00222 
00223   // Transform each source point and compute its distance to the corresponding target point
00224   for (int i = 0; i < m_values; i++)
00225   {
00226     const PointSource & p_src = src_points.points[i];
00227     const PointTarget & p_tgt = tgt_points.points[i];
00228 
00229     // Transform the source point based on the current warp parameters
00230     PointSource p_src_warped;
00231     estimator_->warp_point_->warpPoint (p_src, p_src_warped);
00232     
00233     // Estimate the distance (cost function)
00234     fvec[i] = estimator_->computeDistance (p_src_warped, p_tgt);
00235   }
00236   return 0;
00237 }
00238 
00240 template <typename PointSource, typename PointTarget> int
00241 pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::OptimizationFunctorWithIndices::operator() (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
00242 {
00243   const PointCloud<PointSource> & src_points = *estimator_->tmp_src_;
00244   const PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_;
00245   const std::vector<int> & src_indices = *estimator_->tmp_idx_src_;
00246   const std::vector<int> & tgt_indices = *estimator_->tmp_idx_tgt_;
00247 
00248   // Initialize the warp function with the given parameters
00249   Eigen::VectorXf params = x.cast<float> ();
00250   estimator_->warp_point_->setParam (params);
00251 
00252   // Transform each source point and compute its distance to the corresponding target point
00253   for (int i = 0; i < m_values; ++i)
00254   {
00255     const PointSource & p_src = src_points.points[src_indices[i]];
00256     const PointTarget & p_tgt = tgt_points.points[tgt_indices[i]];
00257 
00258     // Transform the source point based on the current warp parameters
00259     PointSource p_src_warped;
00260     estimator_->warp_point_->warpPoint (p_src, p_src_warped);
00261     
00262     // Estimate the distance (cost function)
00263     fvec[i] = estimator_->computeDistance (p_src_warped, p_tgt);
00264   }
00265   return (0);
00266 }
00267 /*
00269 template <typename PointSource, typename PointTarget> inline double 
00270 pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::computeMedian (double *fvec, int m)
00271 {
00272   double median;
00273   // Copy the values to vectors for faster sorting
00274   std::vector<double> data (m);
00275   memcpy (&data[0], fvec, sizeof (double) * m);
00276 
00277   std::sort (data.begin (), data.end ());
00278 
00279   int mid = data.size () / 2;
00280   if (data.size () % 2 == 0)
00281     median = (data[mid-1] + data[mid]) / 2.0;
00282   else
00283     median = data[mid];
00284   return (median);
00285 }
00286 */
00287 
00288 //#define PCL_INSTANTIATE_TransformationEstimationLM(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationLM<T,U>;
00289 
00290 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines