python+opencv2相機位姿估計

最近在做基於圖像的室內定位方面的研究，於是使用到了百度最新的室內數據庫Image-based Localization (IBL) 。由於該數據庫給出的數據是每幅圖像和其對應相機的內外參數和光心投影方向，所以我需要先求出其6DOF預估姿態。再利用PoseNet網絡對其實現基於圖像的定位估計。好了，問題就很明確了：

(1)根據圖像和激光雷達參數的3D點雲實現2D-3D的匹配，找到每張圖像上的至少四個特征點。即找到至少4個二維像素和3D點雲點的對應點。

(2)根據這四組對應點和相機內外參數估計相機6DOF，即相機姿態。

今天先實現第二個問題。很幸運網上有這樣幾篇博客已經將相機位姿整個過程講的比較清楚了http://www.cnblogs.com/singlex/p/pose_estimation_1.html。

但這篇文章是由c++寫的，我在python上簡單的對其進行了驗證。

這是這張圖給出的數據。

import cv2
import numpy as np
import math
object_3d_points = np.array(([0, 0, 0],
                            [0, 200, 0],
                            [150, 0, 0],
                            [150, 200, 0]), dtype=np.double)
object_2d_point = np.array(([2985, 1688],
                            [5081, 1690],
                            [2997, 2797],
                            [5544, 2757]), dtype=np.double)
camera_matrix = np.array(([6800.7, 0, 3065.8],
                         [0, 6798.1, 1667.6],
                         [0, 0, 1.0]), dtype=np.double)
dist_coefs = np.array([-0.189314, 0.444657, -0.00116176, 0.00164877, -2.57547], dtype=np.double)
# 求解相機位姿
found, rvec, tvec = cv2.solvePnP(object_3d_points, object_2d_point, camera_matrix, dist_coefs)
rotM = cv2.Rodrigues(rvec)[0]
camera_postion = -np.matrix(rotM).T * np.matrix(tvec)
print(camera_postion.T)
# 驗證根據博客http://www.cnblogs.com/singlex/p/pose_estimation_1.html提供方法求解相機位姿
# 計算相機坐標系的三軸旋轉歐拉角，旋轉后可以轉出世界坐標系。旋轉順序z,y,x
thetaZ = math.atan2(rotM[1, 0], rotM[0, 0])*180.0/math.pi
thetaY = math.atan2(-1.0*rotM[2, 0], math.sqrt(rotM[2, 1]**2 + rotM[2, 2]**2))*180.0/math.pi
thetaX = math.atan2(rotM[2, 1], rotM[2, 2])*180.0/math.pi
# 相機坐標系下值
x = tvec[0]
y = tvec[1]
z = tvec[2]
# 進行三次旋轉
def RotateByZ(Cx, Cy, thetaZ):
    rz = thetaZ*math.pi/180.0
    outX = math.cos(rz)*Cx - math.sin(rz)*Cy
    outY = math.sin(rz)*Cx + math.cos(rz)*Cy
    return outX, outY
def RotateByY(Cx, Cz, thetaY):
    ry = thetaY*math.pi/180.0
    outZ = math.cos(ry)*Cz - math.sin(ry)*Cx
    outX = math.sin(ry)*Cz + math.cos(ry)*Cx
    return outX, outZ
def RotateByX(Cy, Cz, thetaX):
    rx = thetaX*math.pi/180.0
    outY = math.cos(rx)*Cy - math.sin(rx)*Cz
    outZ = math.sin(rx)*Cy + math.cos(rx)*Cz
    return outY, outZ
(x, y) = RotateByZ(x, y, -1.0*thetaZ)
(x, z) = RotateByY(x, z, -1.0*thetaY)
(y, z) = RotateByX(y, z, -1.0*thetaX)
Cx = x*-1
Cy = y*-1
Cz = z*-1
# 輸出相機位置
print(Cx, Cy, Cz)
# 輸出相機旋轉角
print(thetaX, thetaY, thetaZ)
# 對第五個點進行驗證
Out_matrix = np.concatenate((rotM, tvec), axis=1)
pixel = np.dot(camera_matrix, Out_matrix)
pixel1 = np.dot(pixel, np.array([0, 100, 105, 1], dtype=np.double))
pixel2 = pixel1/pixel1[2]
print(pixel2)

輸出結果

[[ 528.66321122 -2.88452091 358.60341802]]
[ 528.66321122] [-2.88452091] [ 358.60341802]
178.3558701005234 56.02221316618043 88.63218026484252
[ 4.15960851e+03 6.73694373e+02 1.00000000e+00]

驗證結果證明確實python代碼6行就求解出了相機6DOF位姿估計，厲害！通過驗證第5個點[0, 100, 105]對應像素點[4159.6, 673.69]和真實像素位置[4146, 673]相差不大。