Skip to content

Commit b4d11c8

Browse files
gitounigitouni
authored
Add files via upload
1 parent ff7264e commit b4d11c8

File tree

5 files changed

+269
-14
lines changed

5 files changed

+269
-14
lines changed

‎script/demo_IBA_FS_CMP.py‎

Lines changed: 201 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,201 @@
1+
import pykitti
2+
import numpy as np
3+
import argparse
4+
import os
5+
import cv2
6+
# import matplotlib
7+
# matplotlib.use('agg')
8+
from matplotlib import pyplot as plt
9+
from cv_tools import *
10+
from scipy.spatial import cKDTree
11+
from GP_reg import GPR
12+
from joblib import delayed, Parallel
13+
import yaml
14+
15+
os.chdir(os.path.dirname(__file__))
16+
17+
def print_params(title:str="", params:dict={}):
18+
print(title,end="")
19+
for key, value in params.items():
20+
print("{}: {}".format(key, value),end=" ")
21+
print()
22+
23+
def str2bool(s:str) -> bool:
24+
if s.isdigit():
25+
if float(s) > 0:
26+
return True
27+
else:
28+
return False
29+
if s.lower() == "false":
30+
return False
31+
else:
32+
return True
33+
34+
def options():
35+
parser = argparse.ArgumentParser()
36+
kitti_parser = parser.add_argument_group()
37+
kitti_parser.add_argument("--base_dir",type=str,default="/data/DATA/data_odometry/dataset/")
38+
kitti_parser.add_argument("--seq",type=int,default=4,choices=[i for i in range(11)])
39+
40+
io_parser = parser.add_argument_group()
41+
io_parser.add_argument("--KeyFrameDir",type=str,default="../KITTI-04/KeyFrames")
42+
io_parser.add_argument("--FrameIdFile",type=str,default="../KITTI-04/FrameId.yml")
43+
io_parser.add_argument("--MapFile",type=str,default="../KITTI-04/Map.yml")
44+
io_parser.add_argument("--KeyFrameIdKey",type=str,default="mnId")
45+
io_parser.add_argument("--FrameIdKey",type=str,default="mnFrameId")
46+
io_parser.add_argument("--KeyPointsKey",type=str,default="mvKeysUn")
47+
io_parser.add_argument("--MapPointKey",type=str,default="mvpMapPointsId")
48+
io_parser.add_argument("--CorrKey",type=str,default="mvpCorrKeyPointsId")
49+
io_parser.add_argument("--index_i",type=int,default=0)
50+
io_parser.add_argument("--index_j",type=int,default=1)
51+
io_parser.add_argument("--debug_log",type=str,default="")
52+
io_parser.add_argument("--extrinsic_file1",type=str,default="../KITTI-04/calib_res/he_rb_calib_04.txt")
53+
io_parser.add_argument("--extrinsic_file2",type=str,default="../KITTI-04/calib_res/iba_global_pl_04.txt")
54+
io_parser.add_argument("--Twc_file",type=str,default="../Twc.txt")
55+
io_parser.add_argument("--Twl_file",type=str,default="../Twl.txt")
56+
io_parser.add_argument("--save",type=str,default="../fig/04/cba_combine.png")
57+
arg_parser = parser.add_argument_group()
58+
arg_parser.add_argument("--fps_sample",type=int,default=50)
59+
arg_parser.add_argument("--pixel_corr_dist",type=float,default=1.5)
60+
arg_parser.add_argument("--max_2d_nn",type=int, default=10)
61+
args = parser.parse_args()
62+
args.seq_id = "%02d"%args.seq
63+
return args
64+
65+
66+
def get_fs_info(fs, keypt_nodename, mappt_nodename, corrkpt_nodename):
67+
kptnode = fs.getNode(keypt_nodename)
68+
keypts = [[int(kptnode.at(i).at(0).real()), int(kptnode.at(i).at(1).real())] for i in range(kptnode.size())]
69+
mapptnode = fs.getNode(mappt_nodename)
70+
mappt_indices = [int(mapptnode.at(i).real()) for i in range(mapptnode.size())]
71+
corrkpt_node = fs.getNode(corrkpt_nodename)
72+
corr_keypt_indices = [int(corrkpt_node.at(i).real()) for i in range(corrkpt_node.size())]
73+
return np.array(keypts), mappt_indices, corr_keypt_indices
74+
75+
def getMatchedId(src_mappt_indices:list, tgt_mappt_indices:list, src_corrkpt_indices:list, tgt_corrkpt_indices:list):
76+
src_matched_indices = []
77+
tgt_matched_indices = []
78+
for i in range(len(src_mappt_indices)):
79+
src_mappt_id = src_mappt_indices[i]
80+
if src_mappt_id in tgt_mappt_indices:
81+
tgt_id = tgt_mappt_indices.index(src_mappt_id)
82+
src_matched_indices.append(src_corrkpt_indices[i])
83+
tgt_matched_indices.append(tgt_corrkpt_indices[tgt_id])
84+
return np.array(src_matched_indices), np.array(tgt_matched_indices) # Keypoint Indices
85+
86+
87+
if __name__ == "__main__":
88+
np.random.seed(10) # color consistency
89+
args = options()
90+
dataStruct = pykitti.odometry(args.base_dir, args.seq_id)
91+
calibStruct = dataStruct.calib
92+
extran1 = np.eye(4)
93+
extran_raw1 = np.loadtxt(args.extrinsic_file1)[:12]
94+
extran1[:3,:] = extran_raw1.reshape(3,4)
95+
extran2 = np.eye(4)
96+
extran_raw2 = np.loadtxt(args.extrinsic_file2)[:12]
97+
extran2[:3,:] = extran_raw2.reshape(3,4)
98+
KeyFramesFiles = list(sorted(os.listdir(args.KeyFrameDir)))
99+
KeyFramesFiles = [file for file in KeyFramesFiles if os.path.splitext(file)[1] == '.yml']
100+
FrameIdCfg = yaml.load(open(args.FrameIdFile,'r'), yaml.SafeLoader)
101+
mnIds:list = FrameIdCfg[args.KeyFrameIdKey]
102+
mFrameIds:list = FrameIdCfg[args.FrameIdKey]
103+
assert(args.index_i in mnIds and args.index_j in mnIds), "{} and {} must be in mnIds".format(args.index_i, args.index_j)
104+
src_kffile_index = mnIds.index(args.index_i)
105+
tgt_kffile_index = mnIds.index(args.index_j)
106+
src_file_index = mFrameIds[src_kffile_index]
107+
tgt_file_index = mFrameIds[tgt_kffile_index]
108+
src_KeyFrameFile = os.path.join(args.KeyFrameDir, KeyFramesFiles[src_kffile_index])
109+
tgt_KeyFrameFile = os.path.join(args.KeyFrameDir, KeyFramesFiles[tgt_kffile_index])
110+
src_fs = cv2.FileStorage(src_KeyFrameFile, cv2.FILE_STORAGE_READ)
111+
tgt_fs = cv2.FileStorage(tgt_KeyFrameFile, cv2.FILE_STORAGE_READ)
112+
src_keypts, src_mappt_indices, src_corrkpt_indices = get_fs_info(src_fs, args.KeyPointsKey, args.MapPointKey, args.CorrKey)
113+
tgt_keypts, tgt_mappt_indices, tgt_corrkpt_indices = get_fs_info(tgt_fs, args.KeyPointsKey, args.MapPointKey, args.CorrKey)
114+
intran = calibStruct.K_cam0
115+
src_matched_pts_idx, tgt_matched_pts_idx = getMatchedId(src_mappt_indices, tgt_mappt_indices, src_corrkpt_indices, tgt_corrkpt_indices)
116+
src_pcd_arr = dataStruct.get_velo(src_file_index)[:,:3] # [N, 3]
117+
tgt_pcd_arr = dataStruct.get_velo(tgt_file_index)[:,:3] # [N, 3]
118+
src_img = np.array(dataStruct.get_cam0(src_file_index)) # [H, W, 3]
119+
tgt_img = np.array(dataStruct.get_cam0(tgt_file_index)) # [H, W, 3]
120+
img_shape = src_img.shape[:2]
121+
src_pose = dataStruct.poses[src_file_index]
122+
tgt_pose = dataStruct.poses[tgt_file_index]
123+
camera_motion:np.ndarray = inv_pose(tgt_pose) @ src_pose # Tc2w * Twc1
124+
src_matched_pts_raw = src_keypts[src_matched_pts_idx]
125+
tgt_matched_pts_raw = tgt_keypts[tgt_matched_pts_idx]
126+
print("Keypoint-Keypoint Matched:{}".format(len(src_matched_pts_raw)))
127+
src_pcd_camcoord = nptran(src_pcd_arr, extran1)
128+
proj_src_pcd, src_rev = npproj(src_pcd_camcoord, np.eye(4), intran, src_img.shape)
129+
src_pcd_camcoord = src_pcd_camcoord[src_rev]
130+
tgt_pcd_camcoord = nptran(src_pcd_camcoord, camera_motion)
131+
proj_tgt_pcd, tgt_rev = npproj(tgt_pcd_camcoord, np.eye(4), intran, tgt_img.shape)
132+
tgt_pcd_camcoord = tgt_pcd_camcoord[tgt_rev]
133+
src_2d_kdtree = cKDTree(proj_src_pcd, leafsize=10)
134+
src_dist, src_pcd_query = src_2d_kdtree.query(src_matched_pts_raw, 1, eps=1e-4, p=2, workers=-1)
135+
src_dist_rev = src_dist <= args.pixel_corr_dist**2
136+
src_matched_pts = src_matched_pts_raw[src_dist_rev]
137+
tgt_matched_pts = tgt_matched_pts_raw[src_dist_rev]
138+
src_pcd_query = src_pcd_query[src_dist_rev]
139+
src_pcd_3d_query = np.arange(src_pcd_camcoord.shape[0])
140+
src_pcd_3d_query = src_pcd_3d_query[src_pcd_query]
141+
sp_pts3d = src_pcd_camcoord[src_pcd_3d_query]
142+
src_proj_pcd, _ = npproj(sp_pts3d, np.eye(4), intran, src_img.shape)
143+
tgt_pcd_arr = nptran(sp_pts3d, camera_motion)
144+
tgt_proj_pcd, tgt_proj_rev = npproj(tgt_pcd_arr, np.eye(4), intran, tgt_img.shape)
145+
src_matched_pts = src_matched_pts[tgt_proj_rev]
146+
tgt_matched_pts = tgt_matched_pts[tgt_proj_rev]
147+
err1 = np.mean(np.sqrt(np.sum((tgt_matched_pts - tgt_proj_pcd)**2,axis=1)))
148+
Nmatch1 = src_matched_pts.shape[0]
149+
if args.fps_sample > 0:
150+
src_matched_pts, src_proj_pcd, fps_indices = fps_sample_corr_pts(src_matched_pts, src_proj_pcd, args.fps_sample, return_idx=True)
151+
tgt_matched_pts = tgt_matched_pts[fps_indices,:]
152+
tgt_proj_pcd = tgt_proj_pcd[fps_indices,:]
153+
draw_tgt_img1, _ = draw4corrpoints(tgt_img, src_img,
154+
*list(map(lambda arr: arr.astype(np.int32),[tgt_matched_pts, tgt_proj_pcd, src_matched_pts, src_proj_pcd])),
155+
5,8)
156+
157+
src_pcd_camcoord = nptran(src_pcd_arr, extran2)
158+
proj_src_pcd, src_rev = npproj(src_pcd_camcoord, np.eye(4), intran, src_img.shape)
159+
src_pcd_camcoord = src_pcd_camcoord[src_rev]
160+
tgt_pcd_camcoord = nptran(src_pcd_camcoord, camera_motion)
161+
proj_tgt_pcd, tgt_rev = npproj(tgt_pcd_camcoord, np.eye(4), intran, tgt_img.shape)
162+
tgt_pcd_camcoord = tgt_pcd_camcoord[tgt_rev]
163+
src_2d_kdtree = cKDTree(proj_src_pcd, leafsize=10)
164+
src_dist, src_pcd_query = src_2d_kdtree.query(src_matched_pts_raw, 1, eps=1e-4, p=2, workers=-1)
165+
src_dist_rev = src_dist <= args.pixel_corr_dist**2
166+
src_matched_pts = src_matched_pts_raw[src_dist_rev]
167+
tgt_matched_pts = tgt_matched_pts_raw[src_dist_rev]
168+
src_pcd_query = src_pcd_query[src_dist_rev]
169+
src_pcd_3d_query = np.arange(src_pcd_camcoord.shape[0])
170+
src_pcd_3d_query = src_pcd_3d_query[src_pcd_query]
171+
sp_pts3d = src_pcd_camcoord[src_pcd_3d_query]
172+
src_proj_pcd, _ = npproj(sp_pts3d, np.eye(4), intran, src_img.shape)
173+
tgt_pcd_arr = nptran(sp_pts3d, camera_motion)
174+
tgt_proj_pcd, tgt_proj_rev = npproj(tgt_pcd_arr, np.eye(4), intran, tgt_img.shape)
175+
src_matched_pts = src_matched_pts[tgt_proj_rev]
176+
tgt_matched_pts = tgt_matched_pts[tgt_proj_rev]
177+
err2 = np.mean(np.sqrt(np.sum((tgt_matched_pts - tgt_proj_pcd)**2,axis=1)))
178+
Nmatch2 = src_matched_pts.shape[0]
179+
if args.fps_sample > 0:
180+
src_matched_pts, src_proj_pcd, fps_indices = fps_sample_corr_pts(src_matched_pts, src_proj_pcd, args.fps_sample, return_idx=True)
181+
tgt_matched_pts = tgt_matched_pts[fps_indices,:]
182+
tgt_proj_pcd = tgt_proj_pcd[fps_indices,:]
183+
draw_tgt_img2, _ = draw4corrpoints(tgt_img, src_img,
184+
*list(map(lambda arr: arr.astype(np.int32),[tgt_matched_pts, tgt_proj_pcd, src_matched_pts, src_proj_pcd])),
185+
5,8)
186+
187+
plt.figure(dpi=200)
188+
plt.subplot(2,1,1)
189+
plt.title("Frame {} - {} | Matched:{} | Mean Error:{:0.2f}".format(src_file_index+1, tgt_file_index+1, Nmatch1,err1))
190+
plt.imshow(draw_tgt_img1)
191+
plt.axis([0,img_shape[1],img_shape[0],0])
192+
plt.axis('off')
193+
plt.subplot(2,1,2)
194+
plt.title("Frame {} - {} | Matched:{} | Mean Error:{:0.2f}".format(src_file_index+1, tgt_file_index+1, Nmatch2,err2))
195+
plt.imshow(draw_tgt_img2)
196+
plt.axis([0,img_shape[1],img_shape[0],0])
197+
plt.axis('off')
198+
plt.tight_layout(pad=0.5,h_pad=0,w_pad=0)
199+
plt.savefig(args.save)
200+
201+

