基于最小二乘法估计点云的曲面法向量

之前对PCL库计算三维点云数据的曲面法向量有过介绍, 点云的曲面法向量估计,PCL库是采用主成份分析方法的,近几天通过理论推导发现最小二乘法应该也能计算曲面法向量。首先介绍下其理论知识。
估计某个点的法向量,可以类似于 点云的曲面法向量估计,将该点附近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。
主要代码如下所示:

[cpp]  view plain  copy
  1.               for (size_t idx = 0; idx < indices_->size (); ++idx)  
  2. {  
  3.     if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)  
  4.     {  
  5.         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 ();  
  6.   
  7.         output.is_dense = false;  
  8.         continue;  
  9.     }  
  10.   
  11.     computePointNormal (*surface_, nn_indices,  
  12.         output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);  
  13.   
  14.     //flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,  
  15.     //  output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);  
  16.   
  17. }  
上述代码与NormalEstimation类的对应代码相差无几,都是采用K近邻法获取每个点附近的点,然后基于这些点计算该点的法向量。
本文的最小二乘法估计法向量,主要在computePointNormal()实现:

[cpp]  view plain  copy
  1. /** \brief 计算点的法向量,曲率暂时不求 
  2. * \param[in] 输入点云 
  3. * \param[in] 给定点的索引下标  
  4. * \param[out] 法向量x分量  
  5. * \param[out] 法向量y分量   
  6. * \param[out] 法向量z分量   
  7. * \param[out] 曲率  
  8. */  
  9. inline void  
  10.     computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, float &nx, float &ny, float &nz, float &curvature)  
  11.     {  
  12.         u_matrix_.resize(k_ , 3);  
  13.   
  14.         Eigen::VectorXf z_vec(k_);  
  15.         for (size_t i = 0, ind_num = indices.size(); i < ind_num; ++i)  
  16.         {  
  17.             size_t indice = indices[i];  
  18.   
  19.             u_matrix_(i, 0) = cloud.points[indice].x;  
  20.             u_matrix_(i, 1) = cloud.points[indice].y;  
  21.             u_matrix_(i, 2) = 1;  
  22.   
  23.             z_vec[i] = cloud.points[indice].z;  
  24.         }  
  25.   
  26.         EIGEN_ALIGN16 Eigen::Matrix3f utu_matrix = u_matrix_.transpose() * u_matrix_;  
  27.         EIGEN_ALIGN16 Eigen::Vector3f solve_vec = utu_matrix.inverse() * u_matrix_.transpose() * z_vec;  
  28.         EIGEN_ALIGN16 Eigen::Vector3f normal_vec(solve_vec[0], solve_vec[1], -1);  
  29.             
  30.             normal_vec /= normal_vec.squaredNorm();    //归一化为单位向量  
  31.         nx = normal_vec[0];  
  32.         ny = normal_vec[1];  
  33.         nz = normal_vec[2];  
  34.     }  

编写完以上代码后,编译PCL源码,生成新的pcl_features_debug.lib和pcl_features_debug.dll库;然后编写测试程序,并引用pcl_features_debug.lib和pcl_features_debug.dll库。
这里构造一个平面方程为x + y + z = 1的平面:
[cpp]  view plain  copy
  1. #include <iostream>  
  2. #include <pcl/point_types.h>  
  3. #include <pcl/io/pcd_io.h>  
  4. #include <pcl/features/normal_esti_leastsquare.h>  
  5.   
  6. using namespace std;  
  7. int main(int argc, char* argv[])  
  8. {  
  9.         pcl::PointCloud<pcl::PointXYZ>::Ptr inCloud(new pcl::PointCloud<pcl::PointXYZ>);    
  10.     pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);  
  11.   
  12.     for (float x = -5.0; x <= 5.0; x += 0.25)    
  13.     {    
  14.         for (float y = -5.0; y <= 5.0; y += 0.25)    
  15.         {    
  16.             pcl::PointXYZ cloud;    
  17.   
  18.             cloud.x = x;    
  19.             cloud.y = y;    
  20.             cloud.z = 1 - x - y;    
  21.             inCloud->push_back(cloud);    
  22.         }    
  23.     }    
  24.   
  25.     tree.reset (new pcl::search::KdTree<pcl::PointXYZ> (false));  
  26.     tree->setInputCloud (inCloud);  
  27.   
  28.     // Normal estimation  
  29.     pcl::NormalEstimation2<pcl::PointXYZ, pcl::Normal> ne2;  
  30.     pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal> ());  
  31.     ne2.setInputCloud (inCloud);      
  32.     ne2.setSearchMethod (tree);  
  33.     ne2.setKSearch (20);  
  34.     ne2.compute (*normals);  
  35.   
  36.     pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);    
  37.     pcl::concatenateFields(*inCloud, *normals, *cloud_with_normals);    
  38.   
  39.     pcl::io::savePCDFile("plane_cloud_out.pcd", *cloud_with_normals);  
  40. }  
