Skip to content

Commit fc4faa9

Browse files
gitounigitouni
authored
Add files via upload
1 parent e9e1129 commit fc4faa9

File tree

7 files changed

+299
-17
lines changed

7 files changed

+299
-17
lines changed

‎script/demo_IBA_FS.py‎

Lines changed: 23 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -35,19 +35,19 @@ def options():
3535
parser = argparse.ArgumentParser()
3636
kitti_parser = parser.add_argument_group()
3737
kitti_parser.add_argument("--base_dir",type=str,default="/data/DATA/data_odometry/dataset/")
38-
kitti_parser.add_argument("--seq",type=int,default=0,choices=[i for i in range(11)])
38+
kitti_parser.add_argument("--seq",type=int,default=5,choices=[i for i in range(11)])
3939

4040
io_parser = parser.add_argument_group()
41-
io_parser.add_argument("--KeyFrameDir",type=str,default="../KITTI-00/KeyFrames")
42-
io_parser.add_argument("--FrameIdFile",type=str,default="../KITTI-00/FrameId.yml")
43-
io_parser.add_argument("--MapFile",type=str,default="../KITTI-00/Map.yml")
41+
io_parser.add_argument("--KeyFrameDir",type=str,default="../KITTI-05/KeyFrames")
42+
io_parser.add_argument("--FrameIdFile",type=str,default="../KITTI-05/FrameId.yml")
43+
io_parser.add_argument("--MapFile",type=str,default="../KITTI-05/Map.yml")
4444
io_parser.add_argument("--KeyFrameIdKey",type=str,default="mnId")
4545
io_parser.add_argument("--FrameIdKey",type=str,default="mnFrameId")
4646
io_parser.add_argument("--KeyPointsKey",type=str,default="mvKeysUn")
4747
io_parser.add_argument("--MapPointKey",type=str,default="mvpMapPointsId")
4848
io_parser.add_argument("--CorrKey",type=str,default="mvpCorrKeyPointsId")
49-
io_parser.add_argument("--index_i",type=int,default=75)
50-
io_parser.add_argument("--index_j",type=int,default=79)
49+
io_parser.add_argument("--index_i",type=int,default=1039)
50+
io_parser.add_argument("--index_j",type=int,default=1040)
5151
io_parser.add_argument("--debug_log",type=str,default="")
5252
io_parser.add_argument("--Twc_file",type=str,default="../Twc.txt")
5353
io_parser.add_argument("--Twl_file",type=str,default="../Twl.txt")
@@ -165,6 +165,7 @@ def gpr_approx(idx):
165165
tgt_pcd_arr = dataStruct.get_velo(tgt_file_index)[:,:3] # [N, 3]
166166
src_img = np.array(dataStruct.get_cam0(src_file_index)) # [H, W, 3]
167167
tgt_img = np.array(dataStruct.get_cam0(tgt_file_index)) # [H, W, 3]
168+
img_shape = src_img.shape[:2]
168169
src_pose = dataStruct.poses[src_file_index]
169170
tgt_pose = dataStruct.poses[tgt_file_index]
170171
camera_motion:np.ndarray = inv_pose(tgt_pose) @ src_pose # Tc2w * Twc1
@@ -174,6 +175,20 @@ def gpr_approx(idx):
174175
src_pcd_camcoord = nptran(src_pcd_arr, extran)
175176
proj_src_pcd, src_rev = npproj(src_pcd_camcoord, np.eye(4), intran, src_img.shape)
176177
src_pcd_camcoord = src_pcd_camcoord[src_rev]
178+
tgt_pcd_camcoord = nptran(src_pcd_camcoord, camera_motion)
179+
proj_tgt_pcd, tgt_rev = npproj(tgt_pcd_camcoord, np.eye(4), intran, tgt_img.shape)
180+
tgt_pcd_camcoord = tgt_pcd_camcoord[tgt_rev]
181+
plt.figure(dpi=200)
182+
plt.subplot(2,1,1)
183+
plt.title("Frame {} - {}".format(src_file_index+1, tgt_file_index+1))
184+
plt.imshow(src_img,cmap="Greys_r")
185+
plt.scatter(proj_src_pcd[:,0],proj_src_pcd[:,1],c=src_pcd_camcoord[:,-1],cmap='rainbow_r',alpha=0.8,s=0.5)
186+
plt.axis([0,img_shape[1],img_shape[0],0])
187+
plt.subplot(2,1,2)
188+
plt.imshow(tgt_img,cmap="Greys_r")
189+
plt.scatter(proj_tgt_pcd[:,0],proj_tgt_pcd[:,1],c=tgt_pcd_camcoord[:,-1],cmap='rainbow_r',alpha=0.8,s=0.5)
190+
plt.axis([0,img_shape[1],img_shape[0],0])
191+
plt.show()
177192
src_2d_kdtree = cKDTree(proj_src_pcd, leafsize=10)
178193
if args.depth_conflict:
179194
src_dists, src_pcd_querys = src_2d_kdtree.query(src_matched_pts, args.max_2d_nn, eps=1e-4, p=2, workers=-1)
@@ -228,8 +243,10 @@ def gpr_approx(idx):
228243
plt.subplot(2,1,1)
229244
plt.title("Frame {} - {} | Matched:{} | error:{:0.4}".format(src_file_index+1, tgt_file_index+1, tgt_matched_pts.shape[0],err))
230245
plt.imshow(draw_src_img)
246+
plt.axis([0,img_shape[1],img_shape[0],0])
231247
plt.subplot(2,1,2)
232248
plt.imshow(draw_tgt_img)
249+
plt.axis([0,img_shape[1],img_shape[0],0])
233250
plt.show()
234251

