3D 平面拟合方法（C++ 与 Python）

Question

Given :

I have a set of 3D points in a csv file.

   X,  Y,  Z
  x0, y0, z0
  x1, y1, z1
     ...
  xn, yn, zn

Problem Statement : The objective is to fit a plane based on the Least square error. Get the Error distance between the point and the plane. I have freedom to choose python or C++. I prefer c++ but python is fine too.

Equation of the Plane is:

                         Ax + By + Cz + D = 0                            -- Equation 1

Option 1 : C++ Way

I found this link online, describing a c++ way to solve the plane fitting http://www.janssenprecisionengineering.com/downloads/Fit-plane-through-data-points.pdf But here instead of using the equation 1, they use equation2

                         Z = A'x + B'y + C'                               -- Equation 2

                        where,  A' = -A/C
                                B' = -B/C
                                C' = -D/C

Once I know the A' B' C', I will be able to calculate the distance from each point to the plane based on equation 3 (Reference from -> Math Insight ):

                   Error = abs(A * x + B * y - z + C) / sqrt(pow(A, 2) + pow(B, 2) + 1);   -- Equation 3

I started to implement it with C++

    std::ofstream abc;
    abc.open("Logs\\abc.csv"); 

    cv::Mat Plane;
    double Xi = 0;
    double Yi = 0;
    double Zi = 0;

    double X2i = 0;
    double Y2i = 0;

    double XiYi = 0;
    double XiZi = 0;
    double YiZi = 0;

    for (int o = 0; o < GL.PointX.size(); ++o) {

        std::cout << "POINT X : " << GL.PointX[o] << std::endl;
        std::cout << "POINT Y : " << GL.PointY[o] << std::endl;
        std::cout << "POINT Z : " << GL.PointZ[o] << std::endl;

        Xi = Xi + GL.PointX[o];
        Yi = Yi + GL.PointY[o];
        Zi = Zi + GL.PointZ[o];

        X2i = X2i + (GL.PointX[o] * GL.PointX[o]);
        Y2i = Y2i + (GL.PointY[o] * GL.PointY[o]);

        XiYi = XiYi + (GL.PointX[o] * GL.PointY[o]);
        XiZi = XiZi + (GL.PointX[o] * GL.PointZ[o]);
        YiZi = YiZi + (GL.PointY[o] * GL.PointZ[o]);

    }

    cv::Mat PlaneA_Mat = (cv::Mat_<double>(3, 3) << X2i, XiYi, Xi, XiYi, Y2i, Yi, Xi, Yi, 1);
    cv::Mat PlaneB_Mat(3, 1, CV_64FC1);
    double R = 0, R2 = 0, FR2=0;
    int Observation = 70;
    PlaneB_Mat.at<double>(0, 0) = XiZi;
    PlaneB_Mat.at<double>(1, 0) = YiZi;
    PlaneB_Mat.at<double>(2, 0) = Zi;

    Plane = PlaneA_Mat.inv() * PlaneB_Mat;



    double A = Plane.at<double>(0, 0); //-A/C
    double B = Plane.at<double>(1, 0); //-B/C
    double C = Plane.at<double>(2, 0); //-D/C


    abc << A << "," << B << "," << "-1" << "," << C;
    abc <<"\n";


    double Dsum = 0;
    for (int o = 0; o < GL.PointX.size(); ++o) {
        double Error = abs(A * GL.PointX[o] + B * GL.PointY[o] - GL.PointZ[o] + C) / sqrt(pow(A, 2) + pow(B, 2) + 1);
        std::cout << "Error : " << Error << std::endl;
        Error_projection << Error ;
        Error_projection << "\n";
        Dsum = Dsum + Error;
        GL.D.push_back(Error);
    }
    R = (Observation * XiYi - Xi * Yi) / sqrt((Observation * X2i - Xi * Xi) * (Observation * Y2i - Yi * Yi));
R2 = pow(R, 2);

FR2 = pow(Zi - Dsum, 2) / pow(Zi - (1 / Observation)*Zi, 2);


std::cout << "PlaneA_Mat : " << PlaneA_Mat << std::endl;
std::cout << "PlaneB_Mat: " << PlaneB_Mat << std::endl;
std::cout << "FINAL PLANE: " << Plane << std::endl;
std::cout << "R: " << R << std::endl;
std::cout << " Correlation coefficient (R^2) : " << R2 << std::endl;
std::cout << " Final Correlation coefficient (FR^2) : " << FR2 << std::endl;

The result I get is :

  A : 12.36346708893272
  B : 0.07867292340114898
  C : -3.714791111490779

And also got the errors for each point relative to the plane. The Error values are very big .

*Option 2 : Python Way *

Then I started to look into web to find some code and found this --> 3D Plane Fit code. I modified according to my needs and this works perfectly.

import numpy as np
import scipy.optimize

from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import pandas



fig = plt.figure()
ax = fig.gca(projection='3d')

def fitPlaneLTSQ(XYZ):
    (rows, cols) = XYZ.shape
    print(rows, cols)
    G = np.ones((rows, 3))
    print(XYZ)
    print(XYZ[0])
    G[:, 0] = XYZ[0]  # X
    G[:, 1] = XYZ[1]  # Y
    print(G)
    Z = XYZ[2]
    (a, b, c), resid, rank, s = np.linalg.lstsq(G, Z)
    print("a : ", a )
    print("b : ", b )
    print("c : ", c)
    print("Residual : ", resid)
    print("Rank : ", rank)
    print("Singular Value", s)

    normal = (a, b, -1)                                             # I Don't Know WHY ?
    print("normal : ", normal)

    '''
    normala = abs(pow(a,2)+pow(b,2)+pow(-1,2))
    np.sqrt(normala)
    abb=normal/normala
    print("Normala : ",normala)
    print("Normala : ", abb)

    '''

    nn = np.linalg.norm(normal)                                       # I Don't Know WHY ?
    print("nn : ", nn)
    normal = normal/nn                                                # I Don't Know WHY ?

    print("Normal : ", normal)

    return (c, normal)


#Import Data from CSV
result = pandas.read_csv("C:/Users/Logs/points_L.csv", header=None) # , names=['X', 'Y','Z']
#result =result.head(5)
print(result)

normal1 = pandas.read_csv("C:/Users/Logs/pose_left.csv", header=None) # , names=['X', 'Y','Z']
print(normal1)

abc = pandas.read_csv("C:/Users/Logs/abc.csv", header=None) # , names=['X', 'Y','Z']
print(abc)

#standard normal distribution / Bell.
#np.random.seed(seed=1)

data = result
#print(data)
print("NEW : ")
print(data)

c, normal = fitPlaneLTSQ(data)
print(c, normal)

# plot fitted plane
maxx = np.max(data[0])
maxy = np.max(data[1])
minx = np.min(data[0])
miny = np.min(data[1])
print(maxx,maxy, minx, miny)

point = np.array([0.0, 0.0, c])                                             # I Don't Know WHY ?
print("Point : ", point)
d = -point.dot(normal)                                                      # I Don't Know WHY ?
print("D : ",  d)

# plot original points
ax.scatter(data[0], data[1], data[2])
ax.quiver(data[0], data[1], data[2], normal1[0], normal1[1], normal1[2], length=0.2)
# compute needed points for plane plotting
xx, yy = np.meshgrid([minx, maxx], [miny, maxy])                           


print(xx)
print(yy)

print("minx : ", minx)
print("maxx : ", maxx)
print("miny : ", miny)
print("maxy : ", maxy)


print("xx : ", xx)
print("yy : ", yy)
z = (-normal[0]*xx - normal[1]*yy - d)*1. / normal[2]                   # I Don't Know WHY ?


unit1 = np.sqrt(pow(normal[0], 2) + pow(normal[1],2) + pow(normal[2],2))
print("Unit 1 : ", unit1)
Error = abs(normal[0]*data[0] + normal[1]*data[1] + normal[2]*data[2] + d)/unit1
print("Error", Error)
Error_F = pandas.DataFrame(Error)
print("Print : ", Error_F)
Error_F.to_csv("C:/Users/Logs/Py_Error.csv")


print("Z : ", z)
# plot plane
ax.plot_surface(xx, yy, z, alpha=0.2)
ax.set_xlim(-1, 1)
ax.set_ylim(-1,1)
ax.set_zlim(1,2)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()

It would be really helpful if you could answer the following :

1. In the Option 2 python code, I have commented # I Don't Know WHY ? could you provide the Mathematical reasoning behind why they have used it. Please care to explain it as if you are explaining to noob.
2. What is the problem with my C++ code? how can I get it to produce the same results as the python code?

3. Why my Final Correlation coefficient R^2 provides strange results?

    Thank You Very Much !

Answer 1

Well, this isn't an answer but it's too long for a comment. Another interpretation of 'distance between the point and the plane' is, for a point p

n.p + d

where the plane has equation

n.p + d = 0

and we assume

|n| = l

So if we have a collection of N points p[], we seek real d and a unit vector n to minimise

S = Sum{ i | (n.P[i] + d)*(n.P[i] + d) }

Some rather algebra derives the following method for computing this:

a/ compute Pbar, the mean point and Q, the 'covariance' of the P[] via

Pbar = Sum{ i | P[i]}/N
Q = Sum{ i | (P[i]-Pbar)*(P[i]-Pbar)' } / N

b/ compute n, and eigenvector of Q corresponding to a minimal eigenvalue.

c/ compute

d = -n'*Pbar