查看生成的plane_cloud_out.pcd文件内容,

[cpp]  view plain  copy
  1. # .PCD v0.7 - Point Cloud Data file format  
  2. VERSION 0.7  
  3. FIELDS x y z intensity normal_x normal_y normal_z curvature  
  4. SIZE 4 4 4 4 4 4 4 4  
  5. TYPE F F F F F F F F  
  6. COUNT 1 1 1 1 1 1 1 1  
  7. WIDTH 1681  
  8. HEIGHT 1  
  9. VIEWPOINT 0 0 0 1 0 0 0  
  10. POINTS 1681  
  11. DATA ascii  
  12. -5 -5 11 0 -0.33333454 -0.3333388 -0.33334672 -1.0737418e+008  
  13. -5 -4.75 10.75 0 -0.33333459 -0.33333915 -0.33334744 -1.0737418e+008  
  14. -5 -4.5 10.5 0 -0.33333349 -0.33333221 -0.33333147 -1.0737418e+008  
  15. -5 -4.25 10.25 0 -0.33333686 -0.33333236 -0.33333844 -1.0737418e+008  
  16. -5 -4 10 0 -0.33332857 -0.33334196 -0.33334109 -1.0737418e+008  
  17. -5 -3.75 9.75 0 -0.33333248 -0.33333272 -0.33333042 -1.0737418e+008  
  18. -5 -3.5 9.5 0 -0.33333799 -0.33333102 -0.33333802 -1.0737418e+008  
  19. -5 -3.25 9.25 0 -0.33333382 -0.33333775 -0.33334321 -1.0737418e+008  
  20. -5 -3 9 0 -0.33334044 -0.33332446 -0.3333298 -1.0737418e+008  
  21. -5 -2.75 8.75 0 -0.33333328 -0.33333099 -0.3333286 -1.0737418e+008  
  22. -5 -2.5 8.5 0 -0.33333698 -0.33332804 -0.33333004 -1.0737418e+008  
  23. -5 -2.25 8.25 0 -0.33332828 -0.33334482 -0.33334616 -1.0737418e+008  
  24. -5 -2 8 0 -0.33332947 -0.33334234 -0.33334357 -1.0737418e+008  
  25. -5 -1.75 7.75 0 -0.33333299 -0.3333362 -0.33333835 -1.0737418e+008  
  26. -5 -1.5 7.5 0 -0.33333793 -0.33332565 -0.33332711 -1.0737418e+008  
  27. -5 -1.25 7.25 0 -0.33333275 -0.33333525 -0.333336 -1.0737418e+008  
  28. -5 -1 7 0 -0.33333316 -0.33333406 -0.33333445 -1.0737418e+008  
  29. -5 -0.75 6.75 0 -0.33333361 -0.33333287 -0.3333329 -1.0737418e+008  
  30. -5 -0.5 6.5 0 -0.33333489 -0.33332995 -0.33332968 -1.0737418e+008  
  31.   
  32. ...  
  33. ...  
  34. ...  
可以看出各点的法向量近似与(1, 1, 1)共线,这与从平面方程x + y + z = 1中得出的平面法向量保持一致!可以看出计算效果非常理想。
尽管上述结果不错,但是如果修改下平面点云的构造,

