|
Point Cloud Library (PCL)
1.6.0
|
|
Point Cloud Library (PCL)
1.6.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2009-2011, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id: mls.h 6439 2012-07-17 07:13:58Z aichim $ 00037 * 00038 */ 00039 00040 #ifndef PCL_MLS_H_ 00041 #define PCL_MLS_H_ 00042 00043 // PCL includes 00044 #include <pcl/pcl_base.h> 00045 #include <boost/bind.hpp> 00046 #include <boost/function.hpp> 00047 #include <boost/random.hpp> 00048 #include <pcl/search/pcl_search.h> 00049 #include <pcl/common/common.h> 00050 #include <pcl/surface/processing.h> 00051 00052 #include <Eigen/SVD> 00053 00054 namespace pcl 00055 { 00065 template <typename PointInT, typename PointOutT> 00066 class MovingLeastSquares: public CloudSurfaceProcessing<PointInT, PointOutT> 00067 { 00068 public: 00069 using PCLBase<PointInT>::input_; 00070 using PCLBase<PointInT>::indices_; 00071 using PCLBase<PointInT>::fake_indices_; 00072 using PCLBase<PointInT>::initCompute; 00073 using PCLBase<PointInT>::deinitCompute; 00074 00075 typedef typename pcl::search::Search<PointInT> KdTree; 00076 typedef typename pcl::search::Search<PointInT>::Ptr KdTreePtr; 00077 typedef pcl::PointCloud<pcl::Normal> NormalCloud; 00078 typedef pcl::PointCloud<pcl::Normal>::Ptr NormalCloudPtr; 00079 00080 typedef pcl::PointCloud<PointOutT> PointCloudOut; 00081 typedef typename PointCloudOut::Ptr PointCloudOutPtr; 00082 typedef typename PointCloudOut::ConstPtr PointCloudOutConstPtr; 00083 00084 typedef pcl::PointCloud<PointInT> PointCloudIn; 00085 typedef typename PointCloudIn::Ptr PointCloudInPtr; 00086 typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr; 00087 00088 typedef boost::function<int (int, double, std::vector<int> &, std::vector<float> &)> SearchMethod; 00089 00090 enum UpsamplingMethod { NONE, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION }; 00091 00093 MovingLeastSquares () : CloudSurfaceProcessing<PointInT, PointOutT> (), 00094 normals_ (), 00095 search_method_ (), 00096 tree_ (), 00097 order_ (2), 00098 polynomial_fit_ (true), 00099 search_radius_ (0.0), 00100 sqr_gauss_param_ (0.0), 00101 compute_normals_ (false), 00102 upsample_method_ (NONE), 00103 upsampling_radius_ (0.0), 00104 upsampling_step_ (0.0), 00105 rng_uniform_distribution_ (), 00106 desired_num_points_in_radius_ (0), 00107 mls_results_ (), 00108 voxel_size_ (1.0), 00109 dilation_iteration_num_ (0), 00110 nr_coeff_ () 00111 {}; 00112 00113 00117 inline void 00118 setComputeNormals (bool compute_normals) { compute_normals_ = compute_normals; } 00119 00123 inline void 00124 setSearchMethod (const KdTreePtr &tree) 00125 { 00126 tree_ = tree; 00127 // Declare the search locator definition 00128 int (KdTree::*radiusSearch)(int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances, unsigned int max_nn) const = &KdTree::radiusSearch; 00129 search_method_ = boost::bind (radiusSearch, boost::ref (tree_), _1, _2, _3, _4, 0); 00130 } 00131 00133 inline KdTreePtr 00134 getSearchMethod () { return (tree_); } 00135 00139 inline void 00140 setPolynomialOrder (int order) { order_ = order; } 00141 00143 inline int 00144 getPolynomialOrder () { return (order_); } 00145 00149 inline void 00150 setPolynomialFit (bool polynomial_fit) { polynomial_fit_ = polynomial_fit; } 00151 00153 inline bool 00154 getPolynomialFit () { return (polynomial_fit_); } 00155 00160 inline void 00161 setSearchRadius (double radius) { search_radius_ = radius; sqr_gauss_param_ = search_radius_ * search_radius_; } 00162 00164 inline double 00165 getSearchRadius () { return (search_radius_); } 00166 00171 inline void 00172 setSqrGaussParam (double sqr_gauss_param) { sqr_gauss_param_ = sqr_gauss_param; } 00173 00175 inline double 00176 getSqrGaussParam () const { return (sqr_gauss_param_); } 00177 00194 inline void 00195 setUpsamplingMethod (UpsamplingMethod method) { upsample_method_ = method; } 00196 00197 00202 inline void 00203 setUpsamplingRadius (double radius) { upsampling_radius_ = radius; } 00204 00208 inline double 00209 getUpsamplingRadius () { return upsampling_radius_; } 00210 00215 inline void 00216 setUpsamplingStepSize (double step_size) { upsampling_step_ = step_size; } 00217 00218 00222 inline double 00223 getUpsamplingStepSize () { return upsampling_step_; } 00224 00230 inline void 00231 setPointDensity (int desired_num_points_in_radius) { desired_num_points_in_radius_ = desired_num_points_in_radius; } 00232 00233 00237 inline int 00238 getPointDensity () { return desired_num_points_in_radius_; } 00239 00244 inline void 00245 setDilationVoxelSize (float voxel_size) { voxel_size_ = voxel_size; } 00246 00247 00251 inline float 00252 getDilationVoxelSize () { return voxel_size_; } 00253 00258 inline void 00259 setDilationIterations (int iterations) { dilation_iteration_num_ = iterations; } 00260 00264 inline int 00265 getDilationIterations () { return dilation_iteration_num_; } 00266 00270 void 00271 process (PointCloudOut &output); 00272 00273 protected: 00275 NormalCloudPtr normals_; 00276 00278 SearchMethod search_method_; 00279 00281 KdTreePtr tree_; 00282 00284 int order_; 00285 00287 bool polynomial_fit_; 00288 00290 double search_radius_; 00291 00293 double sqr_gauss_param_; 00294 00296 bool compute_normals_; 00297 00299 UpsamplingMethod upsample_method_; 00300 00304 double upsampling_radius_; 00305 00309 double upsampling_step_; 00310 00314 boost::variate_generator<boost::mt19937, boost::uniform_real<float> > *rng_uniform_distribution_; 00315 00319 int desired_num_points_in_radius_; 00320 00321 00325 struct MLSResult 00326 { 00327 MLSResult () : plane_normal (), u (), v (), c_vec (), num_neighbors (), curvature (), valid (false) {} 00328 00329 MLSResult (Eigen::Vector3d &a_plane_normal, 00330 Eigen::Vector3d &a_u, 00331 Eigen::Vector3d &a_v, 00332 Eigen::VectorXd a_c_vec, 00333 int a_num_neighbors, 00334 float &a_curvature); 00335 00336 Eigen::Vector3d plane_normal, u, v; 00337 Eigen::VectorXd c_vec; 00338 int num_neighbors; 00339 float curvature; 00340 bool valid; 00341 }; 00342 00346 std::vector<MLSResult> mls_results_; 00347 00348 00352 class MLSVoxelGrid 00353 { 00354 public: 00355 struct Leaf { Leaf () : valid (true) {} bool valid; }; 00356 00357 MLSVoxelGrid (PointCloudInConstPtr& cloud, 00358 IndicesPtr &indices, 00359 float voxel_size); 00360 00361 void 00362 dilate (); 00363 00364 inline void 00365 getIndexIn1D (const Eigen::Vector3i &index, uint64_t &index_1d) const 00366 { 00367 index_1d = index[0] * data_size_ * data_size_ + 00368 index[1] * data_size_ + index[2]; 00369 } 00370 00371 inline void 00372 getIndexIn3D (uint64_t index_1d, Eigen::Vector3i& index_3d) const 00373 { 00374 index_3d[0] = static_cast<Eigen::Vector3i::Scalar> (index_1d / (data_size_ * data_size_)); 00375 index_1d -= index_3d[0] * data_size_ * data_size_; 00376 index_3d[1] = static_cast<Eigen::Vector3i::Scalar> (index_1d / data_size_); 00377 index_1d -= index_3d[1] * data_size_; 00378 index_3d[2] = static_cast<Eigen::Vector3i::Scalar> (index_1d); 00379 } 00380 00381 inline void 00382 getCellIndex (const Eigen::Vector3f &p, Eigen::Vector3i& index) const 00383 { 00384 for (int i = 0; i < 3; ++i) 00385 index[i] = static_cast<Eigen::Vector3i::Scalar> ((p[i] - bounding_min_(i)) / voxel_size_); 00386 } 00387 00388 inline void 00389 getPosition (const uint64_t &index_1d, Eigen::Vector3f &point) const 00390 { 00391 Eigen::Vector3i index_3d; 00392 getIndexIn3D (index_1d, index_3d); 00393 for (int i = 0; i < 3; ++i) 00394 point[i] = static_cast<Eigen::Vector3f::Scalar> (index_3d[i]) * voxel_size_ + bounding_min_[i]; 00395 } 00396 00397 typedef std::map<uint64_t, Leaf> HashMap; 00398 HashMap voxel_grid_; 00399 Eigen::Vector4f bounding_min_, bounding_max_; 00400 uint64_t data_size_; 00401 float voxel_size_; 00402 }; 00403 00404 00406 float voxel_size_; 00407 00409 int dilation_iteration_num_; 00410 00412 int nr_coeff_; 00413 00419 inline int 00420 searchForNeighbors (int index, std::vector<int> &indices, std::vector<float> &sqr_distances) 00421 { 00422 return (search_method_ (index, search_radius_, indices, sqr_distances)); 00423 } 00424 00435 void 00436 computeMLSPointNormal (int index, 00437 const PointCloudIn &input, 00438 const std::vector<int> &nn_indices, 00439 std::vector<float> &nn_sqr_dists, 00440 PointCloudOut &projected_points, 00441 NormalCloud &projected_points_normals); 00442 00457 void 00458 projectPointToMLSSurface (float &u_disp, float &v_disp, 00459 Eigen::Vector3d &u, Eigen::Vector3d &v, 00460 Eigen::Vector3d &plane_normal, 00461 float &curvature, 00462 Eigen::Vector3f &query_point, 00463 Eigen::VectorXd &c_vec, 00464 int num_neighbors, 00465 PointOutT &result_point, 00466 pcl::Normal &result_normal); 00467 00468 private: 00472 virtual void performProcessing (PointCloudOut &output); 00473 00475 std::string getClassName () const { return ("MovingLeastSquares"); } 00476 00477 public: 00478 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00479 }; 00480 } 00481 00482 #endif /* #ifndef PCL_MLS_H_ */
1.7.6.1
