利用Eigen库，PCA构建点云法向量

疫情在家，想做科研，可是资料都在学校电脑里面。只能看看能不能回想起什么写点什么。

这次主要是想把提取出的点云patch单独进行点云法向量的计算，因为已经构成patch，则不需使用knn或者设定邻域半径。

接下来手撕 PCA 来构建点云法向量。

 1 #define _CRT_SECURE_NO_WARNINGS
 2 #define _SCL_SECURE_NO_WARNINGS
 3 #include <pcl/io/pcd_io.h>
 4 #include <pcl/point_types.h>
 5 #include <pcl/features/normal_3d.h>
 6 #include <Eigen/core>
 7 
 8 #include <vector>
 9 
10 using namespace std;
11 
12 int main() 
13 {
14     pcl::PCDReader reader;
15     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
16     reader.read("../../data/23b_12.pcd", *cloud);
17 
18     int cld_sz = cloud->size();
19     pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
20     normals->resize(cld_sz);
21 
22     //计算中心点坐标
23     double center_x = 0, center_y = 0, center_z = 0;
24     for (int i = 0; i < cld_sz; i++) {
25         center_x += cloud->points[i].x;
26         center_y += cloud->points[i].y;
27         center_z += cloud->points[i].z;
28     }
29     center_x /= cld_sz;
30     center_y /= cld_sz;
31     center_z /= cld_sz;
32     //计算协方差矩阵
33     double xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
34     for (int i = 0; i < cld_sz; i++) {
35         xx += (cloud->points[i].x - center_x) * (cloud->points[i].x - center_x);
36         xy += (cloud->points[i].x - center_x) * (cloud->points[i].y - center_y);
37         xz += (cloud->points[i].x - center_x) * (cloud->points[i].z - center_z);
38         yy += (cloud->points[i].y - center_y) * (cloud->points[i].y - center_y);
39         yz += (cloud->points[i].y - center_y) * (cloud->points[i].z - center_z);
40         zz += (cloud->points[i].z - center_z) * (cloud->points[i].z - center_z);
41     }
42     //大小为3*3的协方差矩阵
43     Eigen::Matrix3f covMat(3, 3);
44     covMat(0, 0) = xx / cld_sz;
45     covMat(0, 1) = covMat(1, 0) = xy / cld_sz;
46     covMat(0, 2) = covMat(2, 0) = xz / cld_sz;
47     covMat(1, 1) = yy / cld_sz;
48     covMat(1, 2) = covMat(2, 1) = yz / cld_sz;
49     covMat(2, 2) = zz / cld_sz;
50 
51     //求特征值与特征向量
52     Eigen::EigenSolver<Eigen::Matrix3f> es(covMat);
53     Eigen::Matrix3f val = es.pseudoEigenvalueMatrix();
54     Eigen::Matrix3f vec = es.pseudoEigenvectors();
55 
56     //找到最小特征值t1
57     double t1 = val(0, 0);
58     int ii = 0;
59     if (t1 > val(1, 1)) {
60         ii = 1;
61         t1 = val(1, 1);
62     }
63     if (t1 > val(2, 2)){
64         ii = 2;
65         t1 = val(2, 2);
66     }
67 
68     //最小特征值对应的特征向量v_n
69     Eigen::Vector3f v(vec(0, ii), vec(1, ii), vec(2, ii));
70     //特征向量单位化
71     v /= v.norm();
72     　for (int i = 0; i < cld_sz; i++) {
74         normals->points[i].normal_x = v(0);
75         normals->points[i].normal_y = v(1);
76         normals->points[i].normal_z = v(2);
77         normals->points[i].curvature = t1 / (val(0, 0) + val(1, 1) + val(2, 2));
78     }
79 
80     cin.get();
81     return 0;
82 }

 

当然写的不是很严谨，仅供大家参考。

posted @ 2020-04-02 19:07  WallyBill  阅读(2161)  评论(3)  编辑  收藏  举报