估计某个点的法向量,可以类似于 点云的曲面法向量估计,将该点附近K近邻的点近似在一个局部平面上,之后就通过最小二乘法拟合该平面方程,通过高等数学空间解析几何知识可知,若平面方程为A*x + B*y + C*z + D = 0,则平面法向量就是(A, B, C);从而求出法向量。
这里将平面方程转化为z = A*x + B*y + C,最小二乘法推导过程如下:
汗!图片手机照的,没带数据线,通过QQ传到网上后,图片质量降低!回去再替换。
根据以上推导出的最终结果,基于PCL库和Eigen库编程实现法向量估计:
原PCL库中法向量估计相关类为NormalEstimation,这里往PCL源码pcl_features工程下添加normal_esti_leastsquare.h、normal_esti_leastsquare.hpp、normal_esti_leastsquare.cpp文件,相关类名NormalEstimation2。
主要代码如下所示:
- for (size_t idx = 0; idx < indices_->size (); ++idx)
- {
- if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
- {
- 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;
- }
- computePointNormal (*surface_, nn_indices,
- output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);
- //flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
- // output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
- }
本文的最小二乘法估计法向量,主要在computePointNormal()实现:
- /** \brief 计算点的法向量,曲率暂时不求
- * \param[in] 输入点云
- * \param[in] 给定点的索引下标
- * \param[out] 法向量x分量
- * \param[out] 法向量y分量
- * \param[out] 法向量z分量
- * \param[out] 曲率
- */
- inline void
- computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, float &nx, float &ny, float &nz, float &curvature)
- {
- u_matrix_.resize(k_ , 3);
- Eigen::VectorXf z_vec(k_);
- for (size_t i = 0, ind_num = indices.size(); i < ind_num; ++i)
- {
- size_t indice = indices[i];
- u_matrix_(i, 0) = cloud.points[indice].x;
- u_matrix_(i, 1) = cloud.points[indice].y;
- u_matrix_(i, 2) = 1;
- z_vec[i] = cloud.points[indice].z;
- }
- EIGEN_ALIGN16 Eigen::Matrix3f utu_matrix = u_matrix_.transpose() * u_matrix_;
- EIGEN_ALIGN16 Eigen::Vector3f solve_vec = utu_matrix.inverse() * u_matrix_.transpose() * z_vec;
- EIGEN_ALIGN16 Eigen::Vector3f normal_vec(solve_vec[0], solve_vec[1], -1);
- normal_vec /= normal_vec.squaredNorm(); //归一化为单位向量
- nx = normal_vec[0];
- ny = normal_vec[1];
- nz = normal_vec[2];
- }
编写完以上代码后,编译PCL源码,生成新的pcl_features_debug.lib和pcl_features_debug.dll库;然后编写测试程序,并引用pcl_features_debug.lib和pcl_features_debug.dll库。
这里构造一个平面方程为x + y + z = 1的平面:
- #include <iostream>
- #include <pcl/point_types.h>
- #include <pcl/io/pcd_io.h>
- #include <pcl/features/normal_esti_leastsquare.h>
- using namespace std;
- int main(int argc, char* argv[])
- {
- pcl::PointCloud<pcl::PointXYZ>::Ptr inCloud(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
- for (float x = -5.0; x <= 5.0; x += 0.25)
- {
- for (float y = -5.0; y <= 5.0; y += 0.25)
- {
- pcl::PointXYZ cloud;
- cloud.x = x;
- cloud.y = y;
- cloud.z = 1 - x - y;
- inCloud->push_back(cloud);
- }
- }
- tree.reset (new pcl::search::KdTree<pcl::PointXYZ> (false));
- tree->setInputCloud (inCloud);
- // Normal estimation
- pcl::NormalEstimation2<pcl::PointXYZ, pcl::Normal> ne2;
- pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal> ());
- ne2.setInputCloud (inCloud);
- ne2.setSearchMethod (tree);
- ne2.setKSearch (20);
- ne2.compute (*normals);
- pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
- pcl::concatenateFields(*inCloud, *normals, *cloud_with_normals);
- pcl::io::savePCDFile("plane_cloud_out.pcd", *cloud_with_normals);
- }
- # .PCD v0.7 - Point Cloud Data file format
- VERSION 0.7
- FIELDS x y z intensity normal_x normal_y normal_z curvature
- SIZE 4 4 4 4 4 4 4 4
- TYPE F F F F F F F F
- COUNT 1 1 1 1 1 1 1 1
- WIDTH 1681
- HEIGHT 1
- VIEWPOINT 0 0 0 1 0 0 0
- POINTS 1681
- DATA ascii
- -5 -5 11 0 -0.33333454 -0.3333388 -0.33334672 -1.0737418e+008
- -5 -4.75 10.75 0 -0.33333459 -0.33333915 -0.33334744 -1.0737418e+008
- -5 -4.5 10.5 0 -0.33333349 -0.33333221 -0.33333147 -1.0737418e+008
- -5 -4.25 10.25 0 -0.33333686 -0.33333236 -0.33333844 -1.0737418e+008
- -5 -4 10 0 -0.33332857 -0.33334196 -0.33334109 -1.0737418e+008
- -5 -3.75 9.75 0 -0.33333248 -0.33333272 -0.33333042 -1.0737418e+008
- -5 -3.5 9.5 0 -0.33333799 -0.33333102 -0.33333802 -1.0737418e+008
- -5 -3.25 9.25 0 -0.33333382 -0.33333775 -0.33334321 -1.0737418e+008
- -5 -3 9 0 -0.33334044 -0.33332446 -0.3333298 -1.0737418e+008
- -5 -2.75 8.75 0 -0.33333328 -0.33333099 -0.3333286 -1.0737418e+008
- -5 -2.5 8.5 0 -0.33333698 -0.33332804 -0.33333004 -1.0737418e+008
- -5 -2.25 8.25 0 -0.33332828 -0.33334482 -0.33334616 -1.0737418e+008
- -5 -2 8 0 -0.33332947 -0.33334234 -0.33334357 -1.0737418e+008
- -5 -1.75 7.75 0 -0.33333299 -0.3333362 -0.33333835 -1.0737418e+008
- -5 -1.5 7.5 0 -0.33333793 -0.33332565 -0.33332711 -1.0737418e+008
- -5 -1.25 7.25 0 -0.33333275 -0.33333525 -0.333336 -1.0737418e+008
- -5 -1 7 0 -0.33333316 -0.33333406 -0.33333445 -1.0737418e+008
- -5 -0.75 6.75 0 -0.33333361 -0.33333287 -0.3333329 -1.0737418e+008
- -5 -0.5 6.5 0 -0.33333489 -0.33332995 -0.33332968 -1.0737418e+008
- ...
- ...
- ...
尽管上述结果不错,但是如果修改下平面点云的构造,
- //减小步长,由0.25减到0.1
- for (float x = -5.0; x <= 5.0; x += 0.1)
- {
- for (float y = -5.0; y <= 5.0; y += 0.1)
- {
- pcl::PointXYZ cloud;
- cloud.x = x;
- cloud.y = y;
- cloud.z = 1 - x - y;
- inCloud->push_back(cloud);
- }
- }
- # .PCD v0.7 - Point Cloud Data file format
- VERSION 0.7
- FIELDS x y z intensity normal_x normal_y normal_z curvature
- SIZE 4 4 4 4 4 4 4 4
- TYPE F F F F F F F F
- COUNT 1 1 1 1 1 1 1 1
- WIDTH 10201
- HEIGHT 1
- VIEWPOINT 0 0 0 1 0 0 0
- POINTS 10201
- DATA ascii
- -5 -5 11 0 -0.36251435 -0.33222172 -0.40937933 -1.0737418e+008
- -5 -4.9000001 10.9 0 -0.2965593 -0.36960894 -0.34049508 -1.0737418e+008
- -5 -4.8000002 10.8 0 -0.30257833 -0.32366723 -0.26829782 -1.0737418e+008
- -5 -4.7000003 10.700001 0 -0.32577419 -0.35322508 -0.36178556 -1.0737418e+008
- -5 -4.6000004 10.6 0 -0.34574831 -0.31297931 -0.31971669 -1.0737418e+008
- -5 -4.5000005 10.5 0 -0.3243891 -0.34880957 -0.34800133 -1.0737418e+008
- -5 -4.4000006 10.400001 0 -0.31732634 -0.34978729 -0.33582667 -1.0737418e+008
- -5 -4.3000007 10.300001 0 -0.32080838 -0.33909816 -0.32085085 -1.0737418e+008
- -5 -4.2000008 10.200001 0 -0.3384032 -0.33768722 -0.35353974 -1.0737418e+008
- -5 -4.1000009 10.1 0 -0.34146273 -0.31813207 -0.32056987 -1.0737418e+008
- -5 -4.000001 10.000001 0 -0.3408044 -0.32376921 -0.32963032 -1.0737418e+008
- -5 -3.900001 9.9000015 0 -0.29470387 -0.359045 -0.31496942 -1.0737418e+008
- -5 -3.8000011 9.8000011 0 -0.35997671 -0.31077951 -0.34562108 -1.0737418e+008
- -5 -3.7000012 9.7000008 0 -0.32181746 -0.33056432 -0.30722877 -1.0737418e+008
- -5 -3.6000013 9.6000013 0 -0.32655463 -0.3454251 -0.34494016 -1.0737418e+008
- -5 -3.5000014 9.5000019 0 -0.35073376 -0.32577616 -0.35558489 -1.0737418e+008
- -5 -3.4000015 9.4000015 0 -0.34808326 -0.32221347 -0.3418338 -1.0737418e+008
- -5 -3.3000016 9.3000011 0 -0.3261568 -0.33644432 -0.32556668 -1.0737418e+008
- ...
- ...
- ...
各行向量非常近似于平行,导致行列式为0,无法求得正确的逆矩阵,而以上计算过程中是用到了逆矩阵;
【2014-03-16】今天偶然看到一篇文章介绍最小二乘法的缺陷,http://www.cnblogs.com/jerrylead/archive/2011/08/21/2148625.html
如果样例数m相比特征数n少(m<n)或者特征间线性相关时,由于(n*n矩阵)的秩小于特征个数(即
不可逆)。因此最小二乘法
就会失效。
可以看出来,样本矩阵的x、y、z三个列向量近乎于共线,当然也近乎于线性相关,如果真是这样,那么这里最小二乘法并不适用!
真实原因有哪位朋友知道的话望告知!
虽然点云密集时,计算出的法向量不够准,但经过实践发现,增加点K近邻的样本个数(即增加K邻近算法中的K值,本例中为k_),计算出的法向量是可以近似共线于(1,1,1)。
个人分析,这里增加点近邻的样本个数,实际上也是减小样本矩阵的x、y、z三个列向量的共线程度,所以增加点近邻的样本个数后,就可以使用最小二乘法了。
对于稀疏点云,以上拟合方式大多数情况下是可以的,但若平面与Z轴平行,则不可行,因为此时平面方程是无法转化为z = A*x + B*y + C形式。下面针对平面方程最原始的一般式进行推导,过程如下:
注意,这里U^T*U是一个4*4的方阵,求齐次线性方程组的非零解,问题可以转换为求一个系数为3*3的方阵对应的齐次线性方程组,
类似地,可以将以上结论延伸至系数矩阵为4*4方阵对应的齐次线性方程组:
由此得到齐次线性方程组的非零解。
采用该方法,编程实现:
- /** \brief 计算点的法向量,曲率暂时不求
- * \param[in] 输入点云
- * \param[in] 给定点的索引下标
- * \param[out] 法向量x分量
- * \param[out] 法向量y分量
- * \param[out] 法向量z分量
- * \param[out] 曲率
- */
- inline void
- computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, float &nx, float &ny, float &nz, float &curvature)
- {
- Eigen::MatrixX4f mat_ext(k_, 4);
- for (size_t i = 0, ind_num = indices.size(); i < ind_num; ++i)
- {
- size_t indice = indices[i];
- mat_ext(i, 0) = cloud.points[indice].x;
- mat_ext(i, 1) = cloud.points[indice].y;
- mat_ext(i, 2) = cloud.points[indice].z;
- mat_ext(i, 3) = 1;
- }
- Eigen::MatrixXf mat_mult_ext = mat_ext.transpose() * mat_ext; //mat是3*k_矩阵,mat_ext是k_*4矩阵
- // A*x = b, A.Row(0).cross(A.Row(1)) = eigenvector(0);
- Eigen::Vector4f row_vec[3];
- row_vec[0] = mat_mult_ext.row(0);
- row_vec[1] = mat_mult_ext.row(1);
- row_vec[2] = mat_mult_ext.row(2);
- Eigen::Matrix3f i_cofactor, j_cofactor, k_cofactor, l_cofactor; //计算叉积时,i、j、k、l分量的代数余子式
- for (size_t row_index = 0; row_index < 3; ++row_index)
- {
- for (size_t col_index = 0; col_index < 3; ++col_index)
- {
- i_cofactor(row_index , col_index) = row_vec[row_index][col_index + 1];
- if (col_index < 1)
- {
- j_cofactor(row_index , col_index) = row_vec[row_index][col_index];
- }
- else
- {
- j_cofactor(row_index , col_index) = row_vec[row_index][col_index + 1];
- }
- if (col_index < 2)
- {
- k_cofactor(row_index , col_index) = row_vec[row_index][col_index];
- }
- else
- {
- k_cofactor(row_index , col_index) = row_vec[row_index][col_index + 1];
- }
- l_cofactor(row_index , col_index) = row_vec[row_index][col_index];
- }
- }
- float i_dim = 0, j_dim = 0, k_dim = 0, l_dim = 0;
- i_dim = i_cofactor.determinant();
- j_dim = j_cofactor.determinant();
- k_dim = k_cofactor.determinant();
- l_dim = l_cofactor.determinant();
- Eigen::Vector3f coeff_vec(i_dim, -j_dim, k_dim);
- float len = coeff_vec.squaredNorm();
- coeff_vec /= len;
- nx = coeff_vec[0];
- ny = coeff_vec[1];
- nz = coeff_vec[2];
- }
- # .PCD v0.7 - Point Cloud Data file format
- VERSION 0.7
- FIELDS x y z intensity normal_x normal_y normal_z curvature
- SIZE 4 4 4 4 4 4 4 4
- TYPE F F F F F F F F
- COUNT 1 1 1 1 1 1 1 1
- WIDTH 1681
- HEIGHT 1
- VIEWPOINT 0 0 0 1 0 0 0
- POINTS 1681
- DATA ascii
- -5 -5 11 0 0.0034180761 0.003418348 0.0033811682 -1.0737418e+008
- -5 -4.75 10.75 0 0.0034180761 0.003418348 0.0033811682 -1.0737418e+008
- -5 -4.5 10.5 0 0.0036986405 0.0036108212 0.0036840038 -1.0737418e+008
- -5 -4.25 10.25 0 0.0041074576 0.0039950516 0.0042172931 -1.0737418e+008
- -5 -4 10 0 0.0040342622 0.004034651 0.0041443072 -1.0737418e+008
- -5 -3.75 9.75 0 0.004033945 0.0041615977 0.0040161251 -1.0737418e+008
- -5 -3.5 9.5 0 0.0041075312 0.0040329853 0.0041812863 -1.0737418e+008
- -5 -3.25 9.25 0 0.0040594526 0.0040964941 0.0040962985 -1.0737418e+008
- -5 -3 9 0 0.0040834057 0.0040837992 0.0041210586 -1.0737418e+008
- -5 -2.75 8.75 0 0.0041877129 0.0040933029 0.0041687908 -1.0737418e+008
- -5 -2.5 8.5 0 0.0040593483 0.0040955977 0.0040955977 -1.0737418e+008
- -5 -2.25 8.25 0 0.0040110592 0.0041013826 0.0040649828 -1.0737418e+008
- -5 -2 8 0 0.0040473556 0.0040835626 0.0040469691 -1.0737418e+008
- -5 -1.75 7.75 0 0.0040717809 0.0040713926 0.0040715868 -1.0737418e+008
- -5 -1.5 7.5 0 0.004047934 0.0040837578 0.0040467749 -1.0737418e+008
- -5 -1.25 7.25 0 0.0040838025 0.004065339 0.0040841922 -1.0737418e+008
- -5 -1 7 0 0.0040716026 0.0040716026 0.0040712142 -1.0737418e+008
- -5 -0.75 6.75 0 0.0040599061 0.0040774848 0.0040595187 -1.0737418e+008
- -5 -0.5 6.5 0 0.0040714089 0.0040717972 0.0040717972 -1.0737418e+008
- -5 -0.25 6.25 0 0.0040713926 0.0040717809 0.0040715868 -1.0737418e+008
- -5 0 6 0 0.0040716026 0.0040712142 0.0040716026 -1.0737418e+008
- -5 0.25 5.75 0 0.0040713926 0.0040715868 0.0040717809 -1.0737418e+008
- -5 0.5 5.5 0 0.0040714089 0.0040717972 0.0040717972 -1.0737418e+008
- ...
- ...
- ...
可以看到点云中每个点对应的法向量也都近似共线于(1, 1, 1)。与之前解非其次线性方程组的方法类似,当点云数据比较密集(点与点间距离很小)时,计算出来的结果也是很不准的;同样地,增加点K近邻的样本个数(即增加K邻近算法中的K值,本例中为k_),计算出的法向量是可以近似共线于(1,1,1)。个人推测的原因见文章之前部分。
还是那句话,真实原因有哪位朋友知道的话望告知!
完整代码可以下载http://download.csdn.net/detail/lming_08/7035195。