使用C++与PCL实现的最小二乘拟合圆柱

#include <iostream> #include <pcl/io/pcd_io.h> #include<pcl/common/pca.h> #include <pcl/common/common.h> #include <pcl/features/normal_3d.h> #include <pcl/segmentation/sac_segmentation.h> #include <pcl/visualization/cloud_viewer.h> using namespace std; int main() { // -----------------------------加载点云------------------------------- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); if (pcl::io::loadPCDFile("E://data//Cylinder2.pcd", *cloud) < 0) { PCL_ERROR("点云读取失败！\n"); return -1; } cout << "点云点数为：" << cloud->points.size() << endl; // -----------------------------法线估计-------------------------------- pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n; // 创建法向量估计对象 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>()); n.setSearchMethod(tree); // 设置搜索方式 n.setInputCloud(cloud); // 设置输入点云 n.setKSearch(20); // 设置K近邻搜索点的个数 pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>); n.compute(*normals); // 计算法向量，并将结果保存到normals中 // -----------------------PCA计算圆柱轴线向量初值----------------------- pcl::PointCloud<pcl::PointXYZ>::Ptr pointNormal(new pcl::PointCloud<pcl::PointXYZ>); pointNormal->resize(normals->size()); for (int i = 0; i < normals->size(); ++i) { pointNormal->points[i].getVector3fMap() = normals->points[i].getNormalVector3fMap(); } pcl::PCA<pcl::PointXYZ>pca; pca.setInputCloud(pointNormal); Eigen::Vector3f e3 = pca.getEigenVectors().col(2); float a0 = e3[0], b0 = e3[1], c0 = e3[2]; cout << "初次计算轴线方向为:\n" << e3 << endl; // -----------------------------构建R33矩阵---------------------------- Eigen::Matrix4f tran = Eigen::Matrix4f::Identity(); Eigen::Matrix3f R = Eigen::Matrix3f::Identity(); float denom = hypot(a0, b0); R(0, 0) = b0 / denom, R(0, 1) = -a0 / denom, R(0, 2) = 0; R(1, 0) = a0 * c0 / denom, R(1, 1) = b0 * c0 / denom, R(1, 2) = -denom; R(2, 0) = a0, R(2, 1) = b0, R(2, 2) = c0; tran.block<3, 3>(0, 0) = R; // ---------------------------圆柱进行坐标转换------------------------- pcl::PointCloud<pcl::PointXYZ>::Ptr circle(new pcl::PointCloud<pcl::PointXYZ>); pcl::transformPointCloud(*cloud, *circle, tran); pcl::io::savePCDFileBinary("circle.pcd", *circle); // 保存速度较快 // ------------------------------拟合二维圆---------------------------- int m = circle->size(); Eigen::MatrixXf B = Eigen::MatrixXf::Ones(m, 3); Eigen::VectorXf L = Eigen::VectorXf::Ones(m); for (size_t i = 0; i < m; ++i) { B(i, 0) = circle->points[i].x * 2; B(i, 1) = circle->points[i].y * 2; L(i) = circle->points[i].getVector3fMap().head(2).squaredNorm(); } Eigen::Vector3f X = (B.transpose() * B).inverse() * B.transpose() * L; float x1 = X[0]; float y1 = X[1]; float r0 = sqrt(x1 * x1 + y1 * y1 + X[2]); Eigen::Vector3f c = Eigen::Vector3f{ x1,y1,0 }; cout << "c=\n" << c << endl; Eigen::Vector3f co = R.inverse() * c; cout << "c0=\n" << co << endl; // 计算圆柱点z最值 pcl::PointXYZ minPt, maxPt; pcl::getMinMax3D(*cloud, minPt, maxPt); x1 = co[0]; y1 = co[1]; float z1 = (minPt.z + maxPt.z) * 0.5; // ------------------------------最小二乘迭代--------------------------- Eigen::MatrixXf Bn = Eigen::MatrixXf::Ones(m, 7); Eigen::VectorXf Ln = Eigen::VectorXf::Ones(m); Eigen::VectorXf Xn = Eigen::VectorXf::Zero(7, 1); Xn << x1, y1, z1, a0, b0, c0, r0; // 迭代初值 Eigen::VectorXf deltX; int count = 0; bool method1 = true; // 两种拟合方法，如果设置成true则使用方法一 // 当半径迭代增量大于0.001时需要继续计算 while (r0 > 0.001) { // 方法一 if (method1 == true) { for (int i = 0; i < cloud->size(); ++i) { float x = cloud->points[i].x; float y = cloud->points[i].y; float z = cloud->points[i].z; Bn(i, 0) = 2 * x1 - 2 * x + 2 * a0 * (a0 * (x - x1) + b0 * (y - y1) + c0 * (z - z1)); Bn(i, 1) = 2 * y1 - 2 * y + 2 * b0 * (a0 * (x - x1) + b0 * (y - y1) + c0 * (z - z1)); Bn(i, 2) = 2 * z1 - 2 * z + 2 * c0 * (a0 * (x - x1) + b0 * (y - y1) + c0 * (z - z1)); Bn(i, 3) = -2 * (x - x1) * (a0 * (x - x1) + b0 * (y - y1) + c0 * (z - z1)); Bn(i, 4) = -2 * (y - y1) * (a0 * (x - x1) + b0 * (y - y1) + c0 * (z - z1)); Bn(i, 5) = -2 * (z - z1) * (a0 * (x - x1) + b0 * (y - y1) + c0 * (z - z1)); Bn(i, 6) = -2 * r0; Ln(i) = -(pow(x - x1, 2) + pow(y - y1, 2) + pow(z - z1, 2) - pow((a0 * (x - x1) + b0 * (y - y1) + c0 * (z - z1)), 2) - pow(r0, 2)); } } // 方法二 else { for (int i = 0; i < cloud->size(); ++i) { float x = cloud->points[i].x; float y = cloud->points[i].y; float z = cloud->points[i].z; Bn(i, 0) = r0 * (x1 - x + a0 * (a0 * (x - x1) + b0 * (y - y1) + c0 * (z - z1))); Bn(i, 1) = r0 * (y1 - y + b0 * (a0 * (x - x1) + b0 * (y - y1) + c0 * (z - z1))); Bn(i, 2) = r0 * (z1 - z + c0 * (a0 * (x - x1) + b0 * (y - y1) + c0 * (z - z1))); Bn(i, 3) = -r0 * (x - x1) * (a0 * (x - x1) + b0 * (y - y1) + c0 * (z - z1)); Bn(i, 4) = -r0 * (y - y1) * (a0 * (x - x1) + b0 * (y - y1) + c0 * (z - z1)); Bn(i, 5) = -r0 * (z - z1) * (a0 * (x - x1) + b0 * (y - y1) + c0 * (z - z1)); Bn(i, 6) = (pow((x - x1), 2) + pow((y - y1), 2) + pow((z - z1), 2) - (a0 * (x - x1) + b0 * (y - y1) + c0 * pow((z - z1), 2)) - (3 * r0 * r0)) * 0.5; Ln(i) = -((pow(x - x1, 2) + pow(y - y1, 2) + pow(z - z1, 2) - pow((a0 * (x - x1) + b0 * (y - y1) + c0 * (z - z1)), 2) - pow(r0, 2))) * 0.5 * r0; } } // 间接平差求解 Eigen::MatrixXf NBB = Bn.transpose() * Bn; Eigen::VectorXf W = Bn.transpose() * Ln; deltX = NBB.inverse() * W; // 计算增量 Xn += deltX; // 获取迭代增量 x1 = deltX[0], y1 = deltX[1], z1 = deltX[2], a0 = deltX[3], b0 = deltX[4], c0 = deltX[5], r0 = deltX[6]; count += 1; //迭代次数足够多，跳出迭代。 if (count >= 10) { break; } } cout << "迭代次数：" << count << endl; pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients); // 保存圆柱体模型系数 coefficients_cylinder->values.resize(7); coefficients_cylinder->values[0] = Xn[0]; coefficients_cylinder->values[1] = Xn[1]; coefficients_cylinder->values[2] = Xn[2]; coefficients_cylinder->values[3] = Xn[3]; coefficients_cylinder->values[4] = Xn[4]; coefficients_cylinder->values[5] = Xn[5]; coefficients_cylinder->values[6] = Xn[6]; cout << "轴线一点坐标：(" << coefficients_cylinder->values[0] << ", " << coefficients_cylinder->values[1] << ", " << coefficients_cylinder->values[2] << ")" << endl; cout << "轴线方向向量：(" << coefficients_cylinder->values[3] << ", " << coefficients_cylinder->values[4] << ", " << coefficients_cylinder->values[5] << ")" << endl; cout << "圆柱体半径：" << coefficients_cylinder->values[6] << endl; //---------------------------------结果可视化------------------------- boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D viewer")); viewer->setBackgroundColor(0.7, 0.7, 0.7); viewer->addText("FIT CYLINDER", 10, 10, "v1 text"); viewer->setWindowName("最小二乘拟合圆柱"); viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "cloud"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud"); viewer->addCylinder(*coefficients_cylinder, "cycler", 0); // 可视化拟合出来的圆柱模型 while (!viewer->wasStopped()) { viewer->spinOnce(100); } return 0; }