计算相机投影矩阵（含代码）（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)
结语
代码源于
链接: AdityaNair111
最后感谢piong233的热心帮助
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