[cpp]  view plain  copy
  1.        //减小步长,由0.25减到0.1  
  2.        for (float x = -5.0; x <= 5.0; x += 0.1)    
  3. {    
  4.     for (float y = -5.0; y <= 5.0; y += 0.1)    
  5.     {    
  6.         pcl::PointXYZ cloud;    
  7.   
  8.         cloud.x = x;    
  9.         cloud.y = y;    
  10.         cloud.z = 1 - x - y;    
  11.         inCloud->push_back(cloud);    
  12.     }    
  13. }    
再次执行测试程序,生成plane_cloud_out.pcd文件内容,
[cpp]  view plain  copy
  1. # .PCD v0.7 - Point Cloud Data file format  
  2. VERSION 0.7  
  3. FIELDS x y z intensity normal_x normal_y normal_z curvature  
  4. SIZE 4 4 4 4 4 4 4 4  
  5. TYPE F F F F F F F F  
  6. COUNT 1 1 1 1 1 1 1 1  
  7. WIDTH 10201  
  8. HEIGHT 1  
  9. VIEWPOINT 0 0 0 1 0 0 0  
  10. POINTS 10201  
  11. DATA ascii  
  12. -5 -5 11 0 -0.36251435 -0.33222172 -0.40937933 -1.0737418e+008  
  13. -5 -4.9000001 10.9 0 -0.2965593 -0.36960894 -0.34049508 -1.0737418e+008  
  14. -5 -4.8000002 10.8 0 -0.30257833 -0.32366723 -0.26829782 -1.0737418e+008  
  15. -5 -4.7000003 10.700001 0 -0.32577419 -0.35322508 -0.36178556 -1.0737418e+008  
  16. -5 -4.6000004 10.6 0 -0.34574831 -0.31297931 -0.31971669 -1.0737418e+008  
  17. -5 -4.5000005 10.5 0 -0.3243891 -0.34880957 -0.34800133 -1.0737418e+008  
  18. -5 -4.4000006 10.400001 0 -0.31732634 -0.34978729 -0.33582667 -1.0737418e+008  
  19. -5 -4.3000007 10.300001 0 -0.32080838 -0.33909816 -0.32085085 -1.0737418e+008  
  20. -5 -4.2000008 10.200001 0 -0.3384032 -0.33768722 -0.35353974 -1.0737418e+008  
  21. -5 -4.1000009 10.1 0 -0.34146273 -0.31813207 -0.32056987 -1.0737418e+008  
  22. -5 -4.000001 10.000001 0 -0.3408044 -0.32376921 -0.32963032 -1.0737418e+008  
  23. -5 -3.900001 9.9000015 0 -0.29470387 -0.359045 -0.31496942 -1.0737418e+008  
  24. -5 -3.8000011 9.8000011 0 -0.35997671 -0.31077951 -0.34562108 -1.0737418e+008  
  25. -5 -3.7000012 9.7000008 0 -0.32181746 -0.33056432 -0.30722877 -1.0737418e+008  
  26. -5 -3.6000013 9.6000013 0 -0.32655463 -0.3454251 -0.34494016 -1.0737418e+008  
  27. -5 -3.5000014 9.5000019 0 -0.35073376 -0.32577616 -0.35558489 -1.0737418e+008  
  28. -5 -3.4000015 9.4000015 0 -0.34808326 -0.32221347 -0.3418338 -1.0737418e+008  
  29. -5 -3.3000016 9.3000011 0 -0.3261568 -0.33644432 -0.32556668 -1.0737418e+008  
  30. ...  
  31. ...  
  32. ...  
可以看出每个点的法向量不再近似共线于(1, 1, 1)。原因暂时还不清楚,个人推测为最小二乘法拟合平面适用于稀疏点集,密集点云并不适合, 原因可能是密集点时u_matrix_

各行向量非常近似于平行,导致行列式为0,无法求得正确的逆矩阵,而以上计算过程中是用到了逆矩阵;

【2014-03-16】今天偶然看到一篇文章介绍最小二乘法的缺陷,http://www.cnblogs.com/jerrylead/archive/2011/08/21/2148625.html

