三维点拟合平面-最小二乘-C++

原理

参考代码

此处求解的平面方程为：z = ax + by + c

//作者：dwy
//日期:2019/07/09
// 用途：最小二乘拟合平面
#include<iostream>
#include<opencv.hpp>
#include<cmath>
using namespace std;
using namespace cv;
void creatTestData(vector<Point3f> &output)
{
	RNG rng(12345);

	int a = 10;
	int b = 2;
	int c = 5;
	cout << "测试系数为：a=" << a << ",b=" << b << ",c=" << c << endl;
	for (int i = 0;i < 10;i++)
	{
		double x = rng.uniform(1, 100);
		double y = rng.uniform(100,200);
		double z = a*x + b*y + c;
		Point3f point;
		point.x = x;
		point.y = y;
		point.z = z;
		output.push_back(point);
	}
}
void plane_fitting(vector<double>& coeffient, vector<Point3f> &input)
{
	Mat dst = Mat(3, 3, CV_32F, Scalar(0));//初始化系数矩阵A
	Mat out = Mat(3, 1, CV_32F, Scalar(0));//初始化矩阵b
	for (int i = 0;i < input.size();i++)
	{
		//计算3*3的系数矩阵
		dst.at<float>(0, 0) = dst.at<float>(0, 0) + pow(input[i].x, 2);
		dst.at<float>(0, 1) = dst.at<float>(0, 1) + input[i].x*input[i].y;
		dst.at<float>(0, 2) = dst.at<float>(0, 2) + input[i].x;
		dst.at<float>(1, 0) = dst.at<float>(1, 0) + input[i].x*input[i].y;
		dst.at<float>(1, 1) = dst.at<float>(1, 1) + pow(input[i].y, 2);
		dst.at<float>(1, 2) = dst.at<float>(1, 2) + input[i].y;
		dst.at<float>(2, 0) = dst.at<float>(2, 0) + input[i].x;
		dst.at<float>(2, 1) = dst.at<float>(2, 1) + input[i].y;	
		dst.at<float>(2, 2) = input.size();
		//计算3*1的结果矩阵
		out.at<float>(0, 0) = out.at<float>(0, 0) + input[i].x*input[i].z;
		out.at<float>(1, 0) = out.at<float>(1, 0) + input[i].y*input[i].z;
		out.at<float>(2, 0) = out.at<float>(2, 0) + input[i].z;
	}
	//判断矩阵是否奇异
	double determ = determinant(dst);
	if (abs(determ) < 0.001) {
		cout << "矩阵奇异" << endl;
		return;
	}
	//Mat inv;
	//invert(dst, inv);//求矩阵的逆
	//Mat output = inv*out;//计算输出
	//coeffient.clear();//把结果输出
	//coeffient.push_back(output.at<float>(0, 0));
	//coeffient.push_back(output.at<float>(1, 0));
	//coeffient.push_back(output.at<float>(2, 0));
	
	// 修改此处代码为SVD分解
	Mat result = Mat(3, 1, CV_32F, Scalar(0));;
	solve(dst, out, result, DECOMP_SVD);
	coeffient.clear();//把结果输出
	coeffient.push_back(result.at<float>(0, 0));
	coeffient.push_back(result.at<float>(1, 0));
	coeffient.push_back(result.at<float>(2, 0));

}
int main()
{
	vector<double> coeffient;
	vector<Point3f> input;
	creatTestData(input);
	plane_fitting(coeffient, input);
	for (int i = 0;i < coeffient.size();i++)
	{
		cout << "第"<<i<<"个系数为："<<coeffient[i] << endl;
	}
	cout << "hello,world" << endl;
	return 0;
}

此处求解的平面方程为Ax+By+Cz=D

/*
 * 
 * 
 *  最小二乘拟合平面，平面方程：Ax+By+Cz=D
 *  A = plane.at<float>(0,0)
 *  B = plane.at<float>(1,0)
 *  C = plane.at<float>(2,0)
 *  D = plane.at<float>(3,0)
 * 
 * */
void fitPlane(const cv::Mat &points, cv::Mat& plane){
    int rows = points.rows;
    int cols = points.cols;

    cv::Mat centroid = cv::Mat::zeros(1,cols,CV_32FC1);
    for(int i=0;i<cols;i++){
        for(int j=0;j<rows;j++){
            centroid.at<float>(0,i) += points.at<float>(j,i);
        }
        centroid.at<float>(0,i)/=rows;

    }

    cv::Mat points2 = cv::Mat::ones(rows,cols,CV_32FC1);
    for(int i=0;i<rows;i++){
        for(int j=0;j<cols;j++){
            points2.at<float>(i,j) = points.at<float>(i,j) - centroid.at<float>(0,j) ;
        }
    }
    cv::Mat A,W,U,V;
    cv::gemm(points2,points,1,NULL,0,A,CV_GEMM_A_T);
    SVD::compute(A,W,U,V);


    plane = cv::Mat::zeros(cols+1,1,CV_32FC1);
    for (int c = 0; c<cols; c++){
        plane.at<float>(c,0) = V.at<float>(cols-1,c);
        plane.at<float>(cols,0) += plane.at<float>(c,0)*centroid.at<float>(0,c);
    }

}

参考资料

基于Opencv的平面拟合 C++实现
opencv3 最小二乘拟合平面