疫情在家，想做科研，可是资料都在学校电脑里面。只能看看能不能回想起什么写点什么。
这次主要是想把提取出的点云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 }
当然写的不是很严谨，仅供大家参考。