如果样例数m相比特征数n少(m<n)或者特征间线性相关时,由于clip_image002(n*n矩阵)的秩小于特征个数(即clip_image002[1]不可逆)。因此最小二乘法clip_image004就会失效。

可以看出来,样本矩阵的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方阵对应的齐次线性方程组:


由此得到齐次线性方程组的非零解。

采用该方法,编程实现:

[cpp]  view plain  copy
  1.  /** \brief 计算点的法向量,曲率暂时不求 
  2.    * \param[in] 输入点云 
  3.    * \param[in] 给定点的索引下标  
  4.    * \param[out] 法向量x分量  
  5.    * \param[out] 法向量y分量   
  6.    * \param[out] 法向量z分量   
  7.    * \param[out] 曲率  
  8.    */  
  9. inline void  
  10.  computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, float &nx, float &ny, float &nz, float &curvature)  
  11. {  
  12.       Eigen::MatrixX4f mat_ext(k_, 4);  
  13.  for (size_t i = 0, ind_num = indices.size(); i < ind_num; ++i)  
  14.  {  
  15.   size_t indice = indices[i];  
  16.   
  17.   mat_ext(i, 0) = cloud.points[indice].x;  
  18.   mat_ext(i, 1) = cloud.points[indice].y;  
  19.   mat_ext(i, 2) = cloud.points[indice].z;  
  20.   mat_ext(i, 3) = 1;  
  21.  }  
  22.   
  23.  Eigen::MatrixXf mat_mult_ext = mat_ext.transpose() * mat_ext;    //mat是3*k_矩阵,mat_ext是k_*4矩阵  
  24.  // A*x = b, A.Row(0).cross(A.Row(1)) = eigenvector(0);  
  25.  Eigen::Vector4f row_vec[3];  
  26.  row_vec[0] = mat_mult_ext.row(0);  
  27.  row_vec[1] = mat_mult_ext.row(1);  
  28.  row_vec[2] = mat_mult_ext.row(2);  
  29.  Eigen::Matrix3f i_cofactor, j_cofactor, k_cofactor, l_cofactor;    //计算叉积时,i、j、k、l分量的代数余子式  
  30.   
  31.  for (size_t row_index = 0; row_index < 3; ++row_index)  
  32.  {  
  33.   for (size_t col_index = 0; col_index < 3; ++col_index)  
  34.   {  
  35.       i_cofactor(row_index , col_index) = row_vec[row_index][col_index + 1];  
  36.   
  37.       if (col_index < 1)  
  38.       {  
  39.           j_cofactor(row_index , col_index) = row_vec[row_index][col_index];  
  40.       }   
  41.       else  
  42.       {  
  43.           j_cofactor(row_index , col_index) = row_vec[row_index][col_index + 1];  
  44.       }  
  45.   
  46.       if (col_index < 2)  
  47.       {  
  48.           k_cofactor(row_index , col_index) = row_vec[row_index][col_index];  
  49.       }   
  50.       else  
  51.       {  
  52.           k_cofactor(row_index , col_index) = row_vec[row_index][col_index + 1];  
  53.       }  
  54.   
  55.       l_cofactor(row_index , col_index) = row_vec[row_index][col_index];  
  56.   }  
  57.  }  
  58.   
  59.  float i_dim = 0, j_dim = 0, k_dim = 0, l_dim = 0;  
  60.  i_dim = i_cofactor.determinant();  
  61.  j_dim = j_cofactor.determinant();  
  62.  k_dim = k_cofactor.determinant();  
  63.  l_dim = l_cofactor.determinant();  
  64.    
  65.  Eigen::Vector3f coeff_vec(i_dim, -j_dim, k_dim);  
  66.  float len = coeff_vec.squaredNorm();  
  67.  coeff_vec /= len;  
  68.   
  69.  nx = coeff_vec[0];  
  70.  ny = coeff_vec[1];  
  71.  nz = coeff_vec[2];  
  72. }  
