手写BA之优化Rt

最近用到了BA，所以记录一下，大家一起学习进步！
这里包含两个部分，一个是GN&BA优化，另一个是基于仿真数据的验证和测试，都是基于Python3

1、GN&BA代码

下面为 GN&BA代码。

import cv2
from itertools import product
import numpy as np
from scipy.spatial.transform import Rotation as R_scipy
from self_calibartion.optimization import Optim


def in_range(uv):
    u, v = uv
    if 0 < u < 1279. and 0 < v < 719:
        return True
    else:
        return False


K = np.array([[800, 0, 640],
              [0, 800, 360],
              [0, 0, 1.]], dtype=np.float)
D = np.zeros(4, dtype=np.float)

# pts on t[k-1]
x = np.linspace(-3, 3, 13)
y = np.linspace(-2, 7, 13)
z = np.linspace(5, 15, 21)
l_xyzs_all = []
for xi, yi, zi in product(x, y, z):
    l_xyzs_all.append([xi, yi, zi])
l_xyzs_all = np.array(l_xyzs_all, dtype=np.float)

# Rt groundtruth
# you can change 'rxyz_gt' and 'txyz_gt' to test this code
rxyz_gt = [-0.05, 0.03, 0.02]
txyz_gt = [-0.01, 0.02, 0.05]
R_gt = R_scipy.from_euler('xyz', rxyz_gt, degrees=True).as_matrix()
t_gt = np.array(txyz_gt, dtype=np.float).reshape((3, 1))

# pts on t[k]
r_xyzs_all = (np.dot(R_gt, l_xyzs_all.T) + t_gt).T

# projection
l_uvs_all, _ = cv2.projectPoints(l_xyzs_all, cv2.Rodrigues(np.zeros(3))[0], np.zeros(3), K, D)
r_uvs_all, _ = cv2.projectPoints(r_xyzs_all, cv2.Rodrigues(np.zeros(3))[0], np.zeros(3), K, D)
lr_uvs_diff = np.sum(l_uvs_all - r_uvs_all)

# get good pts and uvs
l_xyzs, r_xyzs = [], []
l_uvs, r_uvs = [], []
for i in range(len(l_xyzs_all)):
    if in_range(l_uvs_all[i][0]) and in_range(r_uvs_all[i][0]):
        l_xyzs.append(l_xyzs_all[i])
        r_xyzs.append(r_xyzs_all[i])
        l_uvs.append(l_uvs_all[i][0])
        r_uvs.append(r_uvs_all[i][0])
l_xyzs, r_xyzs = np.array(l_xyzs), np.array(r_xyzs)
l_uvs, r_uvs = np.array(l_uvs), np.array(r_uvs)
n = len(l_xyzs)

# optimizaiton
R_init = np.identity(3, dtype=np.float)
t_init = np.zeros((3, 1))
optim = Optim(l_xyzs, l_uvs, r_xyzs, r_uvs, R_init, t_init, K, False, 50)
R_l2r, t_l2r, err_vec_list, aveg_err = optim.optim()
r_l2r = R_scipy.from_matrix(R_l2r).as_euler('xyz', degrees=True)
out = 'gt result: %.6f %.6f %.6f %.6f %.6f %.6f' % (rxyz_gt[0], rxyz_gt[1], rxyz_gt[2], txyz_gt[0], txyz_gt[1], txyz_gt[2])
print('%s' % out)
out = 'BA result: %.6f %.6f %.6f %.6f %.6f %.6f' % (r_l2r[0], r_l2r[1], r_l2r[2], t_l2r[0], t_l2r[1], t_l2r[2])
print('%s' % out)

2、验证和测试代码

下面是BA测试代码 Test on Sim_data。

import sys
import cv2
from itertools import product
import numpy as np
from scipy.spatial.transform import Rotation as R_scipy
from optimization import Optim, in_range


K = np.array([[800, 0, 640],
              [0, 800, 360],
              [0, 0, 1.]], dtype=np.float)
D = np.zeros(4, dtype=np.float)

# pts on t[k-1]
x = np.linspace(-3, 3, 13)
y = np.linspace(-2, 7, 13)
z = np.linspace(5, 15, 21)
l_xyzs_all = []
for xi, yi, zi in product(x, y, z):
    l_xyzs_all.append([xi, yi, zi])
