PCL点云最小二乘法拟合空间直线
#include\x20
using\x20namespace\x20std;
int\x20main() { \x20\x20//------------------------------输入点云----------------------------------- \x20\x20pcl::PointCloudpcl::PointXYZ::Ptr\x20cloud(new\x20pcl::PointCloudpcl::PointXYZ); \x20\x20if\x20(pcl::io::loadPLYFilepcl::PointXYZ("D:\DIANYUNWENJIANJIA\newkruskal2_ply.ply",\x20cloud)\x20<\x200) \x20\x20{ \x20\x20\x20\x20PCL_ERROR("读取文件错误"); \x20\x20\x20\x20return\x20-1; \x20\x20} \x20\x20//---------------------------------PCA------------------------------------- \x20\x20pcl::PCApcl::PointXYZ\x20pca; \x20\x20pca.setInputCloud(cloud); \x20\x20 \x20\x20//\x20获取特征值对应的特征向量 \x20\x20Eigen::RowVector3f\x20V1\x20=\x20pca.getEigenVectors().col(0); \x20\x20Eigen::RowVector3f\x20V2\x20=\x20pca.getEigenVectors().col(1); \x20\x20Eigen::RowVector3f\x20V3\x20=\x20pca.getEigenVectors().col(2); \x20\x20 \x20\x20//---------------------------直线的点向式--------------------------------- \x20\x20float\x20m\x20=\x20V1[0],\x20n\x20=\x20V1[1],\x20p\x20=\x20V1[2]; \x20\x20float\x20x0\x20=\x20pca.getMean()[0],\x20y0\x20=\x20pca.getMean()[1],\x20z0\x20=\x20pca.getMean()[2]; \x20\x20cout\x20<<\x20"直线的方向向量为："\x20<<\x20V1\x20<<\x20"\n" \x20\x20\x20\x20<<\x20"\n直线上一点的坐标为："\x20<<\x20pca.getMean().head<3>().transpose()\x20<<\x20endl; \x20\x20 \x20\x20//---------------------------直线的一般式--------------------------------- \x20\x20Eigen::Matrix<float, 2, 3>A; \x20\x20A.row(0)\x20=\x20V2; \x20\x20A.row(1)\x20=\x20V3; \x20\x20Eigen::Vector3f\x20sigma\x20=\x20pca.getMean().head<3>(); \x20\x20Eigen::Vector2f\x20b\x20=\x20A\x20\x20sigma; \x20\x20 \x20\x20cout\x20<<\x20"\n直线的一般式为：\n" \x20\x20\x20\x20<<\x20V2[0]\x20<<\x20"x+("\x20<<\x20V2[1]\x20<<\x20"y)+("\x20<<\x20V2[2]\x20<<\x20"z)="\x20<<\x20b[0]\x20<<\x20"\n" \x20\x20\x20\x20<<\x20V3[0]\x20<<\x20"x+("\x20<<\x20V3[1]\x20<<\x20"y)+("\x20<<\x20V3[2]\x20<<\x20"z)="\x20<<\x20b[1]\x20<<\x20endl; \x20\x20 \x20\x20//----------------------------点云可视化---------------------------------- \x20\x20pcl::visualization::PCLVisualizer\x20viewer; \x20\x20viewer.addPointCloud(cloud, "cloud");\x20//\x20加载比对点云 \x20\x20pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ\x20line_color(cloud, 255, 0, 0); \x20\x20viewer.addPointCloud(cloud, line_color, "line"); \x20\x20viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "line"); \x20\x20 \x20\x20//--------------------------拟合直线可视化-------------------------------- \x20\x20pcl::ModelCoefficients::Ptr\x20coefficients(new\x20pcl::ModelCoefficients); \x20\x20//获取直线模型系数 \x20\x20coefficients->values.push_back(x0); \x20\x20coefficients->values.push_back(y0); \x20\x20coefficients->values.push_back(z0); \x20\x20coefficients->values.push_back(m); \x20\x20coefficients->values.push_back(n); \x20\x20coefficients->values.push_back(p); \x20\x20 \x20\x20//可视化拟合出来的直线 \x20\x20viewer.setWindowName("最小二乘拟合空间直线"); \x20\x20viewer.addLine(coefficients, "line"); \x20\x20viewer.spin(); \x20\x20 \x20\x20//----------------------------输出点云----------------------------------- \x20\x20pcl::io::savePLYFile("D:\DIANYUNWENJIANJIA\最小二乘法拟合直线_ply.ply",\x20cloud); \x20\x20 \x20\x20return\x200;
原文地址: https://www.cveoy.top/t/topic/pZPY 著作权归作者所有。请勿转载和采集!