您好,欢迎访问代理记账网站
移动应用 微信公众号 联系我们

咨询热线 -

电话 15988168939

联系客服
  • 价格透明
  • 信息保密
  • 进度掌控
  • 售后无忧

最小二乘法拟合平面 -- 代码实现(C++, Eigen3)

最小二乘法拟合平面 -- 代码实现(C++, Eigen3)

    • 说明
    • 作者[ 哈哈kls ]的推导
    • C++实现

说明

本文基于博主 哈哈kls 的博文:最小二乘法拟合平面 的公式推导,用C++进行实现。
文章出处:https://blog.csdn.net/konglingshneg/article/details/82585868

作者[ 哈哈kls ]的推导

Alt
注:上图最后一句应该是: z = a 0 x + a 1 y + a 2 z=a_{0}x+a_{1}y+a{2} z=a0x+a1y+a2

C++实现

#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <Eigen/QR>
#include <Eigen/SVD>

using namespace std;
using namespace Eigen;

/*
* From: https://blog.csdn.net/konglingshneg/article/details/82585868
* Author: JARK006(JARK006@QQ.COM)
* z = a0x + a1y + a2
* a0*x + a1*y - z + a2 = 0
* 入参:points --待拟合平面的点集
* 返回:拟合平面法向量 { a0, a1, -1 }
*/
vector<double> FittingPlaneEigen(vector<vector<double>>& points) {
    vector<vector<double>> mat3x4(3, vector<double>(4, 0));
    for (const auto& p : points) {
        // 3x3 方程组参数   p[0]:x, p[1]:y, p[2]:z
        // 对角线对称,只需处理5个
        mat3x4[0][0] += p[0] * p[0];
        mat3x4[0][1] += p[0] * p[1];
        mat3x4[0][2] += p[0];
        mat3x4[1][1] += p[1] * p[1];
        mat3x4[1][2] += p[1];

        // 3x1 方程组值
        mat3x4[0][3] += p[0] * p[2];
        mat3x4[1][3] += p[1] * p[2];
        mat3x4[2][3] += p[2];
    }

    MatrixXd A(3, 3);
    A <<
        mat3x4[0][0], mat3x4[0][1], mat3x4[0][2],
        mat3x4[0][1], mat3x4[1][1], mat3x4[1][2],
        mat3x4[0][2], mat3x4[1][2], (double)points.size();
    MatrixXd B(3, 1);
    B << mat3x4[0][3], mat3x4[1][3], mat3x4[2][3];

    //以下三个方式前14位有效数字基本一致,任选一个
    //来源:https://eigen.tuxfamily.org/dox/group__LeastSquares.html
    MatrixXd a = A.bdcSvd(ComputeThinU | ComputeThinV).solve(B);
    //MatrixXd a = A.colPivHouseholderQr().solve(B);
    //MatrixXd a = A.lu().solve(B);

    // a0*x + a1*y - z + a2 = 0 法向量为:a(0), a(1), -1
    return { a(0), a(1), -1.0 };
}

分享:

低价透明

统一报价,无隐形消费

金牌服务

一对一专属顾问7*24小时金牌服务

信息保密

个人信息安全有保障

售后无忧

服务出问题客服经理全程跟进