235252

‎script/demo_IBA_Mappoint.py‎

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -37,20 +37,20 @@ def options():
3737
parser = argparse.ArgumentParser()
3838
kitti_parser = parser.add_argument_group()
3939
kitti_parser.add_argument("--base_dir",type=str,default="/data/DATA/data_odometry/dataset/")
40-
kitti_parser.add_argument("--seq",type=int,default=0,choices=[i for i in range(11)])
40+
kitti_parser.add_argument("--seq",type=int,default=8,choices=[i for i in range(11)])
4141

4242
io_parser = parser.add_argument_group()
43-
io_parser.add_argument("--KeyFrameDir",type=str,default="../KITTI-00/KeyFrames")
44-
io_parser.add_argument("--FrameIdFile",type=str,default="../KITTI-00/FrameId.yml")
45-
io_parser.add_argument("--MapFile",type=str,default="../KITTI-00/Map.yml")
46-
io_parser.add_argument("--sim3_file",type=str,default="../KITTI-00/calib_res/iba_local_00.txt")
43+
io_parser.add_argument("--KeyFrameDir",type=str,default="../KITTI-07/KeyFrames")
44+
io_parser.add_argument("--FrameIdFile",type=str,default="../KITTI-07/FrameId.yml")
45+
io_parser.add_argument("--MapFile",type=str,default="../KITTI-07/Map.yml")
46+
io_parser.add_argument("--sim3_file",type=str,default="../KITTI-07/calib_res/gt_calib_07.txt")
4747
io_parser.add_argument("--KeyFrameIdKey",type=str,default="mnId")
4848
io_parser.add_argument("--FrameIdKey",type=str,default="mnFrameId")
4949
io_parser.add_argument("--KeyPointsKey",type=str,default="mvKeysUn")
5050
io_parser.add_argument("--MapPointKey",type=str,default="mvpMapPointsId")
5151
io_parser.add_argument("--CorrKey",type=str,default="mvpCorrKeyPointsId")
52-
io_parser.add_argument("--index_i",type=int,default=75)
53-
io_parser.add_argument("--index_j",type=int,default=76)
52+
io_parser.add_argument("--index_i",type=int,default=0)
53+
io_parser.add_argument("--index_j",type=int,default=1)
5454
io_parser.add_argument("--debug_log",type=str,default="")
5555
io_parser.add_argument("--Twc_file",type=str,default="../Twc.txt")
5656
io_parser.add_argument("--Twl_file",type=str,default="../Twl.txt")
@@ -59,7 +59,7 @@ def options():
5959
arg_parser.add_argument("--fps_sample",type=int,default=-1)
6060
arg_parser.add_argument("--pixel_corr_dist",type=float,default=1.5)
6161
arg_parser.add_argument("--max_2d_nn",type=int, default=30)
62-
arg_parser.add_argument("--max_2d_std",type=float,default=0.05)
62+
arg_parser.add_argument("--max_2d_std",type=float,default=0.07)
6363
arg_parser.add_argument("--max_depth_diff",type=float,default=0.1)
6464
arg_parser.add_argument("--gpr_radius",type=float,default=0.6)
6565
arg_parser.add_argument("--gpr_max_pts",type=int,default=30)
@@ -188,6 +188,7 @@ def gpr_approx(idx):
188188
scale = sim3_data[-1]
189189
extran = np.eye(4)
190190
extran[:3,:] = sim3_data[:12].reshape(3,4)
191+
# extran[1,-1] -= 0.7
191192
# extran = calibStruct.T_cam0_velo # [4,4]
192193
print("GT TCL:\n{}".format(extran))
193194
print("GT TCL Rvec:{}\ntvec:{}".format(*toVec(extran)))