‎script/demo_IBA_Mappoint.py‎

Lines changed: 28 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -37,13 +37,13 @@ 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=8,choices=[i for i in range(11)])
40+
kitti_parser.add_argument("--seq",type=int,default=4,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-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")
43+
io_parser.add_argument("--KeyFrameDir",type=str,default="../KITTI-04/KeyFrames")
44+
io_parser.add_argument("--FrameIdFile",type=str,default="../KITTI-04/FrameId.yml")
45+
io_parser.add_argument("--MapFile",type=str,default="../KITTI-04/Map.yml")
46+
io_parser.add_argument("--sim3_file",type=str,default="../KITTI-04/calib_res/iba_global_pl_04.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")
@@ -59,14 +59,15 @@ 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.07)
62+
arg_parser.add_argument("--max_2d_std",type=float,default=0.00)
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)
6666
arg_parser.add_argument("--gpr_max_cov",type=float,default=0.1)
67-
arg_parser.add_argument("--gpr",type=str2bool,default=True)
67+
arg_parser.add_argument("--gpr",type=str2bool,default=False)
6868
arg_parser.add_argument("--gpr_opt",type=str2bool, default=False)
6969
arg_parser.add_argument("--mp",type=str2bool,default=True)
70+
arg_parser.add_argument("--threshold",type=float,default=10.0)
7071
arg_parser.add_argument("--depth_conflict",type=str2bool,default=False)
7172
args = parser.parse_args()
7273
args.seq_id = "%02d"%args.seq
@@ -178,6 +179,15 @@ def gpr_approx(idx):
178179
sp_pts = np.array(parallel_res)
179180
return sp_pts
180181