l_xyzs_all = np.array(l_xyzs_all, dtype=np.float)

# Rt groundtruth
# you can change 'rxyz_gt' and 'txyz_gt' to test this code
rxyz_gt = [-0.05, 0.05, 0.02]
txyz_gt = [-0.01, 0.01, 0.05]
R_gt = R_scipy.from_euler('xyz', rxyz_gt, degrees=True).as_matrix()
t_gt = np.array(txyz_gt, dtype=np.float).reshape((3, 1))

# pts on t[k]
r_xyzs_all = (np.dot(R_gt, l_xyzs_all.T) + t_gt).T

# projection
l_uvs_all, _ = cv2.projectPoints(l_xyzs_all, cv2.Rodrigues(np.zeros(3))[0], np.zeros(3), K, D)
r_uvs_all, _ = cv2.projectPoints(r_xyzs_all, cv2.Rodrigues(np.zeros(3))[0], np.zeros(3), K, D)
lr_uvs_diff = np.sum(l_uvs_all - r_uvs_all)

# get good pts and uvs
l_xyzs, r_xyzs = [], []
l_uvs, r_uvs = [], []
for i in range(len(l_xyzs_all)):
    if in_range(l_uvs_all[i][0]) and in_range(r_uvs_all[i][0]):
        l_xyzs.append(l_xyzs_all[i])
        r_xyzs.append(r_xyzs_all[i])
        l_uvs.append(l_uvs_all[i][0])
        r_uvs.append(r_uvs_all[i][0])
l_xyzs, r_xyzs = np.array(l_xyzs), np.array(r_xyzs)
l_uvs, r_uvs = np.array(l_uvs), np.array(r_uvs)
n = len(l_xyzs)

# optimizaiton
R_init = np.identity(3, dtype=np.float)
t_init = np.zeros((3, 1))
optim = Optim(l_xyzs, l_uvs, r_xyzs, r_uvs, R_init, t_init, K, False, 50)
R_l2r, t_l2r, err_vec_list, aveg_err = optim.optim()
r_l2r = R_scipy.from_matrix(R_l2r).as_euler('xyz', degrees=True)
out = 'gt result: %.6f %.6f %.6f %.6f %.6f %.6f' % (rxyz_gt[0], rxyz_gt[1], rxyz_gt[2], txyz_gt[0], txyz_gt[1], txyz_gt[2])
print('%s' % out)
out = 'BA result: %.6f %.6f %.6f %.6f %.6f %.6f' % (r_l2r[0], r_l2r[1], r_l2r[2], t_l2r[0], t_l2r[1], t_l2r[2])
print('%s' % out)

3、Result

0: loss: 34201.736 loss_prev: 1000000.000 delta_x_norm: 0.05478 lr: 0.5000
1: loss: 8555.679 loss_prev: 34201.736 delta_x_norm: 0.02740 lr: 0.5000
2: loss: 2141.406 loss_prev: 8555.679 delta_x_norm: 0.01370 lr: 0.5000
3: loss: 536.148 loss_prev: 2141.406 delta_x_norm: 0.00685 lr: 0.5000
4: loss: 134.260 loss_prev: 536.148 delta_x_norm: 0.00342 lr: 0.5000
5: loss: 33.624 loss_prev: 134.260 delta_x_norm: 0.00171 lr: 0.5000
6: loss: 8.421 loss_prev: 33.624 delta_x_norm: 0.00086 lr: 0.5000
7: loss: 2.109 loss_prev: 8.421 delta_x_norm: 0.00043 lr: 0.5000
8: loss: 0.528 loss_prev: 2.109 delta_x_norm: 0.00021 lr: 0.5000
9: loss: 0.132 loss_prev: 0.528 delta_x_norm: 0.00011 lr: 0.5000
10: loss: 0.033 loss_prev: 0.132 delta_x_norm: 0.00005 lr: 0.5000
11: loss: 0.008 loss_prev: 0.033 delta_x_norm: 0.0000 lr: 0.5000
break for small update_step
pts_num: 2496  aver reg_err: 0.000036
gt result: -0.050000 0.030000 0.020000 -0.010000 0.020000 0.050000
BA result: -0.049976 0.029985 0.019990 -0.009995 0.019990 0.049976

有问题欢迎大家留言交流～