‎script/edge.py‎

Lines changed: 95 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
import numpy as np
2+
import cv2
3+
4+
def canny(img:np.ndarray,T1:int=200,T2:int=50):
5+
if len(img.shape) == 3:
6+
img_ = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
7+
else:
8+
img_ = img.copy()
9+
# img_ = cv2.bilateralFilter(img_,9,50,150)
10+
# img_ = cv2.equalizeHist(img_)
11+
12+
return cv2.Canny(img_,T1,T2)
13+
14+
def inverse_distance_transform(edge:np.ndarray,alpha:float,gamma:float):
15+
inv_edge = np.array(edge==0,dtype=np.uint8) # edge: zero; non-edge: non-zero
16+
inv_map = cv2.distanceTransform(inv_edge,cv2.DIST_C,maskSize=5) # max(|x-i|,|y-j|)
17+
D = alpha*edge + (1-alpha)*255*gamma**inv_map
18+
return np.uint8(D)
19+
20+
21+
def cal_pitch(X:np.ndarray):
22+
R = np.sqrt(X[:,0]**2+X[:,1]**2) # [n,]
23+
pitch = np.arctan(X[:,2]/R) #[n,]
24+
return pitch
25+
26+
def cal_yaw(X:np.ndarray):
27+
x,y = X[:,0], X[:,1]
28+
yaw = np.arctan2(y,x) # invert x & y
29+
return yaw
30+
31+
def range_dif(x:np.ndarray):
32+
x_df, x_bk = np.zeros_like(x), np.zeros_like(x)
33+
x_df[:-1] = np.abs(x[1:] - x[:-1])
34+
x_bk[1:] = np.abs(x[:-1] - x[1:])
35+
return np.maximum(x_df,x_bk)
36+
37+
def yaw_diff(line_scan:np.ndarray):
38+
x = cal_yaw(line_scan)
39+
x_df, x_bk = np.zeros_like(x), np.zeros_like(x)
40+
x_df[:-1] = np.abs(x[1:] - x[:-1])
41+
x_bk[1:] = np.abs(x[:-1] - x[1:])
42+
return np.maximum(x_df, x_bk) * 180 / np.pi
43+
44+
45+
def sort_pcd(unsrt_pcd:np.ndarray,scan_num:int=16)->list:
46+
srt_pcd = list()
47+
pitch = np.degrees(cal_pitch(unsrt_pcd))
48+
scan_idx = np.array((pitch+15)/2+0.5,dtype=np.int32) # VLP16 refer to A-LOAM
49+
for i in range(scan_num):
50+
srt_pcd.append(unsrt_pcd[scan_idx==i])
51+
return srt_pcd
52+
53+
def sort_pcd_with_idx(unsrt_pcd:np.ndarray,scan_num:int=16)->list:
54+
srt_pcd = list()
55+
srt_idx = list()
56+
pitch = np.degrees(cal_pitch(unsrt_pcd))
57+
idx = np.arange(len(pitch))
58+
scan_idx = np.array((pitch+15)/2+0.5,dtype=np.int32) # VLP16 refer to A-LOAM
59+
for i in range(scan_num):
60+
mask = scan_idx==i
61+
srt_pcd.append(unsrt_pcd[mask])
62+
srt_idx.append(idx[mask])
63+
return srt_pcd, srt_idx
64+
65+
def pcd_dsc(pcd:np.ndarray,dis_thre=0.3, nan_extract=False, max_yaw_diff=0.5):
66+
srt_pcd = sort_pcd(pcd)
67+
dsc_pcd = np.empty((0,3),dtype=np.float32)
68+
for line_scan in srt_pcd:
69+
pcd_range = np.linalg.norm(line_scan,axis=1) # (n,)
70+
dif_pcd_range = range_dif(pcd_range)
71+
if(nan_extract):
72+
dif_yaw_deg = yaw_diff(line_scan)
73+
rev = np.logical_or(dif_pcd_range > dis_thre, dif_yaw_deg > max_yaw_diff)
74+
else:
75+
rev = (dif_pcd_range > dis_thre)
76+
dsc_pcd = np.vstack((dsc_pcd,line_scan[rev]))
77+
return dsc_pcd
78+
79+
def pcd_dsc_with_idx(pcd:np.ndarray, dis_thre=0.3, nan_extract=False, max_yaw_diff=0.5):
80+
srt_pcd, srt_idx = sort_pcd_with_idx(pcd)
81+
dsc_pcd = np.empty((0,3),dtype=np.float32)
82+
dst_idx = np.empty((0,3),dtype=np.int32)
83+
for line_scan,idx_scan in zip(srt_pcd,srt_idx):
84+
pcd_range = np.linalg.norm(line_scan,axis=1) # (n,)
85+
dif_pcd_range = range_dif(pcd_range)
86+
if(nan_extract):
87+
dif_yaw_deg = yaw_diff(pcd)
88+
rev = np.logical_or(dif_pcd_range > dis_thre, dif_yaw_deg > max_yaw_diff)
89+
else:
90+
rev = (dif_pcd_range > dis_thre)
91+
dsc_pcd = np.vstack((dsc_pcd,line_scan[rev]))
92+
dst_idx = np.vstack((dst_idx,idx_scan[rev]))
93+
return dsc_pcd, dst_idx
94+
95+

