python opencv最小化重投影误差求解PnP(高斯牛顿法)

使用jupyter (参考视觉SLAM十四讲第二版第七章源码)

 1.ORB特征点提取和配准 得到3D坐标-2D坐标

import cv2 as cv
import numpy as np
from  matplotlib import pyplot as plt

# 读取图片 以及深度
im1 = cv.imread('./data/1.png')
im2 = cv.imread('./data/2.png')
im1_d = cv.imread('./data/1_depth.png',-1)

# ORB特征提取
orb = cv.ORB_create()
kp1 = orb.detect(im1)  
kp2 = orb.detect(im2)
kp1, des1 = orb.compute(im1, kp1) # 求特征
kp2, des2 = orb.compute(im2, kp2) # 求特征
bf = cv.DescriptorMatcher_create('BruteForce-Hamming')
matches = bf.match(des1, des2)    # 配准
# 进行初步筛选
min_distance = 10000
max_distance = 0
for x in matches:
    if x.distance < min_distance: min_distance = x.distance
    if x.distance > max_distance: max_distance = x.distance
print('最小距离：%f' % min_distance)
print('最大距离：%f' % max_distance)
good_match = []
for x in matches:
    if x.distance <= max(2 * min_distance, 30):
        good_match.append(x)
print('匹配数：%d' % len(good_match))

# 提取配准点
points1 = []
points2 = []
for i in good_match:
    points1.append(list(kp1[i.queryIdx].pt));
    points2.append(list(kp2[i.trainIdx].pt));
points1 = np.array(points1).astype(int)
points2 = np.array(points2).astype(int)
#print(im1_d.shape)

## 提取深度 再次筛选
K = np.array([520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1]).reshape([3,-1]) # 相机参数
point1_3D = []
point2_2D = []
for i in range(points1.shape[0]):
    p = points1[i]
    d = im1_d[p[1],p[0]]/5000.0 # 深度距离  以5000为一个单位
    if d==0 :continue          # 无深度信息 跳过
    p = (p - (K[0][2],K[1][2]))/(K[0][0],K[1][1])*d  # 归一化坐标 根据深度单位转为实际坐标
    point1_3D.append([p[0],p[1],d])
    point2_2D.append(points2[i])
    #print([p[0],p[1],d])
    #print(d)
print('最终匹配数：%d' % len(point1_3D))
point1_3D = np.array(point1_3D).astype(np.float64)
point2_2D = np.array(point2_2D).astype(np.float64)
print(point1_3D[0:5])
#print(point2_2D.shape)

# 最小化重投影误差求解PnP 使用高斯牛顿法迭代求解 
pose = np.array([0,0,0,0,0,0], dtype=np.float64)    # 初始位姿 
K = np.array([520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1]).reshape([3,-1]) # 相机参数
last_cost = 0.0   # 损失
fx = K[0,0]
fy = K[1,1]
cx = K[0,2]
cy = K[1,2]
for iter in range(10):
    cost = 0
    H = 0
    b = 0
    for i in range(point1_3D.shape[0]):
        R,j = cv.Rodrigues(pose[3:6])   # 旋转矩阵
        t = pose[0:3]                   # 平移矩阵
        p_3D = np.dot(R,point1_3D[i])+t # 3D坐标点
        X = p_3D[0]                     #
        Y = p_3D[1]                     #
        inv_z = 1.0/p_3D[2]             # 深度距离
        inv_z2 = inv_z*inv_z            # 深度距离的平方
        p_2D = np.dot(K,p_3D)[0:2]*inv_z# 重投影   一开始忘记归一化深度了
        e = point2_2D[i] - p_2D         # 误差向量 一开始写反了  cost一直增大
        cost += np.linalg.norm(e)       # 向量的模 累加
        J = [[-fx * inv_z,           0, fx * X * inv_z2,      fx * X * Y * inv_z2, -fx - fx * X * X * inv_z2,  fx * Y * inv_z],
             [          0, -fy * inv_z, fy * Y * inv_z2, fy + fy * Y * Y * inv_z2,      -fy * X * Y * inv_z2, -fy * X * inv_z]]
        J = np.array(J)          # 以上建立雅克比矩阵
        H += np.dot(J.T,J)       # 得到H
        b += np.dot(-J.T,e)      # 得到b         

    if iter>0 and cost>=last_cost:# 误差变大则退出
        print('cost = ', cost,' last_cost = ',last_cost)
        break
    last_cost = cost
    if np.linalg.det(H) == 0:     # 奇异矩阵 
        print('Singular matrix')
        break
    x = np.linalg.solve(H,b)     # 求解方程组 得到增量
    pose += x                    # 更新位姿
    #print(np.linalg.norm(x))
    if np.linalg.norm(x) < 1e-6: # 增量很小 
        print(x)
        break
    print('%d: %f'%(iter,cost))

R,j = cv.Rodrigues(pose[3:6])
print('R = ', R)
print('t = ', pose[0:3])