38 #ifndef PCL_VOXEL_GRID_COVARIANCE_H_
39 #define PCL_VOXEL_GRID_COVARIANCE_H_
41 #include <pcl/filters/boost.h>
42 #include <pcl/filters/voxel_grid.h>
44 #include <pcl/point_types.h>
45 #include <pcl/kdtree/kdtree_flann.h>
56 template<
typename Po
intT>
87 typedef boost::shared_ptr< VoxelGrid<PointT> >
Ptr;
88 typedef boost::shared_ptr< const VoxelGrid<PointT> >
ConstPtr;
100 mean_ (Eigen::Vector3d::Zero ()),
102 cov_ (Eigen::Matrix3d::Identity ()),
103 icov_ (Eigen::Matrix3d::Zero ()),
104 evecs_ (Eigen::Matrix3d::Identity ()),
105 evals_ (Eigen::Vector3d::Zero ())
224 if(min_points_per_voxel > 2)
230 PCL_WARN (
"%s: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", this->
getClassName ().c_str ());
305 typename std::map<size_t, Leaf>::iterator leaf_iter =
leaves_.find (index);
306 if (leaf_iter !=
leaves_.end ())
331 typename std::map<size_t, Leaf>::iterator leaf_iter =
leaves_.find (idx);
332 if (leaf_iter !=
leaves_.end ())
351 int ijk1 =
static_cast<int> (floor (p[1] * inverse_leaf_size_[1]) -
min_b_[1]);
352 int ijk2 =
static_cast<int> (floor (p[2] * inverse_leaf_size_[2]) -
min_b_[2]);
358 typename std::map<size_t, Leaf>::iterator leaf_iter =
leaves_.find (idx);
359 if (leaf_iter !=
leaves_.end ())
382 inline const std::map<size_t, Leaf>&
415 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
422 PCL_WARN (
"%s: Not Searchable", this->
getClassName ().c_str ());
427 std::vector<int> k_indices;
431 k_leaves.reserve (k);
432 for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++)
449 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
451 if (index >= static_cast<int> (cloud.
points.size ()) || index < 0)
467 std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0)
474 PCL_WARN (
"%s: Not Searchable", this->
getClassName ().c_str ());
479 std::vector<int> k_indices;
483 k_leaves.reserve (k);
484 for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++)
502 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
503 unsigned int max_nn = 0)
505 if (index >= static_cast<int> (cloud.
points.size ()) || index < 0)
507 return (
radiusSearch (cloud.
points[index], radius, k_leaves, k_sqr_distances, max_nn));
540 #ifdef PCL_NO_PRECOMPILE
541 #include <pcl/filters/impl/voxel_grid_covariance.hpp>
544 #endif //#ifndef PCL_VOXEL_GRID_COVARIANCE_H_