使用最小二乘的3D点和2D点之间的对应关系遇到了一个简单的问题

如何解决使用最小二乘的3D点和2D点之间的对应关系遇到了一个简单的问题

我正在尝试使用最小二乘法估算3D和2D点之间的投影矩阵。

我正在使用虚拟3D数据，从中获取相同的X和Y，并将X平移3个点/像素。尽管沿X轴进行了简单平移，但非线性最小二乘法scipy.optimize.least_squares难以估计投影矩阵。我是在做错事还是应该以不同的方式处理问题？

这是带有虚拟2D和3D数据的独立Python代码

import numpy as np
from scipy.optimize import least_squares
import matplotlib.pyplot as plt

initial_guess_K = np.array([[ 500,535],[   0,500,390],-1]])

initial_guess_R_T = np.array([[ 0.5,-1,0],-1],[   1,0.5,0]])

initial_guess_I_t = np.array([[   1,300],1,30]])

initial_guess_P = np.matmul(initial_guess_K,np.matmul(initial_guess_R_T,initial_guess_I_t))


translate_x = 3
points_3d = np.random.randint(500,size=(50,3))
#2D same as 3D,only translated by 3 points in x direction and without Z dimension
points_2d = np.column_stack((np.array(points_3d[:,0:1] + translate_x,dtype=int),points_3d[:,1:2]))

#project 3D point onto 2D
def projection(P,points_3d):
    p_1 = P[0,:]
    p_2 = P[1,:]
    p_3 = P[2,:]
    projected_points_2d = []
    for n in range(points_3d.shape[0]):
        points = points_3d[n,:]
        if points.shape[0] == 3:
            points = np.concatenate((points,np.array([1])))
        x = np.sum(p_1*points) / np.sum(p_3*points)
        y = np.sum(p_2*points) / np.sum(p_3*points)
        projected_points_2d.append([x,y])
    return np.array(projected_points_2d)

#lsq objective function
def objective_func(x,**kwargs):
    x = np.concatenate((x,np.array([1])))
    P = np.array([x[0:4],x[4:8],x[8:12]])

    points_2d = kwargs['pts2d']
    points_3d = kwargs['pts3d']
    proj = projection(P,points_3d)

    diff = proj - points_2d
    return diff.flatten()

#function to run least squares optimisation
def least_sq(pts2d,pts3d,initial_guess):
    dic = {}
    dic['pts2d'] = pts2d
    dic['pts3d'] = pts3d
    ls = least_squares(objective_func,initial_guess.flatten()[:11],method='lm',verbose=2,max_nfev=50000,loss='linear',kwargs=dic)
    M = np.concatenate((ls.x,np.array([1])))
    M = M.reshape((3,4))

    return M

#return 2d points and calculate residual
def evaluate_points(P,points_2d,points_3d):
    estimated_points_2d = projection(P,points_3d)

    residual = np.sum(np.hypot(estimated_points_2d[:,0] - points_2d[:,estimated_points_2d[:,1] - points_2d[:,1]))
    return estimated_points_2d,residual

#visualise real and estimated 2D points
def visualize_points_image(actual_pts,projected_pts):
    _,ax = plt.subplots()

    ax.scatter(actual_pts[:,actual_pts[:,1],c='red',marker='o',label='Actual points')
    ax.scatter(projected_pts[:,projected_pts[:,c='green',marker='+',label='Projected points')
    ax.set_ylim([0,500])
    ax.set_xlim([0,500])

    ax.legend()
    plt.show()


P = least_sq(points_2d,points_3d,initial_guess_P)

[projected_2d_pts,residual] = evaluate_points(P,points_3d);

print ("Residual",residual)

visualize_points_image(points_2d,projected_2d_pts)

编辑：我也尝试了线性最小二乘解（np.linalg.lstsq(X,Y)），它对平移和缩放均适用，但在平移缩放为非线性时会失败，例如什么时候：

import random
translate = random.randint(20,50)
translate2 = random.randint(0,10)
scale = random.random()
scale2 = random.random()*2
points_3d = np.random.randint(500,3))
#apply translation and scaling to xs < 250 and different translation and scaling to xs > 250
points_2d = np.column_stack(([item*scale+translate if item[0] < 250 else item*scale2+translate2 for item in points_3d[:,0:1]],np.array(points_3d[:,1:2] * scale + translate,dtype=int)))

n = points_3d.shape[0]
pad = lambda x: np.hstack([x,np.ones((x.shape[0],1))])
unpad = lambda x: x[:,:-1]
X = pad(points_3d)
Y = pad(points_2d)

A,res,rank,s = np.linalg.lstsq(X,Y)

transform = lambda x: unpad(np.dot(pad(x),A))

visualize_points_image(points_2d,transform(points_3d))