PCL （Matlab）拟合椭球

一、椭球点云数学模型二、PCL生成点云int main() { // 生成椭球点云 噪声 pcl::PointCloudpcl::PointXYZ::Ptr cloud( new pcl::PointCloudpcl::PointXYZ); // 椭球参数 float a 2.0f; // x轴 float b 1.5f; // y轴 float c 1.0f; // z轴 int N 20000; // 随机数 std::default_random_engine gen; std::uniform_real_distributionfloat dist_u(-1.0f, 1.0f); std::uniform_real_distributionfloat dist_phi(0.0f, 2 * M_PI); std::normal_distributionfloat noise(0.0f, 0.02f); // σ0.02 for (int i 0; i N; i) { float u dist_u(gen); float phi dist_phi(gen); float theta acos(u); // 椭球点 float x a * sin(theta) * cos(phi); float y b * sin(theta) * sin(phi); float z c * cos(theta); // -------- 法向噪声 -------- float nx x / (a * a); float ny y / (b * b); float nz z / (c * c); float norm sqrt(nx * nx ny * ny nz * nz); nx / norm; ny / norm; nz / norm; float eps noise(gen); pcl::PointXYZ p; p.x x eps * nx; p.y y eps * ny; p.z z eps * nz; cloud-points.push_back(p); } cloud-width cloud-points.size(); cloud-height 1; cloud-is_dense false; pcl::io::savePCDFileASCII(ellipsoid_noise.pcd, *cloud); std::cout Ellipsoid point cloud generated! std::endl; system(pause); return 0; }三、拟合1、模型2、SVD拟合1、构造线性方程2、写成矩阵的方式3、SVD 求解3、从一般二次曲面恢复“椭球参数”SVD 的问题必须知道❌ 不是最小几何距离❌ 对噪声敏感❌ 可能拟合成双曲面所以必须4、LM 非线性优化核心1、 参数化椭球推荐用中心c旋转R半轴a,b,c2、残差关键3、LM 目标四、Matlab PCL测试流程点云↓SVDDLT↓提取中心 轴↓LM 精化↓高精度椭球Matlab % 构造点 [X,Y,Z] sphere(200); X 2*X(:); Y 1.5*Y(:); Z 1*Z(:); % 加噪声 X X 0.05*randn(size(X)); Y Y 0.05*randn(size(Y)); Z Z 0.05*randn(size(Z)); % 构造 A A [X.^2 Y.^2 Z.^2 X.*Y X.*Z Y.*Z X Y Z ones(size(X))]; % SVD [~,~,V] svd(A); p V(:,end); disp(p) -0.1663 -0.2954 -0.6623 -0.0000 0.0006 -0.0001 0.0001 0.0009 0.0007 0.6681 PCL 测试1、构造矩阵用 EigenEigen::MatrixXd A(N,10); for (int i0;iN;i) { double x cloud-points[i].x; double y cloud-points[i].y; double z cloud-points[i].z; A.row(i) x*x, y*y, z*z, x*y, x*z, y*z, x, y, z, 1.0; }2、SVDEigen::JacobiSVDEigen::MatrixXd svd(A, Eigen::ComputeFullV); Eigen::VectorXd p svd.matrixV().col(9);3、构造二次曲面矩阵 QEigen::Matrix4d Q; Q p(0), p(3)/2, p(4)/2, p(6)/2, p(3)/2, p(1), p(5)/2, p(7)/2, p(4)/2, p(5)/2, p(2), p(8)/2, p(6)/2, p(7)/2, p(8)/2, p(9);4、提取椭球中心Eigen::Matrix3d Q3 Q.block3,3(0,0); Eigen::Vector3d q Q.block3,1(0,3); Eigen::Vector3d center -Q3.inverse() * q;5、提取椭球半轴初值double constant Q(3,3) q.transpose() * center; Eigen::Matrix3d M Q3 / (-constant); // 特征分解 Eigen::SelfAdjointEigenSolverEigen::Matrix3d es(M); Eigen::Vector3d eigenvalues es.eigenvalues(); Eigen::Matrix3d eigenvectors es.eigenvectors(); // 半轴长度 Eigen::Vector3d axes; axes(0) 1.0 / sqrt(eigenvalues(0)); axes(1) 1.0 / sqrt(eigenvalues(1)); axes(2) 1.0 / sqrt(eigenvalues(2));6、残差函数核心-LM 优化struct EllipsoidResidual { EllipsoidResidual(double x, double y, double z) : x_(x), y_(y), z_(z) {} template typename T bool operator()(const T* const param, T* residual) const { // param [cx, cy, cz, a, b, c] T dx (T(x_) - param[0]) / param[3]; T dy (T(y_) - param[1]) / param[4]; T dz (T(z_) - param[2]) / param[5]; residual[0] dx*dx dy*dy dz*dz - T(1); return true; } double x_, y_, z_; };7、构建优化问题-LM 优化double param[6] { center(0), center(1), center(2), axes(0), axes(1), axes(2) }; ceres::Problem problem; for (int i 0; i N; i) { auto pt cloud-points[i]; ceres::CostFunction* cost new ceres::AutoDiffCostFunctionEllipsoidResidual, 1, 6( new EllipsoidResidual(pt.x, pt.y, pt.z)); problem.AddResidualBlock(cost, nullptr, param); }8、设置优化参数-LM 优化ceres::Solver::Options options; options.linear_solver_type ceres::DENSE_QR; options.minimizer_progress_to_stdout true; ceres::Solver::Summary summary; ceres::Solve(options, problem, summary);9、输出结果std::cout Center: param[0] , param[1] , param[2] std::endl; std::cout Axes: param[3] , param[4] , param[5] std::endl;10、误差评估必须做double err 0; for (auto pt : cloud-points) { double dx (pt.x - param[0]) / param[3]; double dy (pt.y - param[1]) / param[4]; double dz (pt.z - param[2]) / param[5]; double r dx*dx dy*dy dz*dz - 1; err r*r; } err / cloud-size(); std::cout Mean error: err std::endl;