Point Cloud Library (PCL)
1.3.1
|
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