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

最新推荐文章于 2023-01-04 15:55:40 发布

getalong

最新推荐文章于 2023-01-04 15:55:40 发布

阅读量1.3k

点赞数 2

分类专栏：投影文章标签： python 矩阵算法

本文链接：https://blog.csdn.net/getalong/article/details/127663454

版权

投影专栏收录该内容

1 篇文章 0 订阅

订阅专栏

计算相机投影矩阵（含代码）（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)