def compute_pose_2d2d(self, kp_ref, kp_cur):
"""Compute the pose from view2 to view1
Args:
kp_ref (Nx2 array): keypoints for reference view
kp_cur (Nx2 array): keypoints for current view
Returns:
pose (SE3): relative pose from current to reference view
best_inliers (N boolean array): inlier mask
"""
principal_points = (self.cam_intrinsics.cx, self.cam_intrinsics.cy)
# initialize ransac setup
best_Rt = []
best_inlier_cnt = 0
max_ransac_iter = self.cfg.compute_2d2d_pose.ransac.repeat
best_inliers = np.ones((kp_ref.shape[0])) == 1
# check flow magnitude
avg_flow = np.mean(np.linalg.norm(kp_ref-kp_cur, axis=1))
min_flow = self.cfg.compute_2d2d_pose.min_flow
if avg_flow > min_flow:
for i in range(max_ransac_iter): # repeat ransac for several times for stable result
# shuffle kp_cur and kp_ref (only useful when random seed is fixed)
new_list = np.random.randint(0, kp_cur.shape[0], (kp_cur.shape[0]))
new_kp_cur = kp_cur.copy()[new_list]
new_kp_ref = kp_ref.copy()[new_list]
start_time = time()
E, inliers = cv2.findEssentialMat(
new_kp_cur,
new_kp_ref,
focal=self.cam_intrinsics.fx,
pp=principal_points,
method=cv2.RANSAC,
prob=0.99,
threshold=self.cfg.compute_2d2d_pose.ransac.reproj_thre,
)
cheirality_cnt, R, t, _ = cv2.recoverPose(E, new_kp_cur, new_kp_ref,
focal=self.cam_intrinsics.fx,
pp=principal_points,)
self.timers.timers["Ess. Mat."].append(time()-start_time)
if inliers.sum() > best_inlier_cnt and cheirality_cnt > 50:
best_Rt = [R, t]
best_inlier_cnt = inliers.sum()
best_inliers = inliers
if len(best_Rt) == 0:
R = np.eye(3)
t = np.zeros((3,1))
best_Rt = [R, t]
else:
R = np.eye(3)
t = np.zeros((3,1))
best_Rt = [R, t]
R, t = best_Rt
pose = SE3()
pose.R = R
pose.t = t
return pose, best_inliers
def __init__(self, cfg):
"""
Args:
cfg (edict): configuration reading from yaml file
"""
self.cam_intrinsics = Intrinsics()
# predicted global poses
self.global_poses = {0: SE3()}
# tracking stage
self.tracking_stage = 0
# configuration
self.cfg = cfg
# visualization interface
self.initialize_visualization_drawer()
self.ref_data = {
'id': [],
'timestamp': {},
'img': {},
'depth': {},
'raw_depth': {},
'pose': {},
'kp': {},
'kp_best': {},
'kp_list': {},
'pose_back': {},
'kp_back': {},
'flow': {}, # from ref->cur
'flow_diff': {}, # flow-consistency-error of ref->cur
}
self.cur_data = {
'id': 0,
'timestamp': 0,
'img': np.zeros(1),
'depth': np.zeros(1),
'pose': np.eye(4),
'kp': np.zeros(1),
'kp_best': np.zeros(1),
'kp_list': np.zeros(1),
'pose_back': np.eye(4),
'kp_back': np.zeros(1),
'flow': {}, # from cur->ref
}
def tracking_hybrid(self):
"""Tracking using both Essential matrix and PnP
Essential matrix for rotation (and direction);
*** triangluate depth v.s. CNN-depth for translation scale ***
PnP if Essential matrix fails
"""
# First frame
if self.tracking_stage == 0:
# initial
self.cur_data['pose'] = SE3(self.gt_poses[self.cur_data['id']])
self.tracking_stage = 1
return
# Second to last frames
elif self.tracking_stage >= 1:
# Flow-net for 2D-2D correspondence
start_time = time()
cur_data, ref_data = self.deep_flow_forward(
self.cur_data,
self.ref_data,
forward_backward=self.cfg.deep_flow.forward_backward)
self.timers.timers['Flow-CNN'].append(time() - start_time)
for ref_id in self.ref_data['id']:
# Compose hybrid pose
hybrid_pose = SE3()
# FIXME: add if statement for deciding which kp to use
# Essential matrix pose
E_pose, _ = self.compute_pose_2d2d(
cur_data['kp_best'],
ref_data['kp_best'][ref_id]) # pose: from cur->ref
# Rotation
hybrid_pose.R = E_pose.R
# translation scale from triangulation v.s. CNN-depth
if np.linalg.norm(E_pose.t) != 0:
scale = self.find_scale_from_depth(
cur_data['kp_best'], ref_data['kp_best'][ref_id],
E_pose.inv_pose, self.cur_data['depth'])
if scale != -1:
hybrid_pose.t = E_pose.t * scale
# PnP if Essential matrix fail
if np.linalg.norm(E_pose.t) == 0 or scale == -1:
pnp_pose, _, _ \
= self.compute_pose_3d2d(
cur_data['kp_best'],
ref_data['kp_best'][ref_id],
ref_data['depth'][ref_id]
) # pose: from cur->ref
# use PnP pose instead of E-pose
hybrid_pose = pnp_pose
self.tracking_mode = "PnP"
ref_data['pose'][ref_id] = copy.deepcopy(hybrid_pose)
# ref_data['pose'][ref_id] = hybrid_pose
self.ref_data = copy.deepcopy(ref_data)
self.cur_data = copy.deepcopy(cur_data)
# copy keypoint for visualization
self.ref_data['kp'] = copy.deepcopy(ref_data['kp_best'])
self.cur_data['kp'] = copy.deepcopy(cur_data['kp_best'])
# update global poses
pose = self.ref_data['pose'][self.ref_data['id'][-1]]
self.update_global_pose(pose, 1)
self.tracking_stage += 1
del (ref_data)
del (cur_data)
def compute_pose_3d2d(self, kp1, kp2, depth_1):
"""Compute pose from 3d-2d correspondences
Args:
kp1 (Nx2 array): keypoints for view-1
kp2 (Nx2 array): keypoints for view-2
depth_1 (HxW array): depths for view-1
Returns:
pose (SE3): relative pose from view-2 to view-1
kp1 (Nx2 array): filtered keypoints for view-1
kp2 (Nx2 array): filtered keypoints for view-2
"""
height, width = depth_1.shape
# Filter keypoints outside image region
x_idx = (kp2[:, 0] >= 0) * (kp2[:, 0] < width)
kp1 = kp1[x_idx]
kp2 = kp2[x_idx]
y_idx = (kp2[:, 1] >= 0) * (kp2[:, 1] < height)
kp1 = kp1[y_idx]
kp2 = kp2[y_idx]
# Filter keypoints outside depth range
kp1_int = kp1.astype(np.int)
kp_depths = depth_1[kp1_int[:, 1], kp1_int[:, 0]]
non_zero_mask = (kp_depths != 0)
depth_range_mask = (kp_depths < self.cfg.depth.max_depth) * (
kp_depths > self.cfg.depth.min_depth)
valid_kp_mask = non_zero_mask * depth_range_mask
kp1 = kp1[valid_kp_mask]
kp2 = kp2[valid_kp_mask]
# Get 3D coordinates for kp1
XYZ_kp1 = unprojection_kp(kp1, kp_depths[valid_kp_mask],
self.cam_intrinsics)
# initialize ransac setup
best_rt = []
best_inlier = 0
max_ransac_iter = self.cfg.PnP.ransac.repeat
for i in range(max_ransac_iter):
# shuffle kp_cur and kp_ref (only useful when random seed is fixed)
new_list = np.random.randint(0, kp2.shape[0], (kp2.shape[0]))
new_XYZ = XYZ_kp1.copy()[new_list]
new_kp2 = kp2.copy()[new_list]
if new_kp2.shape[0] > 4:
flag, r, t, inlier = cv2.solvePnPRansac(
objectPoints=new_XYZ,
imagePoints=new_kp2,
cameraMatrix=self.cam_intrinsics.mat,
distCoeffs=None,
iterationsCount=self.cfg.PnP.ransac.iter,
reprojectionError=self.cfg.PnP.ransac.reproj_thre,
)
if flag and inlier.shape[0] > best_inlier:
best_rt = [r, t]
best_inlier = inlier.shape[0]
pose = SE3()
if len(best_rt) != 0:
r, t = best_rt
pose.R = cv2.Rodrigues(r)[0]
pose.t = t
pose.pose = pose.inv_pose
return pose, kp1, kp2
def __init__(self, cfg):
"""
Args:
cfg (edict): configuration reading from yaml file
"""
# camera intrinsics
self.cam_intrinsics = Intrinsics()
# predicted global poses
self.global_poses = {0: SE3()}
# tracking stage
self.tracking_stage = 0
# configuration
self.cfg = cfg
# window size and keyframe step
self.window_size = 2
self.keyframe_step = 1
# visualization interface
self.initialize_visualization_drawer()
# timer
self.timers = Timers()
self.timers.add([
"img_reading",
"Depth-CNN",
"tracking",
"Ess. Mat.",
"Flow-CNN",
"visualization",
"visualization_traj",
"visualization_match",
"visualization_flow",
"visualization_depth",
"visualization_save_img",
])
# reference data and current data
self.ref_data = {
'id': [],
'timestamp': {},
'img': {},
'depth': {},
'raw_depth': {},
'pose': {},
'kp': {},
'kp_best': {},
'kp_list': {},
'pose_back': {},
'kp_back': {},
'flow': {}, # from ref->cur
'flow_diff': {}, # flow-consistency-error of ref->cur
}
self.cur_data = {
'id': 0,
'timestamp': 0,
'img': np.zeros(1),
'depth': np.zeros(1),
'pose': np.eye(4),
'kp': np.zeros(1),
'kp_best': np.zeros(1),
'kp_list': np.zeros(1),
'pose_back': np.eye(4),
'kp_back': np.zeros(1),
'flow': {}, # from cur->ref
}
def compute_pose_2d2d(self, kp_ref, kp_cur):
"""Compute the pose from view2 to view1
Args:
kp_ref (Nx2 array): keypoints for reference view
kp_cur (Nx2 array): keypoints for current view
Returns:
pose (SE3): relative pose from current to reference view
best_inliers (N boolean array): inlier mask
"""
principal_points = (self.cam_intrinsics.cx, self.cam_intrinsics.cy)
# validity check
valid_cfg = self.cfg.compute_2d2d_pose.validity
valid_case = True
# initialize ransac setup
best_Rt = []
best_inlier_cnt = 0
max_ransac_iter = self.cfg.compute_2d2d_pose.ransac.repeat
best_inliers = np.ones((kp_ref.shape[0])) == 1
if valid_cfg.method == "flow+chei":
# check flow magnitude
avg_flow = np.mean(np.linalg.norm(kp_ref-kp_cur, axis=1))
min_flow = valid_cfg.min_flow
valid_case = avg_flow > min_flow
if valid_case:
for i in range(max_ransac_iter): # repeat ransac for several times for stable result
# shuffle kp_cur and kp_ref (only useful when random seed is fixed)
new_list = np.random.randint(0, kp_cur.shape[0], (kp_cur.shape[0]))
new_kp_cur = kp_cur.copy()[new_list]
new_kp_ref = kp_ref.copy()[new_list]
start_time = time()
E, inliers = cv2.findEssentialMat(
new_kp_cur,
new_kp_ref,
focal=self.cam_intrinsics.fx,
pp=principal_points,
method=cv2.RANSAC,
prob=0.99,
threshold=self.cfg.compute_2d2d_pose.ransac.reproj_thre,
)
# check homography inlier ratio
if valid_cfg.method == "homo_ratio":
# Find homography
H, H_inliers = cv2.findHomography(
new_kp_cur,
new_kp_ref,
method=cv2.RANSAC,
confidence=0.99,
ransacReprojThreshold=0.2,
)
H_inliers_ratio = H_inliers.sum()/(H_inliers.sum()+inliers.sum())
valid_case = H_inliers_ratio < 0.25
if valid_case:
cheirality_cnt, R, t, _ = cv2.recoverPose(E, new_kp_cur, new_kp_ref,
focal=self.cam_intrinsics.fx,
pp=principal_points,)
self.timers.timers["Ess. Mat."].append(time()-start_time)
# check best inlier cnt
if valid_cfg.method == "flow+chei":
inlier_check = inliers.sum() > best_inlier_cnt and cheirality_cnt > 50
elif valid_cfg.method == "homo_ratio":
inlier_check = inliers.sum() > best_inlier_cnt
else:
assert False, "wrong cfg for compute_2d2d_pose.validity.method"
if inlier_check:
best_Rt = [R, t]
best_inlier_cnt = inliers.sum()
best_inliers = inliers
if len(best_Rt) == 0:
R = np.eye(3)
t = np.zeros((3, 1))
best_Rt = [R, t]
else:
R = np.eye(3)
t = np.zeros((3,1))
best_Rt = [R, t]
R, t = best_Rt
pose = SE3()
pose.R = R
pose.t = t
return pose, best_inliers