‎script/edge_corr_opt.py‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
import argparse
2+
import numpy as np
3+
from cv_tools import toVec
4+
from problem import nptrans,binary_projection,CorrLoss
5+
from edge import pcd_dsc
6+
import pykitti as pyk
7+
import os
8+
from scipy.optimize import minimize
9+
from tqdm import tqdm
10+
11+
os.chdir(os.path.abspath(os.path.dirname(__file__)))
12+
def options():
13+
parser = argparse.ArgumentParser()
14+
io_parser = parser.add_argument_group()
15+
io_parser.add_argument("--data_dir",type=str,default="/data/DATA/data_odometry/dataset/")
16+
io_parser.add_argument("--seq",type=int,default=0,choices=[0,2,3,4,5])
17+
io_parser.add_argument("--calib_dir",type=str,default="../KITTI-00/calib_res/")
18+
io_parser.add_argument("--init_calib",type=str,default="he_rb_calib_00.txt")
19+
io_parser.add_argument("--save_calib",type=str,default="edge_calib.csv")
20+
io_parser.add_argument("--data_skip",type=int,default=4)
21+
io_parser.add_argument("--pcd_skip",type=int,default=1)
22+
23+
runtime_parser = parser.add_argument_group()
24+
runtime_parser.add_argument("--pcd_diff_threshold",type=float,default=0.3)
25+
runtime_parser.add_argument("--pcd_diff_yaw",type=float,default=0.5)
26+
runtime_parser.add_argument("--canny_max",type=int,default=120)
27+
runtime_parser.add_argument("--canny_min",type=int,default=20)
28+
runtime_parser.add_argument("--inv_dis_alpha",type=float,default=0.33)
29+
runtime_parser.add_argument("--inv_dis_gamma",type=float,default=0.98)
30+
runtime_parser.add_argument("--max_powell_iter",type=int,default=100)
31+
runtime_parser.add_argument("--delta_x",type=float,nargs=6,default=[0.15,0.15,0.15,0.3,0.3,0.3])
32+
return parser.parse_args()
33+
34+
if __name__ == "__main__":
35+
args = options()
36+
init_calib = np.eye(4)
37+
init_calib_data = np.loadtxt(os.path.join(args.calib_dir, args.init_calib))[:12]
38+
init_calib[:3,:] = init_calib_data.reshape(3,4)
39+
pyk_data = pyk.odometry(args.data_dir,sequence="%02d"%args.seq)
40+
print("Sequence %02d has %d frames of data."%(args.seq, len(pyk_data)))
41+
intran = pyk_data.calib.K_cam0
42+
init_x0 = np.zeros(6)
43+
init_x0[:3], init_x0[3:] = toVec(init_calib)
44+
with open(os.path.join(args.calib_dir, args.save_calib),'w') as f:
45+
for i in tqdm(range(0,len(pyk_data),args.data_skip)):
46+
img = np.array(pyk_data.get_cam0(i))
47+
H, W = img.shape[:2]
48+
pcd:np.ndarray = pyk_data.get_velo(i)[::args.pcd_skip,:3]
49+
pcd = pcd_dsc(pcd, args.pcd_diff_threshold, True, args.pcd_diff_yaw)
50+
miscalib_pcd = nptrans(pcd.T, init_calib)
51+
u,v,mis_rev = binary_projection((H,W),intran,miscalib_pcd)
52+
Loss = CorrLoss(intran, (H,W), miscalib_pcd,
53+
img, args.canny_max, args.canny_min, args.inv_dis_alpha, args.inv_dis_gamma)
54+
bnds = [(x-dx,x+dx) for x,dx in zip(init_x0.tolist(),args.delta_x)]
55+
res = minimize(Loss.loss,init_x0,method="Nelder-Mead",bounds=bnds)
56+
f.write("{:0.4f} {:0.4f} {:0.4f} {:0.4f} {:0.4f} {:0.4f}\n".format(*res.x))
57+
58+

