projectPoints with matrix / 投影計算をマトリクスで行う

projectPoints_with_matrix.py

# カメラ内部パラメータ(内部パラメータ行列)
# ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
# -----reading camera intrinsic matrix
cameraMatrix = np.loadtxt("calib/cameraMatrix.csv",delimiter=",")
distCoeffs = np.loadtxt("calib/distCoeffs.csv",delimiter=",")


# 外部パラメータ行列
# https://gist.github.com/shks/e3cd5fc22ad9f4ddbccd5312a64803c3
# -----load camera extrinsic matrix
RTmatrix34_L = np.load('PM_L.npy')
RTmatrix34_R = np.load('PM_R.npy')

#例として3次元点
point = np.array([0,0.5,-4,1])


# ---- simulating camera rendering process
# K*[R|T] 世界座標系から、透視投影変換
KRT_L = cameraMatrix.dot(RTmatrix34_L)

# 透視投影変換
projected_PL = KRT_L.dot(point.T)

# 同次座標系
projected_PL = projected_PL / projected_PL[2]
print('projected_PL', projected_PL)