⼿写BA之优化Rt
最近⽤到了BA，所以记录⼀下，⼤家⼀起学习进步！
这⾥包含两个部分，⼀个是GN&BA优化，另⼀个是基于仿真数据的验证和测试，都是基于Python3 1、GN&BA代码 下⾯为 GN&BA代码。
import cv2
from itertools import product
import numpy as np
from ansform import Rotation as R_scipy
from self_calibartion.optimization import Optim
def in_range(uv):
u, v = uv
if0< u <1279.and0< 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.s(3))[0], np.zeros(3), K, D)
r_uvs_all, _ = cv2.projectPoints(r_xyzs_all, cv2.s(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)
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 ansform 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)
加工pcb板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.s(3))[0], np.zeros(3), K, D)
r_uvs_all, _ = cv2.projectPoints(r_xyzs_all, cv2.s(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.0500000.0300000.020000-0.0100000.0200000.050000
BA result:-0.0499760.0299850.019990-0.0099950.0199900.049976
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议