‎script/metric.py‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,8 @@
55

66
def options():
77
parser = argparse.ArgumentParser()
8-
parser.add_argument("--gt",type=str,default="../KITTI-02/calib_res/gt_calib_02.txt")
9-
parser.add_argument("--pred",type=str,default="../KITTI-02/calib_res/iba_global_bias_02.txt")
8+
parser.add_argument("--gt",type=str,default="../KITTI-05/calib_res/gt_calib_05.txt")
9+
parser.add_argument("--pred",type=str,default="../KITTI-05/calib_res/iba_global_baonly_05.txt")
1010
return parser.parse_args()
1111

1212
def inv_pose(pose:np.ndarray):
@@ -28,5 +28,8 @@ def inv_pose(pose:np.ndarray):
2828
tsl_err = err_ex[:3,3]
2929
R = Rotation.from_matrix(err_ex[:3,:3])
3030
rot_err = R.as_euler("zyx",True)
31+
print("roll:{},pitch:{},yaw:{}".format(*rot_err))
3132
print("x:{},y:{},z:{}".format(*tsl_err))
32-
print("roll:{},pitch:{},yaw:{}".format(*rot_err))
33+
print("Rotation RMSE:{} deg".format(np.linalg.norm(rot_err)))
34+
print("Translation RMSE:{} m".format(np.linalg.norm(tsl_err)))
35+

