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-2012, 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_FILTERS_IMPL_COVARIANCE_SAMPLING_H_ 00042 #define PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_ 00043 00044 #include <pcl/common/eigen.h> 00045 #include <pcl/filters/covariance_sampling.h> 00046 #include <list> 00047 00048 /////////////////////////////////////////////////////////////////////////////// 00049 template<typename PointT, typename PointNT> bool 00050 pcl::CovarianceSampling<PointT, PointNT>::initCompute () 00051 { 00052 if (!FilterIndices<PointT>::initCompute ()) 00053 return false; 00054 00055 if (num_samples_ > indices_->size ()) 00056 { 00057 PCL_ERROR ("[pcl::CovarianceSampling::initCompute] The number of samples you asked for (%d) is larger than the number of input indices (%zu)\n", 00058 num_samples_, indices_->size ()); 00059 return false; 00060 } 00061 00062 // Prepare the point cloud by centering at the origin and then scaling the points such that the average distance from 00063 // the origin is 1.0 => rotations and translations will have the same magnitude 00064 Eigen::Vector3f centroid (0.f, 0.f, 0.f); 00065 for (size_t p_i = 0; p_i < indices_->size (); ++p_i) 00066 centroid += (*input_)[(*indices_)[p_i]].getVector3fMap (); 00067 centroid /= float (indices_->size ()); 00068 00069 scaled_points_.resize (indices_->size ()); 00070 double average_norm = 0.0; 00071 for (size_t p_i = 0; p_i < indices_->size (); ++p_i) 00072 { 00073 scaled_points_[p_i] = (*input_)[(*indices_)[p_i]].getVector3fMap () - centroid; 00074 average_norm += scaled_points_[p_i].norm (); 00075 } 00076 average_norm /= double (scaled_points_.size ()); 00077 for (size_t p_i = 0; p_i < scaled_points_.size (); ++p_i) 00078 scaled_points_[p_i] /= float (average_norm); 00079 00080 return (true); 00081 } 00082 00083 /////////////////////////////////////////////////////////////////////////////// 00084 template<typename PointT, typename PointNT> double 00085 pcl::CovarianceSampling<PointT, PointNT>::computeConditionNumber () 00086 { 00087 Eigen::Matrix<double, 6, 6> covariance_matrix; 00088 if (!computeCovarianceMatrix (covariance_matrix)) 00089 return (-1.); 00090 00091 Eigen::EigenSolver<Eigen::Matrix<double, 6, 6> > eigen_solver; 00092 eigen_solver.compute (covariance_matrix, true); 00093 00094 Eigen::MatrixXcd complex_eigenvalues = eigen_solver.eigenvalues (); 00095 00096 double max_ev = std::numeric_limits<double>::min (), 00097 min_ev = std::numeric_limits<double>::max (); 00098 for (size_t i = 0; i < 6; ++i) 00099 { 00100 if (real (complex_eigenvalues (i, 0)) > max_ev) 00101 max_ev = real (complex_eigenvalues (i, 0)); 00102 00103 if (real (complex_eigenvalues (i, 0)) < min_ev) 00104 min_ev = real (complex_eigenvalues (i, 0)); 00105 } 00106 00107 return (max_ev / min_ev); 00108 } 00109 00110 00111 /////////////////////////////////////////////////////////////////////////////// 00112 template<typename PointT, typename PointNT> double 00113 pcl::CovarianceSampling<PointT, PointNT>::computeConditionNumber (const Eigen::Matrix<double, 6, 6> &covariance_matrix) 00114 { 00115 Eigen::EigenSolver<Eigen::Matrix<double, 6, 6> > eigen_solver; 00116 eigen_solver.compute (covariance_matrix, true); 00117 00118 Eigen::MatrixXcd complex_eigenvalues = eigen_solver.eigenvalues (); 00119 00120 double max_ev = std::numeric_limits<double>::min (), 00121 min_ev = std::numeric_limits<double>::max (); 00122 for (size_t i = 0; i < 6; ++i) 00123 { 00124 if (real (complex_eigenvalues (i, 0)) > max_ev) 00125 max_ev = real (complex_eigenvalues (i, 0)); 00126 00127 if (real (complex_eigenvalues (i, 0)) < min_ev) 00128 min_ev = real (complex_eigenvalues (i, 0)); 00129 } 00130 00131 return (max_ev / min_ev); 00132 } 00133 00134 00135 /////////////////////////////////////////////////////////////////////////////// 00136 template<typename PointT, typename PointNT> bool 00137 pcl::CovarianceSampling<PointT, PointNT>::computeCovarianceMatrix (Eigen::Matrix<double, 6, 6> &covariance_matrix) 00138 { 00139 if (!initCompute ()) 00140 return false; 00141 00142 //--- Part A from the paper 00143 // Set up matrix F 00144 Eigen::Matrix<double, 6, Eigen::Dynamic> f_mat = Eigen::Matrix<double, 6, Eigen::Dynamic> (6, indices_->size ()); 00145 for (size_t p_i = 0; p_i < scaled_points_.size (); ++p_i) 00146 { 00147 f_mat.block<3, 1> (0, p_i) = scaled_points_[p_i].cross ( 00148 (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ()).template cast<double> (); 00149 f_mat.block<3, 1> (3, p_i) = (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ().template cast<double> (); 00150 } 00151 00152 // Compute the covariance matrix C and its 6 eigenvectors (initially complex, move them to a double matrix) 00153 covariance_matrix = f_mat * f_mat.transpose (); 00154 return true; 00155 } 00156 00157 /////////////////////////////////////////////////////////////////////////////// 00158 template<typename PointT, typename PointNT> void 00159 pcl::CovarianceSampling<PointT, PointNT>::applyFilter (std::vector<int> &sampled_indices) 00160 { 00161 if (!initCompute ()) 00162 return; 00163 00164 //--- Part A from the paper 00165 // Set up matrix F 00166 Eigen::Matrix<double, 6, Eigen::Dynamic> f_mat = Eigen::Matrix<double, 6, Eigen::Dynamic> (6, indices_->size ()); 00167 for (size_t p_i = 0; p_i < scaled_points_.size (); ++p_i) 00168 { 00169 f_mat.block<3, 1> (0, p_i) = scaled_points_[p_i].cross ( 00170 (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ()).template cast<double> (); 00171 f_mat.block<3, 1> (3, p_i) = (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ().template cast<double> (); 00172 } 00173 00174 // Compute the covariance matrix C and its 6 eigenvectors (initially complex, move them to a double matrix) 00175 Eigen::Matrix<double, 6, 6> c_mat (f_mat * f_mat.transpose ()); 00176 00177 Eigen::EigenSolver<Eigen::Matrix<double, 6, 6> > eigen_solver; 00178 eigen_solver.compute (c_mat, true); 00179 Eigen::MatrixXcd complex_eigenvectors = eigen_solver.eigenvectors (); 00180 00181 Eigen::Matrix<double, 6, 6> x; 00182 for (size_t i = 0; i < 6; ++i) 00183 for (size_t j = 0; j < 6; ++j) 00184 x (i, j) = real (complex_eigenvectors (i, j)); 00185 00186 00187 //--- Part B from the paper 00188 /// TODO figure out how to fill the candidate_indices - see subsequent paper paragraphs 00189 std::vector<size_t> candidate_indices; 00190 candidate_indices.resize (indices_->size ()); 00191 for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i) 00192 candidate_indices[p_i] = p_i; 00193 00194 // Compute the v 6-vectors 00195 typedef Eigen::Matrix<double, 6, 1> Vector6d; 00196 std::vector<Vector6d, Eigen::aligned_allocator<Vector6d> > v; 00197 v.resize (candidate_indices.size ()); 00198 for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i) 00199 { 00200 v[p_i].block<3, 1> (0, 0) = scaled_points_[p_i].cross ( 00201 (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ()).template cast<double> (); 00202 v[p_i].block<3, 1> (3, 0) = (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ().template cast<double> (); 00203 } 00204 00205 00206 // Set up the lists to be sorted 00207 std::vector<std::list<std::pair<int, double> > > L; 00208 L.resize (6); 00209 00210 for (size_t i = 0; i < 6; ++i) 00211 { 00212 for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i) 00213 L[i].push_back (std::make_pair (p_i, fabs (v[p_i].dot (x.block<6, 1> (0, i))))); 00214 00215 // Sort in decreasing order 00216 L[i].sort (sort_dot_list_function); 00217 } 00218 00219 // Initialize the 6 t's 00220 std::vector<double> t (6, 0.0); 00221 00222 sampled_indices.resize (num_samples_); 00223 std::vector<bool> point_sampled (candidate_indices.size (), false); 00224 // Now select the actual points 00225 for (size_t sample_i = 0; sample_i < num_samples_; ++sample_i) 00226 { 00227 // Find the most unconstrained dimension, i.e., the minimum t 00228 size_t min_t_i = 0; 00229 for (size_t i = 0; i < 6; ++i) 00230 { 00231 if (t[min_t_i] > t[i]) 00232 min_t_i = i; 00233 } 00234 00235 // Add the point from the top of the list corresponding to the dimension to the set of samples 00236 while (point_sampled [L[min_t_i].front ().first]) 00237 L[min_t_i].pop_front (); 00238 00239 sampled_indices[sample_i] = L[min_t_i].front ().first; 00240 point_sampled[L[min_t_i].front ().first] = true; 00241 L[min_t_i].pop_front (); 00242 00243 // Update the running totals 00244 for (size_t i = 0; i < 6; ++i) 00245 { 00246 double val = v[sampled_indices[sample_i]].dot (x.block<6, 1> (0, i)); 00247 t[i] += val * val; 00248 } 00249 } 00250 00251 // Remap the sampled_indices to the input_ cloud 00252 for (size_t i = 0; i < sampled_indices.size (); ++i) 00253 sampled_indices[i] = (*indices_)[candidate_indices[sampled_indices[i]]]; 00254 } 00255 00256 00257 /////////////////////////////////////////////////////////////////////////////// 00258 template<typename PointT, typename PointNT> void 00259 pcl::CovarianceSampling<PointT, PointNT>::applyFilter (Cloud &output) 00260 { 00261 std::vector<int> sampled_indices; 00262 applyFilter (sampled_indices); 00263 00264 output.resize (sampled_indices.size ()); 00265 output.header = input_->header; 00266 output.height = 1; 00267 output.width = uint32_t (output.size ()); 00268 output.is_dense = true; 00269 for (size_t i = 0; i < sampled_indices.size (); ++i) 00270 output[i] = (*input_)[sampled_indices[i]]; 00271 } 00272 00273 00274 #define PCL_INSTANTIATE_CovarianceSampling(T,NT) template class PCL_EXPORTS pcl::CovarianceSampling<T,NT>; 00275 00276 #endif /* PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_ */