采用以上方法实现后,编译PCL源码得到对应的库文件,然后测试程序引用,执行后生成的plane_cloud_out.pcd文件内容为:

[cpp]  view plain  copy
  1. # .PCD v0.7 - Point Cloud Data file format  
  2. VERSION 0.7  
  3. FIELDS x y z intensity normal_x normal_y normal_z curvature  
  4. SIZE 4 4 4 4 4 4 4 4  
  5. TYPE F F F F F F F F  
  6. COUNT 1 1 1 1 1 1 1 1  
  7. WIDTH 1681  
  8. HEIGHT 1  
  9. VIEWPOINT 0 0 0 1 0 0 0  
  10. POINTS 1681  
  11. DATA ascii  
  12. -5 -5 11 0 0.0034180761 0.003418348 0.0033811682 -1.0737418e+008  
  13. -5 -4.75 10.75 0 0.0034180761 0.003418348 0.0033811682 -1.0737418e+008  
  14. -5 -4.5 10.5 0 0.0036986405 0.0036108212 0.0036840038 -1.0737418e+008  
  15. -5 -4.25 10.25 0 0.0041074576 0.0039950516 0.0042172931 -1.0737418e+008  
  16. -5 -4 10 0 0.0040342622 0.004034651 0.0041443072 -1.0737418e+008  
  17. -5 -3.75 9.75 0 0.004033945 0.0041615977 0.0040161251 -1.0737418e+008  
  18. -5 -3.5 9.5 0 0.0041075312 0.0040329853 0.0041812863 -1.0737418e+008  
  19. -5 -3.25 9.25 0 0.0040594526 0.0040964941 0.0040962985 -1.0737418e+008  
  20. -5 -3 9 0 0.0040834057 0.0040837992 0.0041210586 -1.0737418e+008  
  21. -5 -2.75 8.75 0 0.0041877129 0.0040933029 0.0041687908 -1.0737418e+008  
  22. -5 -2.5 8.5 0 0.0040593483 0.0040955977 0.0040955977 -1.0737418e+008  
  23. -5 -2.25 8.25 0 0.0040110592 0.0041013826 0.0040649828 -1.0737418e+008  
  24. -5 -2 8 0 0.0040473556 0.0040835626 0.0040469691 -1.0737418e+008  
  25. -5 -1.75 7.75 0 0.0040717809 0.0040713926 0.0040715868 -1.0737418e+008  
  26. -5 -1.5 7.5 0 0.004047934 0.0040837578 0.0040467749 -1.0737418e+008  
  27. -5 -1.25 7.25 0 0.0040838025 0.004065339 0.0040841922 -1.0737418e+008  
  28. -5 -1 7 0 0.0040716026 0.0040716026 0.0040712142 -1.0737418e+008  
  29. -5 -0.75 6.75 0 0.0040599061 0.0040774848 0.0040595187 -1.0737418e+008  
  30. -5 -0.5 6.5 0 0.0040714089 0.0040717972 0.0040717972 -1.0737418e+008  
  31. -5 -0.25 6.25 0 0.0040713926 0.0040717809 0.0040715868 -1.0737418e+008  
  32. -5 0 6 0 0.0040716026 0.0040712142 0.0040716026 -1.0737418e+008  
  33. -5 0.25 5.75 0 0.0040713926 0.0040715868 0.0040717809 -1.0737418e+008  
  34. -5 0.5 5.5 0 0.0040714089 0.0040717972 0.0040717972 -1.0737418e+008  
  35. ...  
  36. ...  
  37. ...  

可以看到点云中每个点对应的法向量也都近似共线于(1, 1, 1)。与之前解非其次线性方程组的方法类似,当点云数据比较密集(点与点间距离很小)时,计算出来的结果也是很不准的;同样地,增加点K近邻的样本个数(即增加K邻近算法中的K值,本例中为k_),计算出的法向量是可以近似共线于(1,1,1)。个人推测的原因见文章之前部分。

还是那句话,真实原因有哪位朋友知道的话望告知!

完整代码可以下载http://download.csdn.net/detail/lming_08/7035195



  • 0
    点赞
  • 13
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值