计算相机投影矩阵（含代码）（Python）-Toy模板网

这篇具有很好参考价值的文章主要介绍了计算相机投影矩阵（含代码）（Python）。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

计算相机投影矩阵（含代码）（Python）

前几天处理点云时，需要使用到像片与3D点云的对应关系。在这边找了一圈没有发现直接可用的代码，于是去GitHub试了一下，以下是一个提炼后的矩阵计算代码。
下面展示 矩阵计算代码。

import warnings
import numpy as np
import os
import argparse
import cv2
import os
import glob

def calculate_projection_matrix(points_2d, points_3d):
    """
    To solve for the projection matrix. You need to set up a system of
    equations using the corresponding 2D and 3D points:

                                                      [ M11      [ u1
                                                        M12        v1
                                                        M13        .
                                                        M14        .
    [ X1 Y1 Z1 1 0  0  0  0 -u1*X1 -u1*Y1 -u1*Z1        M21        .
      0  0  0  0 X1 Y1 Z1 1 -v1*X1 -v1*Y1 -v1*Z1        M22        .
      .  .  .  . .  .  .  .    .     .      .       *   M23   =    .
      Xn Yn Zn 1 0  0  0  0 -un*Xn -un*Yn -un*Zn        M24        .
      0  0  0  0 Xn Yn Zn 1 -vn*Xn -vn*Yn -vn*Zn ]      M31        .
                                                        M32        un
                                                        M33 ]      vn ]

    Then you can solve this using least squares with np.linalg.lstsq() or SVD.
    Notice you obtain 2 equations for each corresponding 2D and 3D point
    pair. To solve this, you need at least 6 point pairs.

    Args:
    -   points_2d: A numpy array of shape (N, 2)
    -   points_3d: A numpy array of shape (N, 3)

    Returns:
    -   M: A numpy array of shape (3, 4) representing the projection matrix
    """

    # Placeholder M matrix. It leads to a high residual. Your total residual
    # should be less than 1.
    ###########################################################################
    ###########################################################################
    b=np.zeros(((int(2*points_3d.shape[0])),1))
    A=np.zeros((int(2*points_3d.shape[0]),12))
    x=0
    for i in range(0,points_3d.shape[0]):
    	A[x,0:3]=points_3d[i,:]
    	A[x,3]=1
    	A[x,8:11]=-points_2d[i,0]*points_3d[i,:]
    	A[x,11]=-points_2d[i,0]
    	A[x+1,4:7]=points_3d[i,:]
    	A[x+1,7]=1
    	A[x+1,8:11]=-points_2d[i,1]*points_3d[i,:]
    	A[x+1,11]=-points_2d[i,1]
    	x=x+2
    U,S,VT=np.linalg.svd(A)
    V=VT.T
    Mtemp=V[:,V.shape[1]-1]
    M=np.zeros((3,4))
    M[0,:]=Mtemp[:4]
    M[1,:]=Mtemp[4:8]
    M[2,:]=Mtemp[8:12]
    ###########################################################################
    ###########################################################################

    return M

data_dir = os.path.dirname(__file__) + '/data/'
Points_2D = np.loadtxt(data_dir + 'new2d.txt')
Points_3D = np.loadtxt(data_dir + 'new3d.txt')
P = calculate_projection_matrix(Points_2D,Points_3D)

print(P)