def update_poses_and_points(self,
keyframes,
correction=None,
exclude=set()):
for kf in keyframes:
if len(exclude) > 0 and kf in exclude:
continue
uncorrected = g2o.Isometry3d(kf.orientation, kf.position)
if correction is None:
vertex = self.vertex(kf.id)
if vertex.fixed():
continue
corrected = vertex.estimate()
else:
corrected = uncorrected * correction
delta = uncorrected.inverse() * corrected
if (g2o.AngleAxis(delta.rotation()).angle() < 0.02
and np.linalg.norm(delta.translation()) < 0.03): # 1°, 3cm
continue
for m in kf.measurements():
if m.from_triangulation():
old = m.mappoint.position
new = corrected * (uncorrected.inverse() * old)
m.mappoint.update_position(new)
# update normal ?
kf.update_pose(corrected)
def predict_pose(self,
timestamp,
prev_position=None,
prev_orientation=None):
'''
Predict the next camera pose.
'''
if prev_position is not None:
self.position = prev_position
if prev_orientation is not None:
self.orientation = prev_orientation
if not self.initialized:
return (g2o.Isometry3d(self.orientation,
self.position), self.covariance)
dt = timestamp - self.timestamp
delta_angle = g2o.AngleAxis(self.v_angular_angle * dt * self.damp,
self.v_angular_axis)
delta_orientation = g2o.Quaternion(delta_angle)
orientation = delta_orientation * self.orientation
position = self.position + delta_orientation * (self.v_linear * dt *
self.damp)
return (g2o.Isometry3d(orientation, position), self.covariance)
def update_pose(self,
timestamp,
new_position,
new_orientation,
new_covariance=None):
'''
Update the motion model when given a new camera pose.
'''
if self.initialized:
dt = timestamp - self.timestamp
assert dt != 0
v_linear = (new_position - self.position) / dt
self.v_linear = v_linear
delta_q = self.orientation.inverse() * new_orientation
delta_q.normalize()
delta_angle = g2o.AngleAxis(delta_q)
angle = delta_angle.angle()
axis = delta_angle.axis()
if angle > np.pi:
axis = axis * -1
angle = 2 * np.pi - angle
self.v_angular_axis = axis
self.v_angular_angle = angle / dt
self.timestamp = timestamp
self.position = new_position
self.orientation = new_orientation
self.covariance = new_covariance
self.initialized = True
def predict_pose(self, timestamp):
'''
Predict the next camera pose.
'''
if not self.initialized:
return (g2o.Isometry3d(self.orientation,
self.position), self.covariance)
dt = timestamp - self.timestamp
# Use quaternion instead of rotation matrix due to performance
delta_angle = g2o.AngleAxis(self.v_angular_angle * dt * self.damp,
self.v_angular_axis)
delta_orientation = g2o.Quaternion(delta_angle)
position = self.position + self.v_linear * dt * self.damp
orientation = self.orientation * delta_orientation
return (g2o.Isometry3d(orientation, position), self.covariance)
def update_pose(self, timestamp,
new_position, new_orientation, new_covariance=None):
'''
Update the motion model when given a new camera pose.
'''
transformation_matrix = np.identity(4, dtype=np.float32)
transformation_matrix[0:3, 0:3] = np.array(new_orientation.rotation_matrix(), dtype=np.float32)
transformation_matrix[0:3, 3] = new_position
self.f.write(" ".join(['{:.6e}'.format(x) for x in transformation_matrix.flatten()[0:12]]) + "\n")
if self.initialized:
dt = timestamp - self.timestamp
assert dt != 0
v_linear = (new_position - self.position) / dt
self.v_linear = v_linear
delta_q = self.orientation.inverse() * new_orientation
delta_q.normalize()
delta_angle = g2o.AngleAxis(delta_q)
angle = delta_angle.angle()
axis = delta_angle.axis()
if angle > np.pi:
axis = axis * -1
angle = 2 * np.pi - angle
self.v_angular_axis = axis
self.v_angular_angle = angle / dt
self.timestamp = timestamp
self.position = new_position
self.orientation = new_orientation
self.covariance = new_covariance
self.initialized = True
def match_and_estimate(query_keyframe, match_keyframe, params):
query = defaultdict(list)
for m in query_keyframe.measurements():
if m.from_triangulation():
query['measurements'].append(m)
query['kps1'].append(m.get_keypoint(0))
query['kps2'].append(m.get_keypoint(1))
query['desps1'].append(m.get_descriptor(0))
query['desps2'].append(m.get_descriptor(1))
n = len(query['matches'])
query['matches'].append(cv2.DMatch(n, n, 0))
match = defaultdict(list)
for m in match_keyframe.measurements():
if m.from_triangulation():
match['measurements'].append(m)
match['kps1'].append(m.get_keypoint(0))
match['kps2'].append(m.get_keypoint(1))
match['desps1'].append(m.get_descriptor(0))
match['desps2'].append(m.get_descriptor(1))
n = len(match['matches'])
match['matches'].append(cv2.DMatch(n, n, 0))
stereo_matches = query_keyframe.feature.circular_stereo_match(
query['desps1'], query['desps2'], query['matches'], match['desps1'],
match['desps2'], match['matches'], params.matching_distance,
params.lc_inliers_threshold)
n_matches = len(stereo_matches)
if n_matches < params.lc_inliers_threshold:
return None
for m13, _ in stereo_matches:
i, j = m13.queryIdx, m13.trainIdx
query['px'].append(query['kps1'][i].pt)
query['pt'].append(query['measurements'][i].view)
match['px'].append(match['kps1'][j].pt)
match['pt'].append(match['measurements'][j].view)
# query_keyframe's pose in match_keyframe's coordinates frame
T13, inliers13 = solve_pnp_ransac(query['pt'], match['px'],
match_keyframe.cam.intrinsic)
T31, inliers31 = solve_pnp_ransac(match['pt'], query['px'],
query_keyframe.cam.intrinsic)
if T13 is None or T13 is None:
return None
delta = T31 * T13
if (g2o.AngleAxis(delta.rotation()).angle() > 0.1
or np.linalg.norm(delta.translation()) > 0.5): # 5.7° or 0.5m
return None
n_inliers = len(set(inliers13) & set(inliers31))
query_pose = g2o.Isometry3d(query_keyframe.orientation,
query_keyframe.position)
match_pose = g2o.Isometry3d(match_keyframe.orientation,
match_keyframe.position)
# TODO: combine T13 and T31
constraint = T13
estimated_pose = match_pose * constraint
correction = query_pose.inverse() * estimated_pose
return namedtuple('MatchEstimateResult', [
'estimated_pose', 'constraint', 'correction',
'query_stereo_measurements', 'match_stereo_measurements',
'stereo_matches', 'n_matches', 'n_inliers'
])(estimated_pose, constraint, correction, query['measurements'],
match['measurements'], stereo_matches, n_matches, n_inliers)
def get_rotation_angle_axis(self):
angle_axis = g2o.AngleAxis(self._pose.orientation())
#angle = angle_axis.angle()
#axis = angle_axis.axis()
return angle_axis
def match_and_estimate(query_keyframe, match_keyframe, params):
query = defaultdict(list)
for kp, desp in zip(query_keyframe.feature.keypoints,
query_keyframe.feature.descriptors):
query['kps'].append(kp)
query['desps'].append(desp)
query['px'].append(kp.pt)
match = defaultdict(list)
for kp, desp in zip(match_keyframe.feature.keypoints,
match_keyframe.feature.descriptors):
match['kps'].append(kp)
match['desps'].append(desp)
match['px'].append(kp.pt)
matches = query_keyframe.feature.direct_match(
query['desps'], match['desps'], params.matching_distance,
params.matching_distance_ratio)
query_pts = depth_to_3d(query_keyframe.depth, query['px'],
query_keyframe.cam)
match_pts = depth_to_3d(match_keyframe.depth, match['px'],
match_keyframe.cam)
if len(matches) < params.lc_inliers_threshold:
return None
near = query_keyframe.cam.depth_near
far = query_keyframe.cam.depth_far
for (i, j) in matches:
if (near <= query_pts[i][2] <= far):
query['pt12'].append(query_pts[i])
query['px12'].append(query['kps'][i].pt)
match['px12'].append(match['kps'][j].pt)
if (near <= match_pts[j][2] <= far):
query['px21'].append(query['kps'][i].pt)
match['px21'].append(match['kps'][j].pt)
match['pt21'].append(match_pts[j])
if len(query['pt12']) < 6 or len(match['pt21']) < 6:
return None
T12, inliers12 = solve_pnp_ransac(query['pt12'], match['px12'],
match_keyframe.cam.intrinsic)
T21, inliers21 = solve_pnp_ransac(match['pt21'], query['px21'],
query_keyframe.cam.intrinsic)
if T12 is None or T21 is None:
return None
delta = T21 * T12
if (g2o.AngleAxis(delta.rotation()).angle() > 0.06
or np.linalg.norm(delta.translation()) > 0.06): # 3° or 0.06m
return None
ms = set()
qd = dict()
md = dict()
for i in inliers12:
pt1 = (int(query['px12'][i][0]), int(query['px12'][i][1]))
pt2 = (int(match['px12'][i][0]), int(match['px12'][i][1]))
ms.add((pt1, pt2))
for i in inliers21:
pt1 = (int(query['px21'][i][0]), int(query['px21'][i][1]))
pt2 = (int(match['px21'][i][0]), int(match['px21'][i][1]))
ms.add((pt1, pt2))
for i, (pt1, pt2) in enumerate(ms):
qd[pt1] = i
md[pt2] = i
qd2 = dict()
md2 = dict()
for m in query_keyframe.measurements():
pt = m.get_keypoint(0).pt
idx = qd.get((int(pt[0]), int(pt[1])), None)
if idx is not None:
qd2[idx] = m
for m in match_keyframe.measurements():
pt = m.get_keypoint(0).pt
idx = md.get((int(pt[0]), int(pt[1])), None)
if idx is not None:
md2[idx] = m
shared_measurements = [(qd2[i], md2[i]) for i in (qd2.keys() & md2.keys())]
n_matches = (len(query['pt12']) + len(match['pt21'])) / 2.
n_inliers = max(len(inliers12), len(inliers21))
query_pose = g2o.Isometry3d(query_keyframe.orientation,
query_keyframe.position)
match_pose = g2o.Isometry3d(match_keyframe.orientation,
match_keyframe.position)
# TODO: combine T12 and T21
constraint = T12
estimated_pose = match_pose * constraint
correction = query_pose.inverse() * estimated_pose
return namedtuple('MatchEstimateResult', [
'estimated_pose', 'constraint', 'correction', 'shared_measurements',
'n_matches', 'n_inliers'
])(estimated_pose, constraint, correction, shared_measurements, n_matches,
n_inliers)
import sys
import numpy as np
import math
sys.path.append("../../")
from config import Config
import g2o
position = np.zeros(3) # delta translation
orientation = g2o.Quaternion()
pose = g2o.Isometry3d(orientation, position)
print('pose: ', pose.matrix())
position[0] = 1
print('pose: ', pose.matrix())
orientation2 = g2o.Quaternion(g2o.AngleAxis(math.pi, [0, 0, 1]))
print('position', position)
print('orientation2*position', orientation2 * position)
