三維點雲學習(Ⅰ)- C++實操(PCA降維,升維、模型點雲法向量)
阿新 • • 發佈:2021-02-08
三維點雲學習(Ⅰ)- C++實操
一、VS2013配置PCl1.80庫
參考連結
二、本過程採用資料集為modelnet40
為40種物體的三維點雲資料集
連結:https://pan.baidu.com/s/1LX9xeiXJ0t-Fne8BCGSjlQ
提取碼:es14
三、PCA降維、模型點雲法向量
#include <pcl/point_types.h> #include <pcl/io/pcd_io.h> #include <pcl/kdtree/kdtree_flann.h> #include <pcl/features/normal_3d.h> #include <pcl/surface/gp3.h> #include <pcl/visualization/pcl_visualizer.h> #include <Eigen/Dense> //功能介紹:讀入txt、asc->pcl::PointXYZ格式(資料集中只讀取前三列) void CreateCloudFromTxt(const std::string& file_path, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) { std::ifstream file(file_path.c_str());//c_str():生成一個const char*指標,指向以空字元終止的陣列。 std::string line; pcl::PointXYZ point; while (getline(file, line)) //用到x,y,z { std::stringstream ss(line); ss >> point.x; ss >> point.y; ss >> point.z; cloud->push_back(point); } file.close(); } //功能:點雲轉換為矩陣 void PointConversionEigen(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Eigen::MatrixXd &cloudMat) { cloudMat.resize(cloud->points.size(), 3); //定義點雲行,3列 for (int itr = 0; itr < cloud->points.size(); itr++) { cloudMat(itr, 0) = cloud->points[itr].x; cloudMat(itr, 1) = cloud->points[itr].y; cloudMat(itr, 2) = cloud->points[itr].z; } } //功能:實現PCA功能 void PCAFunctions(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::Normal &pointNormal, pcl::Normal &pointNormal2) { Eigen::MatrixXd cloudMat; cloudMat.resize(cloud->points.size(), 3); //定義點雲行,3列 for (int itr = 0; itr < cloud->points.size(); itr++) { cloudMat(itr, 0) = cloud->points[itr].x; cloudMat(itr, 1) = cloud->points[itr].y; cloudMat(itr, 2) = cloud->points[itr].z; } Eigen::RowVector3d meanMat = cloudMat.colwise().mean(); cloudMat.rowwise() -= meanMat; //求協方差矩陣 Eigen::MatrixXd covMat; if (cloudMat.rows() == 1) covMat = (cloudMat.adjoint()*cloudMat) / double(cloudMat.rows()); else covMat = (cloudMat.adjoint()*cloudMat) / double(cloudMat.rows() - 1); Eigen::JacobiSVD<Eigen::MatrixXd> svd(covMat, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3d V = svd.matrixV(); Eigen::Matrix3d U = svd.matrixU(); Eigen::Matrix3d S = U.inverse() * covMat * V.transpose().inverse(); pointNormal2.normal_x = V(3); pointNormal2.normal_y = V(4); pointNormal2.normal_z = V(5); pointNormal.normal_x = V(6); pointNormal.normal_y = V(7); pointNormal.normal_z = V(8); } //功能:輸入點雲端計算法相 void PointCloudNormal(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr &normals) { //pcl::PointCloud<pcl::Normal>::Ptr normalsExcess(new pcl::PointCloud<pcl::Normal>); for (int iPt = 0; iPt < cloud->points.size(); iPt++) { pcl::KdTreeFLANN<pcl::PointXYZ> kdTree; kdTree.setInputCloud(cloud); pcl::PointCloud<pcl::PointXYZ>::Ptr searchKPoints(new pcl::PointCloud<pcl::PointXYZ>); //搜尋結果 int k = 10; std::vector<int> kIndices(k); std::vector<float> kSqrDistances(k); if (kdTree.nearestKSearch(cloud->points[iPt], k, kIndices, kSqrDistances) > 0) { pcl::PointXYZ kPoint; for (int i = 0; i < k; i++) { kPoint.x = cloud->points[kIndices[i]].x; kPoint.y = cloud->points[kIndices[i]].y; kPoint.z = cloud->points[kIndices[i]].z; searchKPoints->push_back(kPoint); } } // neighbors within radius search //float radius = 1.0; //std::vector<int> radiusIndices; //std::vector<float> radiusSqrDistance; //if (kdTree.radiusSearch(cloud->points[iPt], radius, radiusIndices, radiusSqrDistance) > 0) //{ // //do something //} pcl::Normal pointNormal, pointNormal2; PCAFunctions(searchKPoints, pointNormal, pointNormal2); normals->push_back(pointNormal); } //normals = normalsExcess; } int main() { //載入點雲模型 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); CreateCloudFromTxt("airplane_0008.asc", cloud); Eigen::MatrixXd cloudMat; PointConversionEigen(cloud, cloudMat); //計算所有點雲的法向量 pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>); PointCloudNormal(cloud, normals); //降維 pcl::PointCloud<pcl::PointXYZ>::Ptr projectionCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::Normal pointCloudNormal, pointCloudNormal2; //降維所用法向量 //Eigen::RowVector3d projectionMat; //降維所用法向量 Eigen::MatrixXd projectionPointMat; PCAFunctions(cloud, pointCloudNormal, pointCloudNormal2); //projectionMat << pointCloudNormal.normal_x, pointCloudNormal.normal_y, pointCloudNormal.normal_z; //Eigen::Matrix3d diagonalMat(projectionMat.asDiagonal()); //構造對角矩陣 //projectionPointMat = (diagonalMat*cloudMat.transpose()).transpose(); Eigen::Matrix3d projectionMat; projectionMat << pointCloudNormal.normal_x, pointCloudNormal.normal_y, pointCloudNormal.normal_z, pointCloudNormal2.normal_x, pointCloudNormal2.normal_y, pointCloudNormal2.normal_z, 0, 0, 0; projectionPointMat = (projectionMat*cloudMat.transpose()).transpose(); pcl::PointXYZ mPoint; for (int i = 0; i < projectionPointMat.size(); i++) { mPoint.x = projectionPointMat(i, 0); mPoint.y = projectionPointMat(i, 1); mPoint.z = projectionPointMat(i, 2); projectionCloud->push_back(mPoint); } /*圖形顯示模組*/ boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D viewer")); //viewer->initCameraParameters(); int v1(0), v2(1), v3(2), v4(3); viewer->createViewPort(0.0, 0.0, 0.5, 0.5, v1); viewer->createViewPort(0.5, 0.0, 1.0, 0.5, v2); viewer->createViewPort(0.0, 0.5, 0.5, 1.0, v3); viewer->createViewPort(0.5, 0.5, 1.0, 1.0, v4); //設定背景顏色 viewer->setBackgroundColor(5, 55, 10, v1); //設定點雲顏色 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 225, 0); //新增座標系 viewer->addCoordinateSystem(0.5, v1); viewer->addPointCloud<pcl::PointXYZ>(projectionCloud, "sample cloud", v1); viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud0", v2); viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, normals, 1, 0.01, "normals", v2); viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample cloud1", v3); viewer->addPointCloud<pcl::PointXYZ>(projectionCloud, "sample cloud3", v4); //設定點雲大小 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud3", 4); //新增點雲法向量的另一種方式; //解決來源http://www.pcl-users.org/How-to-add-PointNormal-on-PCLVisualizer-td4037041.html //pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>); //pcl::concatenateFields(*cloud, *normals, *cloud_with_normals); //viewer->addPointCloudNormals<pcl::PointNormal>(cloud_with_normals, 50, 0.01, "normals"); // while (!viewer->wasStopped()) { viewer->spinOnce(100); boost::this_thread::sleep(boost::posix_time::microseconds(100000)); } return 0; }