利用OpenCV实现摄像机坐标点云到世界坐标的齐次变换

我有相机校准的内部和外部(包括旋转和平移,即。 rvecs and tvecs ,对于一组N个摄影机姿势,相对于一个固定的ChArUco目标。

OpenCV camera coordinate system .

经过大量的阅读,我认为我需要首先计算每个相机相对于ChArUCo板的姿势,方法是在python中构建一个齐次变换矩阵:

    # initialize 4x4 transform
    inverted_homogeneous_transform_matrix = np.zeros(shape=(4,4))
    inverted_homogeneous_transform_matrix[3,3] = 1.0

    # convert Rodrigues vector into Rodrigues matrix, and then invert it
    rotation_matrix = np.zeros(shape=(3,3))
    cv2.Rodrigues(rvecs, rotation_matrix)
    inverted_rotation = rotation_matrix.transpose()

    # add inverted rotation to transform
    inverted_homogeneous_transform_matrix[:3,:3] = inverted_rotation

    # compute inverted translation, e.g. see http://ksimek.github.io/2012/08/22/extrinsic/
    inverted_translation_vector = -inverted_rotation * tvecs
    inverted_transform_matrix[:3,3] = np.asarray(inverted_translation_vector).flatten()

    # x_coords, y_coords, z_coords are defined in camera coordinate system
    x_coords=np.asarray([1,2,3,4,5])
    y_coords=np.asarray([2,4,6,8,10])
    z_coords=np.asarray([3,6,9,12,15])

    homogeneous_ones = np.ones(len(x_coords))
    homogeneous_points = np.matrix([x_coords, y_coords, z_coords, homogeneous_ones])

    # perform the transformation
    transformed_points = inverted_transform_matrix * homogeneous_points

    # clean up to extract x,y,z values from matrix and save as 1D array
    x_coords = np.asarray(transformed_points[0,:]).flatten()
    y_coords = np.asarray(transformed_points[1,:]).flatten()
    z_coords = np.asarray(transformed_points[1,:]).flatten()

基本上,上面的代码可以工作,但是当我从不同的相机视角在多个点云上运行它时,它们并不像我预期的那样神奇地排成一行。我可以确认,我的齐次逆变换确实是由rvecs和tvecs直接构造的齐次变换的逆;我试过把它分解成先平移后旋转,反之亦然;并且看到了一个非逆变换使所有的东西从一个角度几乎是一条直线(但是从其他角度看,所有的东西都奇怪地旋转着)。。。

感谢任何帮助!