icp_nl.hpp

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: icp_nl.hpp 1370 2011-06-19 01:06:01Z jspricke $
00035  *
00036  */
00037 
00038 #include <boost/unordered_map.hpp>
00039 
00041 template <typename PointSource, typename PointTarget> void
00042 pcl::IterativeClosestPointNonLinear<PointSource, PointTarget>::estimateRigidTransformationLM (
00043       const PointCloudSource &cloud_src, const PointCloudTarget &cloud_tgt, Eigen::Matrix4f &transformation_matrix)
00044 {
00045   boost::mutex::scoped_lock lock (tmp_mutex_);
00046 
00047   int n_unknowns = 6;      // 6 unknowns: 3 translation + 3 rotation (quaternion)
00048 
00049   if (cloud_src.points.size () != cloud_tgt.points.size ())
00050   {
00051     PCL_ERROR ("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)cloud_src.points.size (), (unsigned long)cloud_tgt.points.size ());
00052     return;
00053   }
00054   if (cloud_src.points.size () < 4)     // need at least 4 samples
00055   {
00056     PCL_ERROR ("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] Need at least 4 points to estimate a transform! Source and target have %lu points!\n", (unsigned long)cloud_src.points.size ());
00057     return;
00058   }
00059 
00060   int m = cloud_src.points.size ();
00061   double *fvec = new double[m];
00062   int *iwa = new int[n_unknowns];
00063   int lwa = m * n_unknowns + 5 * n_unknowns + m;
00064   double *wa = new double[lwa];
00065 
00066   // Set the initial solution
00067   double *x = new double[n_unknowns];
00068   // Translation estimates - initial guess
00069   x[0] = 0; x[1] = 0; x[2] = 0;
00070   // Rotation estimates - initial guess quaternion: x-y-z-w
00071   x[3] = 0; x[4] = 0; x[5] = 0;
00072 
00073   // Set temporary pointers
00074   tmp_src_ = &cloud_src;
00075   tmp_tgt_ = &cloud_tgt;
00076 
00077   // Set tol to the square root of the machine. Unless high solutions are required, these are the recommended settings.
00078   double tol = sqrt (dpmpar (1));
00079 
00080   // Optimize using forward-difference approximation LM
00081   //int info = lmdif1 (&pcl::IterativeClosestPointNonLinear<PointSource, PointTarget>::functionToOptimize, this, m, n_unknowns, x, fvec, tol, iwa, wa, lwa);
00082   int info = lmdif1 (&pcl::IterativeClosestPointNonLinear<PointSource, PointTarget>::functionToOptimize, this, m, n_unknowns, x, fvec, tol, iwa, wa, lwa);
00083 
00084   // Compute the norm of the residuals
00085   PCL_DEBUG ("[pcl::%s::estimateRigidTransformationLM] LM solver finished with exit code %i, having a residual norm of %g. \n",
00086              //"\nFinal solution: [%f %f %f %f] [%f %f %f]", 
00087              getClassName ().c_str (), info, enorm (m, fvec));
00088              //x[0], x[1], x[2], x[3], x[4], x[5], x[6]);
00089 
00090   delete [] wa; delete [] fvec;
00091   // <cloud_src,cloud_src> is the source dataset
00092   transformation_matrix.setZero ();
00093 
00094   // Return the correct transformation
00095   // Compute w from the unit quaternion
00096   Eigen::Quaternionf q (0, x[3], x[4], x[5]);
00097   q.w () = sqrt (1 - q.dot (q));
00098   transformation_matrix.topLeftCorner<3, 3> () = q.toRotationMatrix ();
00099 
00100   Eigen::Vector4f t (x[0], x[1], x[2], 1.0);
00101   transformation_matrix.block <4, 1> (0, 3) = t;
00102 
00103   tmp_src_ = tmp_tgt_ = NULL;
00104 
00105   delete[] iwa;
00106   delete[] x;
00107 }
00108 
00110 template <typename PointSource, typename PointTarget> void
00111   pcl::IterativeClosestPointNonLinear<PointSource, PointTarget>::estimateRigidTransformationLM (
00112       const PointCloudSource &cloud_src, const std::vector<int> &indices_src, const PointCloudTarget &cloud_tgt, const std::vector<int> &indices_tgt, Eigen::Matrix4f &transformation_matrix)
00113 {
00114   boost::mutex::scoped_lock lock (tmp_mutex_);
00115 
00116   int n_unknowns = 6;      // 6 unknowns:  3 translation + 3 rotation (quaternion)
00117 
00118   if (indices_src.size () != indices_tgt.size ())
00119   {
00120     PCL_ERROR ("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)indices_tgt.size ());
00121     return;
00122   }
00123   if (indices_src.size () < 4)     // need at least 4 samples
00124   {
00125     PCL_ERROR ("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] Need at least 4 points to estimate a transform! Source and target have %lu points!", (unsigned long)indices_src.size ());
00126     return;
00127   }
00128 
00129   int m = indices_src.size ();
00130   double *fvec = new double[m];
00131   int *iwa = new int[n_unknowns];
00132   int lwa = m * n_unknowns + 5 * n_unknowns + m;
00133   double *wa = new double[lwa];
00134 
00135   // Set the initial solution
00136   double *x = new double[n_unknowns];
00137   // Translation estimates - initial guess
00138   x[0] = 0; x[1] = 0; x[2] = 0;
00139   // Rotation estimates - initial guess quaternion: x-y-z-w
00140   x[3] = 0; x[4] = 0; x[5] = 0;
00141 
00142   // Set temporary pointers
00143   tmp_src_ = &cloud_src;
00144   tmp_tgt_ = &cloud_tgt;
00145   tmp_idx_src_ = &indices_src;
00146   tmp_idx_tgt_ = &indices_tgt;
00147 
00148   // Set tol to the square root of the machine. Unless high solutions are required, these are the recommended settings.
00149   double tol = sqrt (dpmpar (1));
00150 
00151   // Optimize using forward-difference approximation LM
00152   int info = lmdif1 (&pcl::IterativeClosestPointNonLinear<PointSource, PointTarget>::functionToOptimizeIndices, this, m, n_unknowns, x, fvec, tol, iwa, wa, lwa);
00153 
00154   // Compute the norm of the residuals
00155   PCL_DEBUG ("[pcl::%s::estimateRigidTransformationLM] LM solver finished with exit code %i, having a residual norm of %g. \n",
00156              //"\nFinal solution: [%f %f %f %f] [%f %f %f]", 
00157              getClassName ().c_str (), info, enorm (m, fvec));
00158              //x[0], x[1], x[2], x[3], x[4], x[5], x[6]);
00159 
00160   delete [] wa; delete [] fvec;
00161   // <cloud_src,cloud_src> is the source dataset
00162   transformation_matrix.setZero ();
00163 
00164   // Return the correct transformation
00165   // Compute w from the unit quaternion
00166   Eigen::Quaternionf q (0, x[3], x[4], x[5]);
00167   q.w () = sqrt (1 - q.dot (q));
00168   transformation_matrix.topLeftCorner<3, 3> () = q.toRotationMatrix ();
00169 
00170   Eigen::Vector4f t (x[0], x[1], x[2], 1.0);
00171   transformation_matrix.block <4, 1> (0, 3) = t;
00172 
00173   tmp_src_ = tmp_tgt_ = NULL;
00174   tmp_idx_src_ = tmp_idx_tgt_ = NULL;
00175 
00176   delete[] iwa;
00177   delete[] x;
00178 }
00179 
00181 template <typename PointSource, typename PointTarget> inline int
00182   pcl::IterativeClosestPointNonLinear<PointSource, PointTarget>::functionToOptimize (void *p, int m, int n, const double *x, double *fvec, int iflag)
00183 {
00184   IterativeClosestPointNonLinear *model = (IterativeClosestPointNonLinear*)p;
00185 
00186   // Copy the rotation and translation components
00187   Eigen::Vector4f t (x[0], x[1], x[2], 1.0);
00188   // Compute w from the unit quaternion
00189   Eigen::Quaternionf q (0, x[3], x[4], x[5]);
00190   q.w () = sqrt (1 - q.dot (q));
00191   // If the quaternion is used to rotate several points (>1) then it is much more efficient to first convert it
00192   // to a 3x3 Matrix. Comparison of the operation cost for n transformations:
00193   // * Quaternion: 30n
00194   // * Via a Matrix3: 24 + 15n
00195   Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Zero ();
00196   transformation_matrix.topLeftCorner<3, 3> () = q.toRotationMatrix ();
00197   transformation_matrix.block <4, 1> (0, 3) = t;
00198 
00199   double sigma = model->getMaxCorrespondenceDistance ();
00200   for (int i = 0; i < m; ++i)
00201   {
00202     // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
00203     Vector4fMapConst p_src = model->tmp_src_->points[i].getVector4fMap ();
00204     // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
00205     Vector4fMapConst p_tgt = model->tmp_tgt_->points[i].getVector4fMap ();
00206 
00207     Eigen::Vector4f pp = transformation_matrix * p_src;
00208 
00209     // Estimate the distance (cost function)
00210     //fvec[i] = model->distL2Sqr (p_tgt, pp);
00211     //fvec[i] = model->distL1 (pp, p_tgt);
00212     fvec[i] = model->distHuber (pp, p_tgt, sigma);
00213   }
00214   return (0);
00215 }
00216 
00218 template <typename PointSource, typename PointTarget> inline double
00219   pcl::IterativeClosestPointNonLinear<PointSource, PointTarget>::computeMedian (double *fvec, int m)
00220 {
00221   double median;
00222   // Copy the values to vectors for faster sorting
00223   std::vector<double> data (m);
00224   memcpy (&data[0], fvec, sizeof (double) * m);
00225 
00226   std::sort (data.begin (), data.end ());
00227 
00228   int mid = data.size () / 2;
00229   if (data.size () % 2 == 0)
00230     median = (data[mid-1] + data[mid]) / 2.0;
00231   else
00232     median = data[mid];
00233   return (median);
00234 }
00235 
00237 template <typename PointSource, typename PointTarget> inline int
00238   pcl::IterativeClosestPointNonLinear<PointSource, PointTarget>::functionToOptimizeIndices (void *p, int m, int n, const double *x, double *fvec, int iflag)
00239 {
00240   IterativeClosestPointNonLinear *model = (IterativeClosestPointNonLinear*)p;
00241 
00242   // Copy the rotation and translation components
00243   Eigen::Vector4f t (x[0], x[1], x[2], 1.0);
00244   // Compute w from the unit quaternion
00245   Eigen::Quaternionf q (0, x[3], x[4], x[5]);
00246   q.w () = sqrt (1 - q.dot (q));
00247 
00248   // If the quaternion is used to rotate several points (>1) then it is much more efficient to first convert it
00249   // to a 3x3 Matrix. Comparison of the operation cost for n transformations:
00250   // * Quaternion: 30n
00251   // * Via a Matrix3: 24 + 15n
00252   Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Zero ();
00253   transformation_matrix.topLeftCorner<3, 3> () = q.toRotationMatrix ();
00254   transformation_matrix.block <4, 1> (0, 3) = t;
00255 
00256   //double sigma = model->getMaxCorrespondenceDistance ();
00257   for (int i = 0; i < m; ++i)
00258   {
00259     // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
00260     Vector4fMapConst p_src = model->tmp_src_->points[(*model->tmp_idx_src_)[i]].getVector4fMap ();
00261     // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
00262     Vector4fMapConst p_tgt = model->tmp_tgt_->points[(*model->tmp_idx_tgt_)[i]].getVector4fMap ();
00263 
00264     Eigen::Vector4f pp = transformation_matrix * p_src;
00265 
00266     // Estimate the distance (cost function)
00267     //fvec[i] = model->distL2Sqr (p_tgt, pp);
00268     fvec[i] = model->distL1 (pp, p_tgt);
00269     //fvec[i] = model->distHuber (pp, p_tgt, sigma);
00270   }
00271 /*  // Compute the median
00272   double median = model->computeMedian (fvec, m);
00273 
00274   double stddev = 1.4826 * median;
00275 
00276   std::vector<double> weights (m);
00277   // For all points, compute weights
00278   for (int i = 0; i < m; ++i)
00279   {
00280     weights[i] = model->distHuber (fvec[i], stddev);
00281     fvec[i] *= weights[i];
00282   }*/
00283   return (0);
00284 }
00285