182+
def change_background_to_black(vis):
183+
opt = vis.get_render_option()
184+
opt.background_color = np.asarray([0, 0, 0])
185+
return False
186+
187+
def change_background_to_white(vis):
188+
opt = vis.get_render_option()
189+
opt.background_color = np.asarray([1, 1, 1])
190+
return False
181191

182192
if __name__ == "__main__":
183193
np.random.seed(10) # color consistency
@@ -233,22 +243,29 @@ def gpr_approx(idx):
233243
src_matched_pts = src_matched_pts[src_dist_rev]
234244
tgt_matched_pts = tgt_matched_pts[src_dist_rev]
235245
mappoints = mappoints[src_dist_rev]
236-
print("Left Keypoints:{}".format(len(src_matched_pts)))
237246
src_3d_kdtree = cKDTree(src_pcd_camcoord, leafsize=30)
238247
dist, src_pcd_3d_query = src_3d_kdtree.query(mappoints,k=1,p=2,workers=-1)
239248
dist = np.sqrt(dist)
249+
dist_rev = dist < args.threshold
250+
dist = dist[dist_rev]
251+
src_pcd_3d_query = src_pcd_3d_query[dist_rev]
252+
mappoints = mappoints[dist_rev]
240253
print("Mean 3d-3d Dist:{}".format(dist.mean()))
254+
print("Matched mappoints:{}".format(len(mappoints)))
241255
sp_pts3d = src_pcd_camcoord[src_pcd_3d_query]
242256
src_pcd_o3d = o3d.geometry.PointCloud()
243257
src_pcd_o3d.points = o3d.utility.Vector3dVector(src_pcd_camcoord)
244-
src_pcd_o3d.paint_uniform_color([0,0,0])
258+
src_pcd_o3d.paint_uniform_color([0.8,0.7,0.2])
245259
mappoints_o3d = o3d.geometry.PointCloud()
246260
mappoints_o3d.points = o3d.utility.Vector3dVector(mappoints)
247-
mappoints_o3d.paint_uniform_color([1.0,0,0])
261+
mappoints_o3d.paint_uniform_color([1,0,0])
248262
sp_pts3d_o3d = o3d.geometry.PointCloud()
249263
sp_pts3d_o3d.points = o3d.utility.Vector3dVector(sp_pts3d)
250264
sp_pts3d_o3d.paint_uniform_color([0,1,1])
251-
o3d.visualization.draw_geometries([src_pcd_o3d, mappoints_o3d, sp_pts3d_o3d])
265+
key_to_callback = {}
266+
key_to_callback[ord("K")] = change_background_to_black
267+
key_to_callback[ord("B")] = change_background_to_white
268+
o3d.visualization.draw_geometries_with_key_callbacks([src_pcd_o3d,mappoints_o3d, sp_pts3d_o3d], key_to_callback)
252269
# src_proj_pcd, _ = npproj(sp_pts3d, np.eye(4), intran, src_img.shape)
253270
# tgt_pcd_arr = nptran(sp_pts3d, camera_motion)
254271
# tgt_proj_pcd, tgt_proj_rev = npproj(tgt_pcd_arr, np.eye(4), intran, tgt_img.shape)

