[PCL]2 点云法向量计算NormalEstimation
2016-01-22 13:55
746 查看
从GitHub的代码版本库下载源代码https://github.com/PointCloudLibrary/pcl,用CMake生成VS项目,查看PCL的源码位于pcl_features项目下
1.Feature类:
[align=left]template <typename PointInT, typename PointOutT> class Feature : public PCLBase<PointInT>[/align]
注意 Feature是一个泛型类,有一个compute方法。
2.注意computeFeature (output);方法 ,可以知道这是一个私有的虚方法。
[align=left] private:[/align]
[align=left] /** \brief Abstract feature estimation method.[/align]
[align=left] * \param[out] output the resultant features */[/align]
[align=left] virtual void computeFeature (PointCloudOut &output) = 0;[/align]
[align=left]3.查看Feature的继承关系可以知道[/align]
[align=left]template <typename PointInT, typename PointOutT> class NormalEstimation: public Feature<PointInT, PointOutT>[/align]
[align=left]NormalEstimation类是Feature模板类的子类,因此执行的是NormalEstimation类的computeFeature方法。查看computeFeature方法:[/align]
4.因此分析NormalEstimation的算法流程如下:
[align=left] (1)进行点云的初始化initCompute[/align]
[align=left] (2)初始化计算结果输出对象output[/align]
[align=left] (3)计算点云法向量,具体由子类的computeFeature方法实现。先搜索近邻searchForNeighbors ,然后计算computePointNormal[/align]
[align=left] 采用的方法是PCA主成分分析法。[/align]
[align=left] 参考文献:《Semantic 3D Object Maps for Everyday Manipulation in Human Living Environments》 P45-49[/align]
[align=left] http://www.pclcn.org/study/shownews.php?lang=cn&id=105[/align]
[align=left] 点云的法向量主要是通过点所在区域的局部拟合的表面进行计算。平面通过一个点和法向量进行表示。对于每一个点Pi,对应的协方差矩阵C[/align]
[align=left] 关于主成份分析的基本原理和算法流程参考:http://blog.csdn.net/lming_08/article/details/21335313[/align]
[align=left] (4)flipNormalTowardsViewpoint 法向量定向,采用方法是:使法向量的方向朝向viewpoint。[/align]
[align=left]5.NormalEstimation模板类的重载方法computeFeature分析,computePointNormal分析。[/align]
6.法向量定向
见normal_3d.h文件中,有多个覆写方法。摘其一:
运行的实例结果:
1.Feature类:
[align=left]template <typename PointInT, typename PointOutT> class Feature : public PCLBase<PointInT>[/align]
注意 Feature是一个泛型类,有一个compute方法。
template <typename PointInT, typename PointOutT> void pcl::Feature<PointInT, PointOutT>::compute (PointCloudOut &output) { if (!initCompute ()) { output.width = output.height = 0; output.points.clear (); return; } // Copy the header output.header = input_->header; // Resize the output dataset if (output.points.size () != indices_->size ()) output.points.resize (indices_->size ()); // Check if the output will be computed for all points or only a subset // If the input width or height are not set, set output width as size if (indices_->size () != input_->points.size () || input_->width * input_->height == 0) { output.width = static_cast<uint32_t> (indices_->size ()); output.height = 1; } else { output.width = input_->width; output.height = input_->height; } output.is_dense = input_->is_dense; // Perform the actual feature computation computeFeature (output); deinitCompute (); }
2.注意computeFeature (output);方法 ,可以知道这是一个私有的虚方法。
[align=left] private:[/align]
[align=left] /** \brief Abstract feature estimation method.[/align]
[align=left] * \param[out] output the resultant features */[/align]
[align=left] virtual void computeFeature (PointCloudOut &output) = 0;[/align]
[align=left]3.查看Feature的继承关系可以知道[/align]
[align=left]template <typename PointInT, typename PointOutT> class NormalEstimation: public Feature<PointInT, PointOutT>[/align]
[align=left]NormalEstimation类是Feature模板类的子类,因此执行的是NormalEstimation类的computeFeature方法。查看computeFeature方法:[/align]
template <typename PointInT, typename PointOutT> void pcl::NormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output) { // Allocate enough space to hold the results // \note This resize is irrelevant for a radiusSearch (). std::vector< int> nn_indices (k_); std::vector< float> nn_dists (k_); output.is_dense = true; // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense if (input_->is_dense) { // Iterating over the entire index vector for (size_t idx = 0; idx < indices_->size (); ++idx) { if (this ->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 || !computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature)) { output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float >::quiet_NaN (); output.is_dense = false; continue; } flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]); } } else { // Iterating over the entire index vector for (size_t idx = 0; idx < indices_->size (); ++idx) { if (!isFinite ((*input_)[(*indices_)[idx]]) || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 || !computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature)) { output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float >::quiet_NaN (); output.is_dense = false; continue; } flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]); } } }
4.因此分析NormalEstimation的算法流程如下:
[align=left] (1)进行点云的初始化initCompute[/align]
[align=left] (2)初始化计算结果输出对象output[/align]
[align=left] (3)计算点云法向量,具体由子类的computeFeature方法实现。先搜索近邻searchForNeighbors ,然后计算computePointNormal[/align]
[align=left] 采用的方法是PCA主成分分析法。[/align]
[align=left] 参考文献:《Semantic 3D Object Maps for Everyday Manipulation in Human Living Environments》 P45-49[/align]
[align=left] http://www.pclcn.org/study/shownews.php?lang=cn&id=105[/align]
[align=left] 点云的法向量主要是通过点所在区域的局部拟合的表面进行计算。平面通过一个点和法向量进行表示。对于每一个点Pi,对应的协方差矩阵C[/align]
[align=left] 关于主成份分析的基本原理和算法流程参考:http://blog.csdn.net/lming_08/article/details/21335313[/align]
[align=left] (4)flipNormalTowardsViewpoint 法向量定向,采用方法是:使法向量的方向朝向viewpoint。[/align]
[align=left]5.NormalEstimation模板类的重载方法computeFeature分析,computePointNormal分析。[/align]
inline bool computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, float &nx, float &ny, float &nz, float &curvature) { if (indices.size () < 3 || computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0) { nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN (); return false; } // Get the plane normal and surface curvature solvePlaneParameters (covariance_matrix_, nx, ny, nz, curvature); return true; }
computeMeanAndCovarianceMatrix主要是PCA过程中计算平均值和协方差矩阵,在类centroid.hpp中。 而solvePlaneParameters方法则是为了求解特征值和特征向量。代码见feature.hpp。具体实现时采用了pcl::eigen33方法。
inline void pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, float &nx, float &ny, float &nz, float &curvature) { // Avoid getting hung on Eigen's optimizers // for (int i = 0; i < 9; ++i) // if (!pcl_isfinite (covariance_matrix.coeff (i))) // { // //PCL_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values!\n"); // nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN (); // return; // } // Extract the smallest eigenvalue and its eigenvector EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); nx = eigen_vector [0]; ny = eigen_vector [1]; nz = eigen_vector [2]; // Compute the curvature surface change float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8); if (eig_sum != 0) curvature = fabsf (eigen_value / eig_sum); else curvature = 0; }
6.法向量定向
见normal_3d.h文件中,有多个覆写方法。摘其一:
/** \brief Flip (in place) the estimated normal of a point towards a given viewpoint * \param point a given point * \param vp_x the X coordinate of the viewpoint * \param vp_y the X coordinate of the viewpoint * \param vp_z the X coordinate of the viewpoint * \param nx the resultant X component of the plane normal * \param ny the resultant Y component of the plane normal * \param nz the resultant Z component of the plane normal * \ingroup features */ template <typename PointT> inline void flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, float &nx, float &ny, float &nz) { // See if we need to flip any plane normals vp_x -= point.x; vp_y -= point.y; vp_z -= point.z; // Dot product between the (viewpoint - point) and the plane normal float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz); // Flip the plane normal if (cos_theta < 0) { nx *= -1; ny *= -1; nz *= -1; } }
运行的实例结果:
相关文章推荐
- 新手必备的常用 Android 代码片段整理(2)
- 在VS中添加lib库的三种方法
- Nova游戏官网项目笔记
- C++:多态性1(动态联编测试)
- 高级编程之文件I/O(一)
- 老毛桃发帖子 去广告
- HDU5090模拟,hash
- mongodb事务
- RESTClient
- 一曲君莫问,念君忆君伤断魂
- XMPP扩展协议详解
- DataOutputStream和DataInputStream
- MySQL执行计划初探
- jquery和jqueryMobile的js文件引入
- 这里面70%以上的我都知道。还是普及给那些不知道的人吧
- static关键字--java
- HTML5新特性及标签标记概要
- 常用三个网络协议
- Ext.data.SimpleStore的使用方法
- Git