How do discrete points in 3D space fit a plane?

0 Preface

  In the process of point cloud modeling, sometimes it is necessary to calibrate the scanned point cloud. In actual use, the ground is often used as the reference plane, and the scanned 3D space point cloud needs to be fitted to the plane to correct the scanning results. This paper summarizes the three-dimensional space discrete point fitting plane algorithm, and gives the algorithm implementation codes in several programming languages.

1. Algorithm principle

  (1) Least square method
  

  (2) Plane equation fitting
  

2. Algorithm implementation

  （1）C#

using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;  
  
List<List<double>> dLL =new List<List<double>>();  
using (StreamReader sr = new StreamReader("E:\\4.txt", Encoding.UTF8))  
            {
    
      
                string line;  
                // 从文件读取并显示行，直到文件的末尾   
                while ((line = sr.ReadLine()) != null)  
                {
    
      
                    string[] strs = line.Split(',');  
                    List<double> dL = new List<double>();  
                    dL.Add(double.Parse(strs[0]));  
                    dL.Add(double.Parse(strs[1]));  
                    dL.Add(double.Parse(strs[2]));  
                    dLL.Add(dL);  
                }  
    }  
  
Matrix<double> A,b;  
double[,] dA=new double[dLL.Count(), 3];  
double[,] db = new double[dLL.Count(), 1];  
double[,] da = new double[3, 1];  
for (int i = 0; i < dLL.Count(); i++)  
{
    
      
    dA[i, 0] = dLL[i][0];  
    dA[i, 1] = dLL[i][1];  
    dA[i, 2] = 1;  
    db[i,0] = dLL[i][2];  
}  
A = DenseMatrix.OfArray(dA);  
b = DenseMatrix.OfArray(db);  
Matrix<double> a = (A.Transpose() * A).Inverse() * A.Transpose() * b;  
Console.WriteLine("a0,a1,a2:"+a[0,0].ToString("f6")+","+a[1,0].ToString("f6") + ","+a[2,0].ToString("f6"));  

  （2）C++

//planePoints存储需要拟合的三维点云
vector<Eigen::Vector3d> planePoints;  
Eigen::MatrixXd A(planePoints.size(), 3);  
Eigen::VectorXd b(planePoints.size());  
//将观测点输入矩阵  
for (int i = 0; i < planePoints.size(); i++)  
{
    
      
    A(i, 0) = planePoints[i](0);  
    A(i, 1) = planePoints[i](1);  
    A(i, 2) = 1;  
    b(i) = planePoints[i](2);  
}  
  
//使用最小二乘法求得系数向量  
Eigen::Vector3d a = (A.transpose()*A).inverse()*A.transpose()*b;  

  （3）Matlab

%文件名
fileName = "E:\\4.txt";  
points = csvread(fileName , 0, 0);  
length = size(points(:,1));  
A=[points(:,1),points(:,2),ones(length(1),1)];  
b=points(:,3);  
a=inv(A'*A)*A'*b;  

  （4）Java

//s为点文件数据字符串
String[] strs =  s.toString().split("\n");  
double dA[][] = new double[strs.length][3];  
double db[][] = new double[strs.length][1];  
double da[][] = new double[3][1];  
for(int i = 0;i <strs.length;i++){
    
      
    if(strs[i].equals(""))continue;  
    String[] strs2 = strs[i].split(",");  
    dA[i][0]=Double.parseDouble(strs2[0]);  
    dA[i][1]=Double.parseDouble(strs2[1]);  
    dA[i][2] = 1;  
    db[i][0]=Double.parseDouble(strs2[2]);  
}  
//multiply、inverse、transpose分别为矩阵乘法、求逆、转置  
da = multiply(multiply(inverse(multiply(transpose(dA),dA)),transpose(dA)),db);  

  （5）VBA

Imports MathNet.Numerics.LinearAlgebra
Imports MathNet.Numerics.LinearAlgebra.Double  
  
Dim arr() As String, i As Long  
        arr = Split(CreateObject("scripting.filesystemobject").opentextfile("E:\\4.txt").readall.ToString(), vbLf)  
        Dim dA(UBound(arr), 2) As Double, db(UBound(arr), 0) As Double  
        Dim str() As String  
        For i = 0 To UBound(arr)  
            'ReDim Preserve Txt(i)  
            If arr(i) = "" Then  
                Continue For  
            End If  
            str = Split(arr(i), ",")  
            dA(i, 0) = Convert.ToDouble(str(0))  
            dA(i, 1) = Convert.ToDouble(str(1))  
            dA(i, 2) = 1  
            db(i, 0) = Convert.ToDouble(str(2))  
        Next  
        Dim A, b, ma As Matrix  
        A = DenseMatrix.OfArray(dA)  
        b = DenseMatrix.OfArray(db)  
        ma = (A.Transpose() * A).Inverse() * A.Transpose() * b  
        Console.WriteLine("a0,a1,a2:" + ma(0, 0).ToString("f6") + "," + ma(1, 0).ToString("f6") + "," + ma(2, 0).ToString("f6"))

  （6）Python

from numpy import *;
  
f=open('E:\\4.txt', encoding='gbk')  
txt=[]  
strs = []  
A = []  
b = []  
a = []  
for line in f:  
    strs=line.strip().split(',')  
    A.append([float(strs[0]),float(strs[1]),1])  
    b.append([float(strs[2])])  
A = mat(A)  
b = mat(b)  
a = (A.T*A).I*A.T*b  
print(a)

Reference materials:
[1] HIIWAR_ZB. Least square method - fitting plane equation (depth camera external parameter calibration, ground calibration) ; 2020-06-23 [accessed 2023-06-25].
[2] Haha kls. Minimum two Multiplication fitting plane ; 2018-09-10 [accessed 2023-06-25].
[3] Take off the coat to see the essence. Three-dimensional space discrete point plane fitting algorithm C++ implementation ; 2019-03-07 [accessed 2023-06- 25].