‎script/problem.py‎

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
import numpy as np
2+
import open3d as o3d
3+
from edge import inverse_distance_transform, canny
4+
from cv_tools import toMat
5+
6+
def voxel_downsample(pcd_arr:np.ndarray,voxel_size=0.05):
7+
pcd = o3d.geometry.PointCloud()
8+
pcd.points = o3d.utility.Vector3dVector(pcd_arr)
9+
pcd = pcd.voxel_down_sample(voxel_size)
10+
return np.array(pcd.points)
11+
12+
def pcd_projection(img_shape:tuple,intran:np.ndarray,pcd:np.ndarray,range:np.ndarray):
13+
H,W = img_shape
14+
proj_pcd = intran @ pcd
15+
u,v,w = proj_pcd[0,:], proj_pcd[1,:], proj_pcd[2,:]
16+
u = np.asarray(u/w,dtype=np.int32)
17+
v = np.asarray(v/w,dtype=np.int32)
18+
rev = (0<=u)*(u<W)*(0<=v)*(v<H)*(w>0)
19+
u = u[rev]
20+
v = v[rev]
21+
r = range[rev]
22+
return u,v,r,rev
23+
24+
def binary_projection(img_shape:tuple,intran:np.ndarray,pcd:np.ndarray):
25+
H,W = img_shape
26+
proj_pcd = intran @ pcd
27+
u,v,w = proj_pcd[0,:], proj_pcd[1,:], proj_pcd[2,:]
28+
u = np.asarray(u/w,dtype=np.int32)
29+
v = np.asarray(v/w,dtype=np.int32)
30+
rev = (0<=u)*(u<W)*(0<=v)*(v<H)*(w>0)
31+
return u,v,rev
32+
33+
def nptrans(pcd:np.ndarray,G:np.ndarray)->np.ndarray:
34+
R,t = G[:3,:3], G[:3,[3]] # (3,3), (3,1)
35+
return R @ pcd + t
36+
37+
38+
class CorrLoss:
39+
def __init__(self,intran:np.ndarray,img_shape:tuple,raw_pcd:np.ndarray,raw_img,
40+
canny_max:int=200, canny_min:int=20,
41+
alpha:float=0.33,gamma:float=0.98):
42+
self.intran = intran # (3,3)
43+
self.img_shape = img_shape # (2,) (H,W)
44+
edge = canny(raw_img,canny_max,canny_min)
45+
self.inv_map = np.array(inverse_distance_transform(edge,alpha,gamma),dtype=np.float32)/255. # (H,W) 0~1
46+
self.raw_pcd = raw_pcd
47+
48+
def loss(self,x:np.ndarray):
49+
Tr:np.ndarray = toMat(x[:3],x[3:])
50+
pred_pcd = nptrans(self.raw_pcd,Tr)
51+
u,v,rev = binary_projection(self.img_shape,self.intran,pred_pcd)
52+
depth_img = np.zeros(self.img_shape)
53+
depth_img[v[rev],u[rev]] = 1
54+
return -(depth_img*self.inv_map).mean()
55+
56+
def matloss(self,Tr:np.ndarray):
57+
pred_pcd = nptrans(self.raw_pcd,Tr)
58+
u,v,rev = binary_projection(self.img_shape,self.intran,pred_pcd)
59+
depth_img = np.zeros(self.img_shape)
60+
depth_img[v[rev],u[rev]] = 1
61+
return -(depth_img*self.inv_map).mean()
62+

0 commit comments

Comments
 (0)