‎script/draw_iba_func.py‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,5 @@
11
import argparse
22
import numpy as np
3-
from scipy.spatial.transform import Rotation
43
import os
54
os.chdir(os.path.abspath(os.path.dirname(__file__)))
65
import matplotlib.pyplot as plt

‎script/draw_mads_func.py‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
import argparse
2+
import numpy as np
3+
import os
4+
os.chdir(os.path.abspath(os.path.dirname(__file__)))
5+
import matplotlib.pyplot as plt
6+
7+
def options():
8+
parser = argparse.ArgumentParser()
9+
parser.add_argument("--ax_label_fontsize",type=int,default=14)
10+
parser.add_argument("--ax_legend_fontsize",type=int,default=14)
11+
parser.add_argument("--ax_marker_size",type=int,default=10)
12+
parser.add_argument("--he_threshold",type=float,default=0.03)
13+
parser.add_argument("--data",type=str,default="../KITTI-04/log/feas.txt")
14+
parser.add_argument("--res_dir",type=str,default="../fig/04/")
15+
parser.add_argument("--fig_size",type=float,nargs=2,default=[6.4,4.8])
16+
return parser.parse_args()
17+
18+
if __name__ == "__main__":
19+
args = options()
20+
parameters = {"xtick.labelsize":args.ax_label_fontsize,
21+
"ytick.labelsize":args.ax_label_fontsize,
22+
"font.size":args.ax_label_fontsize,
23+
"legend.fontsize":args.ax_legend_fontsize,
24+
"lines.markersize":args.ax_marker_size}
25+
plt.rcParams.update(parameters)
26+
fig = plt.figure(figsize=args.fig_size)
27+
data = np.loadtxt(args.data,usecols=(0,8,9,10))
28+
ifeas_mask = np.logical_or(data[:,-1]>0, data[:,-2]>0)
29+
feas_mask = np.logical_not(ifeas_mask)
30+
func = data[:,1][feas_mask]
31+
idx = data[:,0][feas_mask]
32+
plt.plot(idx,func,'r-',marker='.')
33+
plt.ylabel("loss")
34+
plt.xlabel("index")
35+
plt.xlim(right=1000*((idx[-1] // 1000) + 1))
36+
plt.tight_layout()
37+
plt.savefig("../fig/04/MADS_func.svg")
38+

‎script/metric.py‎

Lines changed: 2 additions & 2 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-07/calib_res/gt_calib_07.txt")
9-
parser.add_argument("--pred",type=str,default="../KITTI-07/calib_res/iba_global_pt3_07.txt")
8+
parser.add_argument("--gt",type=str,default="../KITTI-04/calib_res/gt_calib_04.txt")
9+
parser.add_argument("--pred",type=str,default="../KITTI-04/calib_res/iba_global_baonly_04.txt")
1010
return parser.parse_args()
1111

1212
def inv_pose(pose:np.ndarray):

0 commit comments

Comments
 (0)