Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2009, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 00041 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_LMEDS_H_ 00042 #define PCL_SAMPLE_CONSENSUS_IMPL_LMEDS_H_ 00043 00044 #include <pcl/sample_consensus/lmeds.h> 00045 00046 ////////////////////////////////////////////////////////////////////////// 00047 template <typename PointT> bool 00048 pcl::LeastMedianSquares<PointT>::computeModel (int debug_verbosity_level) 00049 { 00050 // Warn and exit if no threshold was set 00051 if (threshold_ == std::numeric_limits<double>::max()) 00052 { 00053 PCL_ERROR ("[pcl::LeastMedianSquares::computeModel] No threshold set!\n"); 00054 return (false); 00055 } 00056 00057 iterations_ = 0; 00058 double d_best_penalty = std::numeric_limits<double>::max(); 00059 00060 std::vector<int> best_model; 00061 std::vector<int> selection; 00062 Eigen::VectorXf model_coefficients; 00063 std::vector<double> distances; 00064 00065 int n_inliers_count = 0; 00066 00067 unsigned skipped_count = 0; 00068 // supress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters! 00069 const unsigned max_skip = max_iterations_ * 10; 00070 00071 // Iterate 00072 while (iterations_ < max_iterations_ && skipped_count < max_skip) 00073 { 00074 // Get X samples which satisfy the model criteria 00075 sac_model_->getSamples (iterations_, selection); 00076 00077 if (selection.empty ()) break; 00078 00079 // Search for inliers in the point cloud for the current plane model M 00080 if (!sac_model_->computeModelCoefficients (selection, model_coefficients)) 00081 { 00082 //iterations_++; 00083 ++skipped_count; 00084 continue; 00085 } 00086 00087 double d_cur_penalty = 0; 00088 // d_cur_penalty = sum (min (dist, threshold)) 00089 00090 // Iterate through the 3d points and calculate the distances from them to the model 00091 sac_model_->getDistancesToModel (model_coefficients, distances); 00092 00093 // No distances? The model must not respect the user given constraints 00094 if (distances.empty ()) 00095 { 00096 //iterations_++; 00097 ++skipped_count; 00098 continue; 00099 } 00100 00101 std::sort (distances.begin (), distances.end ()); 00102 // d_cur_penalty = median (distances) 00103 size_t mid = sac_model_->getIndices ()->size () / 2; 00104 if (mid >= distances.size ()) 00105 { 00106 //iterations_++; 00107 ++skipped_count; 00108 continue; 00109 } 00110 00111 // Do we have a "middle" point or should we "estimate" one ? 00112 if (sac_model_->getIndices ()->size () % 2 == 0) 00113 d_cur_penalty = (sqrt (distances[mid-1]) + sqrt (distances[mid])) / 2; 00114 else 00115 d_cur_penalty = sqrt (distances[mid]); 00116 00117 // Better match ? 00118 if (d_cur_penalty < d_best_penalty) 00119 { 00120 d_best_penalty = d_cur_penalty; 00121 00122 // Save the current model/coefficients selection as being the best so far 00123 model_ = selection; 00124 model_coefficients_ = model_coefficients; 00125 } 00126 00127 ++iterations_; 00128 if (debug_verbosity_level > 1) 00129 PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_, max_iterations_, d_best_penalty); 00130 } 00131 00132 if (model_.empty ()) 00133 { 00134 if (debug_verbosity_level > 0) 00135 PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Unable to find a solution!\n"); 00136 return (false); 00137 } 00138 00139 // Classify the data points into inliers and outliers 00140 // Sigma = 1.4826 * (1 + 5 / (n-d)) * sqrt (M) 00141 // @note: See "Robust Regression Methods for Computer Vision: A Review" 00142 //double sigma = 1.4826 * (1 + 5 / (sac_model_->getIndices ()->size () - best_model.size ())) * sqrt (d_best_penalty); 00143 //double threshold = 2.5 * sigma; 00144 00145 // Iterate through the 3d points and calculate the distances from them to the model again 00146 sac_model_->getDistancesToModel (model_coefficients_, distances); 00147 // No distances? The model must not respect the user given constraints 00148 if (distances.empty ()) 00149 { 00150 PCL_ERROR ("[pcl::LeastMedianSquares::computeModel] The model found failed to verify against the given constraints!\n"); 00151 return (false); 00152 } 00153 00154 std::vector<int> &indices = *sac_model_->getIndices (); 00155 00156 if (distances.size () != indices.size ()) 00157 { 00158 PCL_ERROR ("[pcl::LeastMedianSquares::computeModel] Estimated distances (%zu) differs than the normal of indices (%zu).\n", distances.size (), indices.size ()); 00159 return (false); 00160 } 00161 00162 inliers_.resize (distances.size ()); 00163 // Get the inliers for the best model found 00164 n_inliers_count = 0; 00165 for (size_t i = 0; i < distances.size (); ++i) 00166 if (distances[i] <= threshold_) 00167 inliers_[n_inliers_count++] = indices[i]; 00168 00169 // Resize the inliers vector 00170 inliers_.resize (n_inliers_count); 00171 00172 if (debug_verbosity_level > 0) 00173 PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Model: %zu size, %d inliers.\n", model_.size (), n_inliers_count); 00174 00175 return (true); 00176 } 00177 00178 #define PCL_INSTANTIATE_LeastMedianSquares(T) template class PCL_EXPORTS pcl::LeastMedianSquares<T>; 00179 00180 #endif // PCL_SAMPLE_CONSENSUS_IMPL_LMEDS_H_ 00181