Point Cloud Library (PCL)  1.3.1
sac_model_registration.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: sac_model_registration.hpp 2502 2011-09-18 23:35:42Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_
00039 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_
00040 
00041 #include "pcl/sample_consensus/sac_model_registration.h"
00042 
00044 template <typename PointT> bool
00045 pcl::SampleConsensusModelRegistration<PointT>::isSampleGood (const std::vector<int> &samples) const
00046 {
00047   return ((input_->points[samples[1]].getArray4fMap () - input_->points[samples[0]].getArray4fMap ()).matrix ().squaredNorm () > sample_dist_thresh_ && 
00048           (input_->points[samples[2]].getArray4fMap () - input_->points[samples[0]].getArray4fMap ()).matrix ().squaredNorm () > sample_dist_thresh_ && 
00049           (input_->points[samples[2]].getArray4fMap () - input_->points[samples[1]].getArray4fMap ()).matrix ().squaredNorm () > sample_dist_thresh_);
00050 }
00051 
00053 template <typename PointT> bool
00054 pcl::SampleConsensusModelRegistration<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
00055 {
00056   if (!target_)
00057   {
00058     PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeModelCoefficients] No target dataset given!\n");
00059     return (false);
00060   }
00061   // Need 3 samples
00062   if (samples.size () != 3)
00063     return (false);
00064 
00065   std::vector<int> indices_tgt (3);
00066   for (int i = 0; i < 3; ++i)
00067     indices_tgt[i] = correspondences_[samples[i]];
00068 
00069   estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients);
00070   return (true);
00071 }
00072 
00074 template <typename PointT> void
00075 pcl::SampleConsensusModelRegistration<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) 
00076 {
00077   if (indices_->size () != indices_tgt_->size ())
00078   {
00079     PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistancesToModel] Number of source indices (%lu) differs than number of target indices (%lu)!\n", (unsigned long)indices_->size (), (unsigned long)indices_tgt_->size ());
00080     distances.clear ();
00081     return;
00082   }
00083   if (!target_)
00084   {
00085     PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistanceToModel] No target dataset given!\n");
00086     return;
00087   }
00088   // Check if the model is valid given the user constraints
00089   if (!isModelValid (model_coefficients))
00090   {
00091     distances.clear ();
00092     return;
00093   }
00094   distances.resize (indices_->size ());
00095 
00096   // Get the 4x4 transformation
00097   Eigen::Matrix4f transform;
00098   transform.row (0) = model_coefficients.segment<4>(0);
00099   transform.row (1) = model_coefficients.segment<4>(4);
00100   transform.row (2) = model_coefficients.segment<4>(8);
00101   transform.row (3) = model_coefficients.segment<4>(12);
00102 
00103   for (size_t i = 0; i < indices_->size (); ++i)
00104   {
00105     Eigen::Vector4f pt_src = input_->points[(*indices_)[i]].getVector4fMap ();
00106     pt_src[3] = 1;
00107     Eigen::Vector4f pt_tgt = target_->points[(*indices_tgt_)[i]].getVector4fMap ();
00108     pt_tgt[3] = 1;
00109 
00110     Eigen::Vector4f p_tr = transform * pt_src;
00111     // Calculate the distance from the transformed point to its correspondence
00112     // need to compute the real norm here to keep MSAC and friends general
00113     distances[i] = (p_tr - pt_tgt).norm ();
00114   }
00115 }
00116 
00118 template <typename PointT> void
00119 pcl::SampleConsensusModelRegistration<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers) 
00120 {
00121   if (indices_->size () != indices_tgt_->size ())
00122   {
00123     PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", (unsigned long)indices_->size (), (unsigned long)indices_tgt_->size ());
00124     inliers.clear ();
00125     return;
00126   }
00127   if (!target_)
00128   {
00129     PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] No target dataset given!\n");
00130     return;
00131   }
00132 
00133   double thresh = threshold * threshold;
00134 
00135   // Check if the model is valid given the user constraints
00136   if (!isModelValid (model_coefficients))
00137   {
00138     inliers.clear ();
00139     return;
00140   }
00141   
00142   inliers.resize (indices_->size ());
00143 
00144   Eigen::Matrix4f transform;
00145   transform.row (0) = model_coefficients.segment<4>(0);
00146   transform.row (1) = model_coefficients.segment<4>(4);
00147   transform.row (2) = model_coefficients.segment<4>(8);
00148   transform.row (3) = model_coefficients.segment<4>(12);
00149 
00150   int nr_p = 0; 
00151   for (size_t i = 0; i < indices_->size (); ++i)
00152   {
00153     Eigen::Vector4f pt_src = input_->points[(*indices_)[i]].getVector4fMap ();
00154     pt_src[3] = 1;
00155     Eigen::Vector4f pt_tgt = target_->points[(*indices_tgt_)[i]].getVector4fMap ();
00156     pt_tgt[3] = 1;
00157 
00158     Eigen::Vector4f p_tr  = transform * pt_src;
00159     // Calculate the distance from the transformed point to its correspondence
00160     if ((p_tr - pt_tgt).squaredNorm () < thresh)
00161       inliers[nr_p++] = (*indices_)[i];
00162   }
00163   inliers.resize (nr_p);
00164 } 
00165 
00167 template <typename PointT> int
00168 pcl::SampleConsensusModelRegistration<PointT>::countWithinDistance (
00169     const Eigen::VectorXf &model_coefficients, const double threshold)
00170 {
00171   if (indices_->size () != indices_tgt_->size ())
00172   {
00173     PCL_ERROR ("[pcl::SampleConsensusModelRegistration::countWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", (unsigned long)indices_->size (), (unsigned long)indices_tgt_->size ());
00174     return (0);
00175   }
00176   if (!target_)
00177   {
00178     PCL_ERROR ("[pcl::SampleConsensusModelRegistration::countWithinDistance] No target dataset given!\n");
00179     return (0);
00180   }
00181 
00182   double thresh = threshold * threshold;
00183 
00184   // Check if the model is valid given the user constraints
00185   if (!isModelValid (model_coefficients))
00186     return (0);
00187   
00188   Eigen::Matrix4f transform;
00189   transform.row (0) = model_coefficients.segment<4>(0);
00190   transform.row (1) = model_coefficients.segment<4>(4);
00191   transform.row (2) = model_coefficients.segment<4>(8);
00192   transform.row (3) = model_coefficients.segment<4>(12);
00193 
00194   int nr_p = 0; 
00195   for (size_t i = 0; i < indices_->size (); ++i)
00196   {
00197     Eigen::Vector4f pt_src = input_->points[(*indices_)[i]].getVector4fMap ();
00198     pt_src[3] = 1;
00199     Eigen::Vector4f pt_tgt = target_->points[(*indices_tgt_)[i]].getVector4fMap ();
00200     pt_tgt[3] = 1;
00201 
00202     Eigen::Vector4f p_tr  = transform * pt_src;
00203     // Calculate the distance from the transformed point to its correspondence
00204     if ((p_tr - pt_tgt).squaredNorm () < thresh)
00205       nr_p++;
00206   }
00207   return (nr_p);
00208 } 
00209 
00211 template <typename PointT> void
00212 pcl::SampleConsensusModelRegistration<PointT>::optimizeModelCoefficients (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) 
00213 {
00214   if (indices_->size () != indices_tgt_->size ())
00215   {
00216     PCL_ERROR ("[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Number of source indices (%lu) differs than number of target indices (%lu)!\n", (unsigned long)indices_->size (), (unsigned long)indices_tgt_->size ());
00217     optimized_coefficients = model_coefficients;
00218     return;
00219   }
00220 
00221   // Check if the model is valid given the user constraints
00222   if (!isModelValid (model_coefficients) || !target_)
00223   {
00224     optimized_coefficients = model_coefficients;
00225     return;
00226   }
00227 
00228   std::vector<int> indices_src (inliers.size ());
00229   std::vector<int> indices_tgt (inliers.size ());
00230   for (size_t i = 0; i < inliers.size (); ++i)
00231   {
00232     // NOTE: not tested!
00233     indices_src[i] = (*indices_)[inliers[i]];
00234     indices_tgt[i] = (*indices_tgt_)[inliers[i]];
00235   }
00236 
00237   estimateRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients);
00238 }
00239 
00241 template <typename PointT> void
00242 pcl::SampleConsensusModelRegistration<PointT>::estimateRigidTransformationSVD (
00243     const pcl::PointCloud<PointT> &cloud_src, 
00244     const std::vector<int> &indices_src, 
00245     const pcl::PointCloud<PointT> &cloud_tgt,
00246     const std::vector<int> &indices_tgt, 
00247     Eigen::VectorXf &transform)
00248 {
00249   transform.resize (16);
00250   Eigen::Vector4f centroid_src, centroid_tgt;
00251   // Estimate the centroids of source, target
00252   compute3DCentroid (cloud_src, indices_src, centroid_src);
00253   compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt);
00254 
00255   // Subtract the centroids from source, target
00256   Eigen::MatrixXf cloud_src_demean;
00257   demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);
00258 
00259   Eigen::MatrixXf cloud_tgt_demean;
00260   demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean);
00261 
00262   // Assemble the correlation matrix H = source * target'
00263   Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
00264 
00265   // Compute the Singular Value Decomposition
00266   Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
00267   Eigen::Matrix3f u = svd.matrixU ();
00268   Eigen::Matrix3f v = svd.matrixV ();
00269 
00270   // Compute R = V * U'
00271   if (u.determinant () * v.determinant () < 0)
00272   {
00273     for (int x = 0; x < 3; ++x)
00274       v (x, 2) *= -1;
00275   }
00276 
00277   Eigen::Matrix3f R = v * u.transpose ();
00278 
00279   // Return the correct transformation
00280   transform.segment<3> (0) = R.row (0); transform[12]  = 0;
00281   transform.segment<3> (4) = R.row (1); transform[13]  = 0;
00282   transform.segment<3> (8) = R.row (2); transform[14] = 0;
00283 
00284   Eigen::Vector3f t = centroid_tgt.head<3> () - R * centroid_src.head<3> ();
00285   transform[3] = t[0]; transform[7] = t[1]; transform[11] = t[2]; transform[15] = 1.0;
00286 }
00287 
00288 #define PCL_INSTANTIATE_SampleConsensusModelRegistration(T) template class PCL_EXPORTS pcl::SampleConsensusModelRegistration<T>;
00289 
00290 #endif    // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_
00291 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines