3D 平面擬合方法（C++ 與 Python）

Question

鑒於：

我在 csv 文件中有一組 3D 點。

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

問題陳述：目標是根據最小二乘誤差擬合平面。 獲取點與平面之間的誤差距離。 我可以自由選擇 python 或 C++。 我更喜歡 c++，但 python 也很好。

平面方程為：

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

選項 1：C++ 方式

我在網上找到了這個鏈接，描述了一種解決平面擬合的 C++ 方法http://www.janssenprecisionengineering.com/downloads/Fit-plane-through-data-points.pdf但這里不是使用等式 1，而是使用等式 2

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

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

一旦我知道 A' B' C'，我將能夠根據等式 3（參考 -> Math Insight ）計算每個點到平面的距離：

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

我開始用 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;

我得到的結果是：

  A : 12.36346708893272
  B : 0.07867292340114898
  C : -3.714791111490779

並且還得到了相對於平面的每個點的誤差。 Error 值非常大。

*選項2：Python方式*

然后我開始查看網絡以找到一些代碼並找到了這個 --> 3D Plane Fit代碼。 我根據自己的需要進行了修改，效果很好。

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()

如果您能回答以下問題，那將非常有幫助：

1. 在選項 2 python 代碼中，我評論了# I Don't Know WHY ? 你能提供他們為什么使用它背后的數學推理嗎？ 請注意解釋，就像你在向菜鳥解釋一樣。
2. 我的 C++ 代碼有什么問題？ 我怎樣才能讓它產生與python代碼相同的結果？

3. 為什么我的最終相關系數 R^2 提供了奇怪的結果？

    Thank You Very Much !

Answer 1

好吧，這不是答案，但評論太長了。 “點與平面之間的距離”的另一種解釋是，對於點 p

n.p + d

其中平面有方程

n.p + d = 0

我們假設

|n| = l

所以如果我們有 N 個點 p[] 的集合，我們尋找實數 d 和一個單位向量 n 來最小化

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

一些相當的代數推導出以下計算方法：

a/ 計算 Pbar、均值點和 Q、P[] 的“協方差”通過

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

b/ 計算 n 和 Q 的特征向量對應於最小特征值。

c/ 計算

d = -n'*Pbar