def check_motion(self, cam_states):
"""
Check the input camera poses to ensure there is enough translation
to triangulate the feature
Arguments:
cam_states: input camera poses. (dict of <CAMStateID, CAMState>)
Returns:
True if the translation between the input camera poses
is sufficient. (bool)
"""
if self.optimization_config.translation_threshold < 0:
return True
observation_ids = list(self.observations.keys())
first_id = observation_ids[0]
last_id = observation_ids[-1]
first_cam_pose = Isometry3d(
to_rotation(cam_states[first_id].orientation).T,
cam_states[first_id].position)
last_cam_pose = Isometry3d(
to_rotation(cam_states[last_id].orientation).T,
cam_states[last_id].position)
# Get the direction of the feature when it is first observed.
# This direction is represented in the world frame.
feature_direction = np.array([*self.observations[first_id][:2], 1.0])
feature_direction = feature_direction / np.linalg.norm(
feature_direction)
feature_direction = first_cam_pose.R @ feature_direction
# Compute the translation between the first frame and the last frame.
# We assume the first frame and the last frame will provide the
# largest motion to speed up the checking process.
translation = last_cam_pose.t - first_cam_pose.t
parallel = translation @ feature_direction
orthogonal_translation = translation - parallel * feature_direction
return (np.linalg.norm(orthogonal_translation) >
self.optimization_config.translation_threshold)
def process_gt(self):
while True:
x = self.gt_queue.get()
if x is None:
return
data = x
if not self.first_gt_flag:
p0 = data.p
self.first_gt_flag = True
R = utils.to_rotation(data.q)
gt_pose = utils.Isometry3d(R, data.p - p0)
if self.viewer is not None:
self.viewer.update_gt(gt_pose)
def initialize_position(self, cam_states):
"""
Intialize the feature position based on all current available
measurements.
The computed 3d position is used to set the position member variable.
Note the resulted position is in world frame.
Arguments:
cam_states: A dict containing the camera poses with its ID as the
associated key value. (dict of <CAMStateID, CAMState>)
Returns:
True if the estimated 3d position of the feature is valid. (bool)
"""
cam_poses = [] # [Isometry3d]
measurements = [] # [vec2]
T_cam1_cam0 = Isometry3d(Feature.R_cam0_cam1,
Feature.t_cam0_cam1).inverse()
for cam_id, m in self.observations.items():
try:
cam_state = cam_states[cam_id]
except KeyError:
continue
# Add measurements.
measurements.append(m[:2])
measurements.append(m[2:])
# This camera pose will take a vector from this camera frame
# to the world frame.
cam0_pose = Isometry3d(
to_rotation(cam_state.orientation).T, cam_state.position)
cam1_pose = cam0_pose * T_cam1_cam0
cam_poses.append(cam0_pose)
cam_poses.append(cam1_pose)
# All camera poses should be modified such that it takes a vector
# from the first camera frame in the buffer to this camera frame.
T_c0_w = cam_poses[0]
cam_poses_tmp = []
for pose in cam_poses:
cam_poses_tmp.append(pose.inverse() * T_c0_w)
cam_poses = cam_poses_tmp
# Generate initial guess
initial_position = self.generate_initial_guess(cam_poses[-2],
measurements[0],
measurements[-2])
solution = np.array([*initial_position[:2], 1.0]) / initial_position[2]
# Apply Levenberg-Marquart method to solve for the 3d position.
lambd = self.optimization_config.initial_damping
inner_loop_count = 0
outer_loop_count = 0
is_cost_reduced = False
delta_norm = float('inf')
# Compute the initial cost.
total_cost = 0.0
# for i, cam_pose in enumerate(cam_poses):
for cam_pose, measurement in zip(cam_poses, measurements):
total_cost += self.cost(cam_pose, solution, measurement)
# Outer loop.
while (outer_loop_count <
self.optimization_config.outer_loop_max_iteration
and delta_norm > self.optimization_config.estimation_precision):
A = np.zeros((3, 3))
b = np.zeros(3)
for cam_pose, measurement in zip(cam_poses, measurements):
J, r, w = self.jacobian(cam_pose, solution, measurement)
if w == 1.0:
A += J.T @ J
b += J.T @ r
else:
A += w * w * J.T @ J
b += w * w * J.T @ r
# Inner loop.
# Solve for the delta that can reduce the total cost.
while (inner_loop_count <
self.optimization_config.inner_loop_max_iteration
and not is_cost_reduced):
delta = np.linalg.solve(A + lambd * np.identity(3), b) # vec3
new_solution = solution - delta
delta_norm = np.linalg.norm(delta)
new_cost = 0.0
for cam_pose, measurement in zip(cam_poses, measurements):
new_cost += self.cost(cam_pose, new_solution, measurement)
if new_cost < total_cost:
is_cost_reduced = True
solution = new_solution
total_cost = new_cost
lambd = max(lambd / 10., 1e-10)
else:
is_cost_reduced = False
lambd = min(lambd * 10., 1e12)
inner_loop_count += 1
inner_loop_count = 0
outer_loop_count += 1
# Covert the feature position from inverse depth
# representation to its 3d coordinate.
final_position = np.array([*solution[:2], 1.0]) / solution[2]
# Check if the solution is valid. Make sure the feature
# is in front of every camera frame observing it.
is_valid_solution = True
for pose in cam_poses:
position = pose.R @ final_position + pose.t
if position[2] <= 0:
is_valid_solution = False
break
# Convert the feature position to the world frame.
self.position = T_c0_w.R @ final_position + T_c0_w.t
self.is_initialized = is_valid_solution
return is_valid_solution