Question

基于以下主题：Calculating translation value and rotation angle of a rotated 2D image，我创建了一个python脚本，该脚本在源坐标系（传感器）中获取3D坐标并将其转换为目标坐标系（屏幕），该函数返回正确的转换矩阵据我所知（已在matlab中检查），但由于某种原因，当我将一个源数据点乘以所得的转换矩阵时，无法得到目标点。

这是完整的代码（底部是测试代码）：

"""
%% [calibMatrix] = CalculateTransformationMatrix(SourcePoints,TargetPoitns)
% This script calculates the 4x4 transformation matrix given at least 4 points
% in 3-D and their corresponding transformed coordinates
% SourcePoints: a N * 3 set of N points in 3D, in the source coordinate system
% first column is x, second is y, third is z
% TargetPoints: a N * 3 matrix of N points in the target coordinate system
% Need at least 4 points (N>=4) otherwise underdetermined system
"""
import numpy as np


def calcTransMat(SourcePoints, TargetPoints):
    N = SourcePoints.shape[0]

    if N < 4:
        CalibMat = np.zeros((4, 4))
        return CalibMat

    # Cast data as double to be sure
    SourcePoints = np.double(SourcePoints)
    TargetPoints = np.double(TargetPoints)

    # Extract relevant data
    xSource = SourcePoints[:, 0][np.newaxis]
    ySource = SourcePoints[:, 1][np.newaxis]
    zSource = SourcePoints[:, 2][np.newaxis]

    xTarget = TargetPoints[:, 0][np.newaxis]
    yTarget = TargetPoints[:, 1][np.newaxis]
    zTarget = TargetPoints[:, 2][np.newaxis]

    # create helper vectors
    vec0 = np.zeros(N)[np.newaxis]
    vec1 = np.ones(N)[np.newaxis]

    xSourcexTarget = -xSource.T * xTarget.T
    ySourcexTarget = -ySource.T * xTarget.T
    zSourcexTarget = -zSource.T * xTarget.T
    xSourceyTarget = -xSource.T * yTarget.T
    ySourceyTarget = -ySource.T * yTarget.T
    zSourceyTarget = -zSource.T * yTarget.T
    xSourcezTarget = -xSource.T * zTarget.T
    ySourcezTarget = -ySource.T * zTarget.T
    zSourcezTarget = -zSource.T * zTarget.T

    # Build A matrix
    A_1 = np.vstack((xSource.T, vec0.T, vec0.T))
    A_2 = np.vstack((ySource.T, vec0.T, vec0.T))
    A_3 = np.vstack((zSource.T, vec0.T, vec0.T))
    A_4 = np.vstack((vec1.T, vec0.T, vec0.T))
    A_5 = np.vstack((vec0.T, xSource.T, vec0.T))
    A_6 = np.vstack((vec0.T, ySource.T, vec0.T))
    A_7 = np.vstack((vec0.T, zSource.T, vec0.T))
    A_8 = np.vstack((vec0.T, vec1.T, vec0.T))
    A_9 = np.vstack((vec0.T, vec0.T, xSource.T))
    A_10 = np.vstack((vec0.T, vec0.T, ySource.T))
    A_11 = np.vstack((vec0.T, vec0.T, zSource.T))
    A_12 = np.vstack((vec0.T, vec0.T, vec1.T))
    A_13 = np.vstack((xSourcexTarget, xSourceyTarget, xSourcezTarget))
    A_14 = np.vstack((ySourcexTarget, ySourceyTarget, ySourcezTarget))
    A_15 = np.vstack((zSourcexTarget, zSourceyTarget, zSourcezTarget))

    A = np.hstack((A_1, A_2, A_3, A_4, A_5, A_6, A_7, A_8, A_9, A_10, A_11, A_12, A_13, A_14, A_15))

    # Build RHS vector
    b = np.vstack((xTarget.T, yTarget.T, zTarget.T))

    # Solve homography by least squares(this will return 15 * 1 vector)
    h, resid, rank, s = np.linalg.lstsq(A, b, rcond=None)

    # Reshape to a 4x4 matrix
    # Must transpose as reshape is performed in column major format
    h = np.append(h, 1)[np.newaxis].T  # Add in that h44 is 1 before we reshape
    CalibMat = np.reshape(h, (-1, 4))

    return CalibMat


Targets = np.array([[0.05, -0.1, 0],
                    [-0.1, 0 , 0.1],
                    [-0.2, 0, 0.2],
                    [-0.2, -0.15, 0],
                    [-0.1, -0.15, 0.1]])

Sources = np.array([[0.18951242, 0.2132337, -0.07491957],
                    [0.05004782, 0.29628516, -0.09634646],
                    [-0.05507707, 0.34084256, -0.05300118],
                    [-0.01769256, 0.18251753, -0.03618637],
                    [0.06959538, 0.23691658, 0.02355074]])


Calib = calcTransMat(Sources, Targets)
# Calib[3,] = [0, 0, 0, 1]
print Calib

print(np.matmul(np.array([0.18951242, 0.2132337, -0.07491957, 1]),Calib))

最后一行是通过将第一个源数据点（附加1）乘以校准矩阵来测试所得的校准矩阵，结果是：

[-1.48402025 -2.70472551 -0.6893453   0.92650296]

这是错误的。我知道代码不是很有效，但是由于我不是通过传感器在线进行的，所以我不介意它是否有点慢。我只是不确定为什么将源数据点乘以校准矩阵不会返回正确的目标点。

3D转换矩阵可在python中校准3D传感器

0 个答案: