def match_and_estimate(query_keyframe, match_keyframe, params):
print('TODO: Implement how to compute relative camera pose')
T13 = g2o.Isometry3d()
T31 = g2o.Isometry3d()
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
query_pose = query_keyframe.pose
match_pose = match_keyframe.pose
# 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'])(
estimated_pose, constraint, correction)
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 set(self, pose):
if isinstance(pose, g2o.SE3Quat) or isinstance(pose, g2o.Isometry3d):
self._pose = g2o.Isometry3d(pose.orientation(), pose.position())
else:
self._pose = g2o.Isometry3d(pose) # g2o.Isometry3d
#self.position = pose.position() # np.zeros(3)
#self.orientation = pose.orientation() # g2o.Quaternion(), quat_cw
self.Tcw = self._pose.matrix(
) # homogeneous transformation matrix: (4, 4) pc_ = Tcw * pw_
self.Rcw = self.Tcw[:3, :3]
self.tcw = self.Tcw[:3, 3] # pc = Rcw * pw + tcw
self.Rwc = self.Rcw.T
self.Ow = -(self.Rwc @ self.tcw) # origin of camera frame w.r.t world
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)
orientation = self.delta_orientation * self.orientation
position = self.position + self.delta_orientation * self.delta_position
return (g2o.Isometry3d(orientation, position), self.covariance)
def add_edge(self,
vertices,
measurement=None,
information=np.eye(6),
robust_kernel=None):
edge = g2o.EdgeSE3()
for i, vertex in enumerate(vertices):
# check to see if we're passing in actual vertices or just the vertex ids
if isinstance(vertex, int):
vertex = self.optimizer.vertex(vertex)
edge.set_vertex(i, vertex)
edge.set_measurement(g2o.Isometry3d(
measurement)) # relative pose transformation between frames
# information matrix/precision matrix
# represents uncertainty of measurement error
# inverse of covariance matrix
edge.set_information(information)
if robust_kernel is not None:
edge.set_robust_kernel(robust_kernel)
self.optimizer.add_edge(edge)
def handle_duckiebot_message(self, node_id0, node_id1, transform, time_stamp):
"""Processes a message containing the pose of an object seen by a
Duckiebot and adds an edge to the graph. Note: we assume that a
Duckiebot cannot see the April tag of another Duckiebot, so no
adjustment based on the object seen is needed.
Args:
node_id0: ID of the object (Duckiebot) that sees the April tag of the
other object.
node_id1: ID of the object whose April tag is seen by the Duckiebot.
transform: Transform contained in the ROS message.
time_stamp: Timestamp associated to the ROS message.
"""
# Get type of the object that sees the other object, for a sanity check.
type_of_object_seeing = node_id0.split("_")[0]
if (type_of_object_seeing == "duckiebot"):
# The pose needs to be adjusted to take into account the relative
# pose of the camera on the Duckiebot w.r.t. to the base frame of
# the Duckiebot.
t = [0.075, 0.0, 0.025]
# This angle is an estimate of the angle by which the plastic
# support that holds the camera is tilted.
x_angle = -102
z_angle = -90
x_angle = np.deg2rad(x_angle)
z_angle = np.deg2rad(z_angle)
R_x = g.rotation_from_axis_angle(np.array([1, 0, 0]), x_angle)
R_z = g.rotation_from_axis_angle(np.array([0, 0, 1]), z_angle)
R = np.matmul(R_z, R_x) # verified
H_base_to_camera = g2o.Isometry3d(R, t)
transform = H_base_to_camera * transform
else:
print("This should not be here! %s " % node_id0)
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 handle_watchtower_message(self, node_id0, node_id1, data, time_stamp, pose_error=None):
"""Processes a message containing the pose of an object seen by a
watchtower and adds an edge to the graph. If the object seen is a
Duckiebot, adjusts the pose accordingly.
Args:
node_id0: ID of the object (watchtower) that sees the April tag of the
other object.
node_id1: ID of the object whose April tag is seen by the watchtower.
transform: Transform contained in the ROS message.
time_stamp: Timestamp associated to the ROS message.
"""
transform = get_transform_from_data(data)
# Get type of the object seen.
type_of_object_seen = get_node_type(node_id1)
space_dev = self.default_variance["watchtower"]["position_deviation"]
angle_dev = self.default_variance["watchtower"]["angle_deviation"]
if(pose_error != None):
measure_information = create_info_matrix(
space_dev * pose_error, angle_dev * pose_error)
else:
measure_information = create_info_matrix(space_dev, angle_dev)
if (type_of_object_seen == "duckiebot"):
# print("watzchtower %s is seing duckiebot %s" %
# (node_id0, node_id1))
# In case of Duckiebot the pose needs to be adjusted to take into
# account the pose of the April tag w.r.t. the base frame of the
# Duckiebot.
t = [0.0, 0.0, 0.055]
z_angle = 90
x_angle = 178
z_angle = np.deg2rad(z_angle)
x_angle = np.deg2rad(x_angle)
R_z = g.rotation_from_axis_angle(np.array([0, 0, 1]), z_angle)
R_x = g.rotation_from_axis_angle(np.array([1, 0, 0]), x_angle)
R = np.matmul(R_x, R_z) # verified!
H_apriltag_to_base = g2o.Isometry3d(R, t)
transform = transform * H_apriltag_to_base.inverse()
# Add edge to the graph.
return self.resampler.handle_watchtower_edge(node_id0, node_id1, transform, time_stamp, measure_information=measure_information, is_duckiebot=True)
else:
# Add edge to the graph.
if node_id0 not in self.edge_counters:
self.edge_counters[node_id0] = dict()
if node_id1 not in self.edge_counters[node_id0]:
self.edge_counters[node_id0][node_id1] = 0
if(self.edge_counters[node_id0][node_id1] < self.max_number_same_edge):
self.resampler.handle_watchtower_edge(
node_id0, node_id1, transform, time_stamp)
self.edge_counters[node_id0][node_id1] += 1
else:
a = random.randint(0, self.rejection_sampling_int)
if(a == 0):
self.edge_counters[node_id0][node_id1] += 1
return self.resampler.handle_watchtower_edge(node_id0, node_id1, transform, time_stamp, measure_information=measure_information)
def set_data(self, keyframes, loops):
super().clear()
anchor = None
for kf, *_ in loops:
if anchor is None or kf < anchor:
anchor = kf
for i, kf in enumerate(keyframes):
pose = g2o.Isometry3d(kf.orientation, kf.position)
fixed = i == 0
if anchor is not None:
fixed = kf <= anchor
self.add_vertex(kf.id, pose, fixed=fixed)
if kf.preceding_keyframe is not None:
self.add_edge(vertices=(kf.preceding_keyframe.id, kf.id),
measurement=kf.preceding_constraint)
if (kf.reference_keyframe is not None
and kf.reference_keyframe != kf.preceding_keyframe):
self.add_edge(vertices=(kf.reference_keyframe.id, kf.id),
measurement=kf.reference_constraint)
for kf, kf2, meas in loops:
self.add_edge((kf.id, kf2.id), measurement=meas)
def apply_correction(self, correction): # corr: g2o.Isometry3d or matrix44
'''
Reset the model given a new camera pose.
Note: This method will be called when it happens an abrupt change in the pose (LoopClosing)
'''
if not isinstance(correction, g2o.Isometry3d):
correction = g2o.Isometry3d(correction)
current = g2o.Isometry3d(self.orientation, self.position)
current = correction * current
self.position = current.position()
self.orientation = current.orientation()
# correction= Tcw_corrected * Tcw_uncorrected.inverse() (transform from camera_uncorrected to camera_corrected)
self.v_angular_axis = correction.orientation() * self.v_angular_axis
self.v_linear = correction.orientation() * self.v_linear
def apply_correction(self, correction): # corr: g2o.Isometry3d or matrix44
'''
Reset the model given a new camera pose.
Note: This method will be called when it happens an abrupt change in the pose (LoopClosing)
'''
if not isinstance(correction, g2o.Isometry3d):
correction = g2o.Isometry3d(correction)
current = g2o.Isometry3d(self.orientation, self.position)
current = current * correction
self.position = current.position()
self.orientation = current.orientation()
self.v_linear = (correction.inverse().orientation() * self.v_linear)
self.v_angular_axis = (correction.inverse().orientation() *
self.v_angular_axis)
def track(self, frame):
while self.is_paused():
time.sleep(1e-4)
self.set_tracking(True)
self.current = frame
# print('Tracking:', frame.idx, ' <- ', self.reference.id, self.reference.idx)
predicted_pose, _ = self.motion_model.predict_pose(frame.timestamp)
frame.update_pose(predicted_pose)
if self.loop_closing is not None:
if self.loop_correction is not None:
estimated_pose = g2o.Isometry3d(
frame.orientation,
frame.position)
estimated_pose = estimated_pose * self.loop_correction
frame.update_pose(estimated_pose)
self.motion_model.apply_correction(self.loop_correction)
self.loop_correction = None
local_mappoints = self.filter_points(frame)
measurements = frame.match_mappoints(
local_mappoints, Measurement.Source.TRACKING)
# print('measurements:', len(measurements), ' ', len(local_mappoints))
tracked_map = set()
for m in measurements:
mappoint = m.mappoint
mappoint.update_descriptor(m.get_descriptor())
mappoint.increase_measurement_count()
tracked_map.add(mappoint)
try:
self.reference = self.graph.get_reference_frame(tracked_map)
pose = self.tracker.refine_pose(frame.pose, frame.cam, measurements)
frame.update_pose(pose)
self.motion_model.update_pose(
frame.timestamp, pose.position(), pose.orientation())
tracking_is_ok = True
except:
tracking_is_ok = False
print('tracking failed!!!')
if tracking_is_ok and self.should_be_keyframe(frame, measurements):
# print('new keyframe', frame.idx)
keyframe = frame.to_keyframe()
keyframe.update_reference(self.reference)
keyframe.update_preceding(self.preceding)
self.mapping.add_keyframe(keyframe, measurements)
if self.loop_closing is not None:
self.loop_closing.add_keyframe(keyframe)
self.preceding = keyframe
self.set_tracking(False)
def make_graph(self, data):
"""
This is the method we added. It takes data in the dictionary format we recorded, and
turns it into g2o edges and nodes.
"""
lidar = data['scans'] # list of laser scan data
odom = data['odom'] # list of odom data
closure = data['closures'] # index of loop closure scan in laser scans
i = -1
# Loop through lidar data and add each measured point as a vertex
for k,scan in enumerate(lidar): # loop through each scan
for point in scan: # loop through each point in each scan
i = i+1
# t is the pose estimate of the scanned point
t = g2o.Isometry3d(np.identity(3),[point[0], point[1], 0])
self.add_vertex(i, t)
vertices = super().vertices()
start_odom_vertices = len(vertices)
print(len(vertices))
# Loop through odom points and add them as vertices. Then, add edges between the odom point and each point in the most
# recent laser scan.
for f, point in enumerate(odom):
# End the loop if there are more odom points than LiDAR, since we are only keeping even values
if f > len(lidar): # just in case, stop when you have an odom point for each laser scan. otherwise the indexes won't match
break
i = i+1
# Add odom vertices
t = g2o.Isometry3d(np.identity(3),[point[0], point[1], 0])
self.add_vertex(i, t)
# Add edges between current odom point and all corresponding lidar points
start_index = int((f) * 361)
for x in range(361):
lidar_pt = lidar[f][x]
diff = g2o.Isometry3d(np.identity(3), [(point[0]-lidar_pt[0]), (point[1]-lidar_pt[1]), 0])
self.add_edge([i, x], diff)
# Add edge from loop closures
diff = g2o.Isometry3d(np.identity(3)*5, [0,0,0])
self.add_edge([start_odom_vertices, len(super().vertices())-1], diff)
print('num vertices:', len(super().vertices()))
print('num edges: ', len(super().edges()))
def interpolate_measure(measure, alpha):
R = measure.R
t = measure.t
q = g.SE3_from_rotation_translation(R, t)
vel = g.SE3.algebra_from_group(q)
rel = g.SE3.group_from_algebra(vel * alpha)
newR, newt = g.rotation_translation_from_SE3(rel)
return g2o.Isometry3d(newR, newt)
def pose_to_isometry(pose):
"""Convert a pose vector to a :class: g2o.Isometry3d instance.
Args:
pose: A 7 element 1-d numpy array encoding x, y, z, qx, qy, qz, and qw respectively.
Returns:
A :class: g2o.Isometry3d instance encoding the same information as the input pose.
"""
return g2o.Isometry3d(g2o.Quaternion(*np.roll(pose[3:7], 1)), pose[:3])
def update_pose(self, pose):
if isinstance(pose, g2o.SE3Quat):
self.pose = g2o.Isometry3d(pose.orientation(), pose.position())
else:
self.pose = pose
self.transform_matrix = self.pose.inverse().matrix()[:3]
self.projection_matrix = (self.cam.intrinsic.dot(
self.transform_matrix))
def add_vertex(self,
node_id,
theta,
position,
is_initial_floor_tag=False,
fixed=False,
time_stamp=0.0):
"""Adds a node with an ID given in the format outputted from the
transform_listener and with a given pose to the g2o graph.
TODO : add a more general add_vertex function that takes a 3D pose and not only a 2D one.
Args:
node_id: ID of the node whose pose should be added as a node
in the graph, in the format <node_type>_<node_id>.
theta: Angle of the 2D rotation around the z axis defines the
pose of the node.
position: 2D translation vector (x, y) that define the pose of the node.
is_initial_floor_tag: True if the node is an initial floor tag,
False otherwise.
fixed: True if the node should be added as a fixed node (i.e.,
not optimizable) in the graph, False otherwise.
time_stamp: Timestamp.
"""
# Adds (assigns) a timestamp to a node, by giving it a 'local index'
# w.r.t. the node.
node = self.get_node(node_id)
added = node.add_timestamp(time_stamp)
if (not added):
print(
"add_vertex did not add : node %s at time %f as already there"
% (node_id, time_stamp))
else:
# Translation vector, z coordinate does not vary.
position = [position[0], position[1], 0.0]
# Rotation around the z axis of and angle theta.
R = g.rotation_from_axis_angle(np.array([0, 0, 1]),
np.deg2rad(theta))
if (is_initial_floor_tag):
# For initial floor tags, apply a further rotation of 180
# degrees around the x axis, so that the orientation of the z
# axis is reverted.
R2 = g.rotation_from_axis_angle(np.array([1, 0, 0]),
np.deg2rad(180))
R = np.matmul(R, R2)
# Add vertex with pose and ID in the right format to the g2o graph.
vertex_pose = g2o.Isometry3d(R, position)
vertex_index = node.get_g2o_index(time_stamp)
self.graph.add_vertex(vertex_index, vertex_pose, fixed=fixed)
if (is_initial_floor_tag):
# For initial floor tags, add an edge between the reference
# vertex and the newly-added vertex for the pose of the tag.
self.graph.add_edge(vertex0_id=0,
vertex1_id=vertex_index,
measure=vertex_pose)
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 optimize(self, max_iterations=20):
optimizer = G2OPoseGraphOptimizer()
id_to_name = {}
name_to_id = {}
# populate optimizer
with self._lock:
# add vertices
for nname, ndata in self.nodes.items():
# compute node ID
node_id = len(id_to_name)
id_to_name[node_id] = nname
name_to_id[nname] = node_id
# get node pose and other attributes
pose = g2o.Isometry3d(g2o.Quaternion(ndata["pose"].Q('wxyz')), ndata["pose"].t) \
if "pose" in ndata else None
fixed = ndata["fixed"] if "fixed" in ndata else False
# add vertex
optimizer.add_vertex(node_id, pose=pose, fixed=fixed)
# add edges
for u, v, edata in self.edges.data():
# get nodes IDs
iu = name_to_id[u]
iv = name_to_id[v]
# get edge measurement and other attributes
measurement = edata["measurement"]
T = tr.compose_matrix(
translate=measurement.t,
angles=tr.euler_from_quaternion(measurement.q)
)
# add edge
optimizer.add_edge([iu, iv], g2o.Isometry3d(T))
# optimize
optimizer.optimize(max_iterations=max_iterations)
# update graph
with self._lock:
for nid, nname in id_to_name.items():
npose = optimizer.get_pose(nid)
T = tr.compose_matrix(
translate=npose.t,
angles=tr.euler_from_matrix(npose.R)
)
q = tr.quaternion_from_matrix(T)
self.nodes[nname]["pose"] = TF(t=npose.t, q=q)
def solve_pnp_ransac(pts3d, pts, intrinsic_matrix):
val, rvec, tvec, inliers = cv2.solvePnPRansac(
np.array(pts3d), np.array(pts),
intrinsic_matrix, None, None, None,
False, 50, 2.0, 0.9, None)
if inliers is None or len(inliers) < 5:
return None, None
T = g2o.Isometry3d(cv2.Rodrigues(rvec)[0], tvec)
return T, inliers.ravel()
def add_vertex(self, id, pose, is_fixed=False):
# Rt (pose) matrix, absolute
v = g2o.VertexSE3()
v.set_id(id)
v.set_estimate(g2o.Isometry3d(pose))
v.set_fixed(is_fixed)
self.optimizer.add_vertex(v)
self.nodes.append(v)
def transform_callback(self, data):
""" ROS callback.
"""
self.num_messages_received += 1
# Get frame IDs of the objects to which the ROS messages are referred.
id0 = data.header.frame_id
id1 = data.child_frame_id
# Convert the frame IDs to the right format.
id0 = self.filter_name(id0)
id1 = self.filter_name(id1)
# Ignore messages from one watchtower to another watchtower (i.e.,
# odometry messages between watchtowers). TODO: check if we can avoid
# sending these messages.
is_from_watchtower = False
if (id0.startswith("watchtower")):
is_from_watchtower = True
if (id1.startswith("watchtower")):
# print(data)
return 0
# Create translation vector.
t = [
data.transform.translation.x, data.transform.translation.y,
data.transform.translation.z
]
# Create rotation matrix. NOTE: in Pygeometry, quaternion is
# the (w, x, y, z) form.
q = [
data.transform.rotation.w, data.transform.rotation.x,
data.transform.rotation.y, data.transform.rotation.z
]
M = g.rotations.rotation_from_quaternion(np.array(q))
# Verify that the rotation is a proper rotation.
det = np.linalg.det(M)
if (det < 0):
print("det is %f" % det)
# Obtain complete transform and use it to add vertices in the graph.
transform = g2o.Isometry3d(M, t)
time = Time(data.header.stamp)
time_stamp = time.data.secs + time.data.nsecs * 10**(-9)
if (id1 == id0):
# Same ID: odometry message, e.g. the same Duckiebot sending
# odometry information at different instances in time.
self.handle_odometry_message(id1, transform, time_stamp)
elif (is_from_watchtower):
# Tag detected by a watchtower.
self.handle_watchtower_message(id0, id1, transform, time_stamp)
else:
# Tag detected by a Duckiebot.
self.handle_duckiebot_message(id0, id1, transform, time_stamp)
def handle_watchtower_message(self, node_id0, node_id1, transform,
time_stamp):
"""Processes a message containing the pose of an object seen by a
watchtower and adds an edge to the graph. If the object seen is a
Duckiebot, adjusts the pose accordingly.
Args:
node_id0: ID of the object (watchtower) that sees the April tag of the
other object.
node_id1: ID of the object whose April tag is seen by the watchtower.
transform: Transform contained in the ROS message.
time_stamp: Timestamp associated to the ROS message.
"""
# Get type of the object seen.
type_of_object_seen = node_id1.split("_")[0]
if (type_of_object_seen == "duckiebot"):
print("watzchtower %s is seing duckiebot %s" %
(node_id0, node_id1))
# In case of Duckiebot the pose needs to be adjusted to take into
# account the pose of the April tag w.r.t. the base frame of the
# Duckiebot.
t = [0.0, 0.0, 0.055]
z_angle = 90
x_angle = 178
z_angle = np.deg2rad(z_angle)
x_angle = np.deg2rad(x_angle)
R_z = g.rotation_from_axis_angle(np.array([0, 0, 1]), z_angle)
R_x = g.rotation_from_axis_angle(np.array([1, 0, 0]), x_angle)
R = np.matmul(R_x, R_z) # verified!
H_apriltag_to_base = g2o.Isometry3d(R, t)
transform = transform * H_apriltag_to_base.inverse()
# Add edge to the graph.
return self.pose_graph.add_edge(node_id0, node_id1, transform,
time_stamp)
else:
# Add edge to the graph.
if node_id0 not in self.edge_counters:
self.edge_counters[node_id0] = dict()
if node_id1 not in self.edge_counters[node_id0]:
self.edge_counters[node_id0][node_id1] = 0
if (self.edge_counters[node_id0][node_id1] <
self.max_number_same_edge):
self.pose_graph.add_edge(node_id0, node_id1, transform,
time_stamp)
self.edge_counters[node_id0][node_id1] += 1
else:
a = random.randint(0, self.rejection_sampling_int)
if (a == 0):
self.edge_counters[node_id0][node_id1] += 1
return self.pose_graph.add_edge(node_id0, node_id1,
transform, time_stamp)
def make_graph(self, data):
lidar = data['scans']
odom = data['odom']
i = -1
# Loop through lidar
for k, scan in enumerate(lidar):
for point in scan:
i = i + 1
# q = g2o.Quaternion(0,0,0,0)
t = g2o.Isometry3d(np.identity(3), [point[0], point[1], 0])
self.add_vertex(i, t)
vertices = super().vertices()
start_odom_vertices = len(vertices)
print(len(vertices))
# Loop through odom points
for f, point in enumerate(odom):
# End the loop if there are more odom points than LiDAR, since we are only keeping even values
if f > len(lidar):
break
i = i + 1
# Add odom vertices
t = g2o.Isometry3d(np.identity(3), [point[0], point[1], 0])
self.add_vertex(i, t)
# Add edges between current odom point and all corresponding lidar points
start_index = int((f) * 361)
for x in range(361):
lidar_pt = lidar[f][x]
diff = g2o.Isometry3d(np.identity(3),
[(point[0] - lidar_pt[0]),
(point[1] - lidar_pt[1]), 0])
self.add_edge([i, x], diff)
# Add edge from loop closures
closure = data['closures']
diff = g2o.Isometry3d(np.identity(3) * 5, [0, 0, 0])
self.add_edge(start_odom_vertices, len(super().vertices()) - 1, diff)
print('num vertices:', len(super().vertices()))
print('num edges: ', len(super().edges()))
def update_pose(self, pose):
if isinstance(pose, g2o.SE3Quat):
self.pose = g2o.Isometry3d(pose.orientation(), pose.position())
else:
self.pose = pose
self.orientation = self.pose.orientation()
self.position = self.pose.position()
self.pose_matrix = self.pose.matrix()
self.inv_pose_matrix = np.linalg.inv(self.pose_matrix)
self.update_points()
def __init__(self,
idx,
cam,
depth,
relative_pose_matrix,
preceding_keyframe=None,
image=None,
depth_image=None,
timestamp=None,
sparse_points_depths=None,
name=None):
self.idx = idx
self.pose_matrix = relative_pose_matrix
self.relative_pose_matrix = relative_pose_matrix
if preceding_keyframe:
self.pose_matrix = relative_pose_matrix.dot(
preceding_keyframe.pose_matrix)
# g2o representations
self.pose = g2o.Isometry3d(self.pose_matrix[:3, :3],
self.pose_matrix[:3, 3])
self.orientation = self.pose.orientation()
self.position = self.pose.position()
self.cam = cam
self.timestamp = timestamp
# transform matrix: shape (3,4)
self.inv_pose_matrix = np.linalg.inv(self.pose_matrix)
self.raw_depth = depth.copy()
self.depth = depth.copy() # depth map, predicted by CNN
self.image = image
self.depth_image = depth_image
self.grayimage = 0.299 * image[:, :,
0] + 0.587 * image[:, :,
1] + 0.114 * image[:, :,
2]
self.grayimage_f = RectBivariateSpline(self.cam.uy, self.cam.ux,
self.grayimage)
self.colors = None
if self.image is not None:
self.colors = self.image.copy().reshape(-1, 3) / 255
self.update_points()
self.raw_points_world = self.points_world.copy()
self.sparse_points_depths = sparse_points_depths
# if self.sparse_points_depths is not None:
# self.update_sparse_points()
self.name = name
def make_vertices_and_edges(self, data):
"""
Adds all vertices and edges to an optimizer object from data.
data: pickle file containing a dictionary of IndividualVertex objects. See collate_data.py for more details
"""
# loops through dict and makes all vertices
for key in data:
ind = data[key].index
# vertex pose
x = data[key].x
y = data[key].y
arr = np.array(
x, y, 0
) # using a 3D isometry to represent 2D data, so 3rd dimension is 0
t = g2o.Isometry3d(
np.identity(3),
arr) # needs an indentity matrix to make the 3D object happy
self.add_vertex(ind, t)
vertices = super().vertices()
# need to add edges after vertices, so we have to loop through the same data twice
for vertex in data.values():
ind = vertex.index # id of current vertex
for index, edge in vertex.edge.items(
): # index is id of vertex the current vertex has an edge to through visual odometry
# need to convert our 2D data to 3D again
arr = np.array(edge[0], edge[1], 0)
diff = g2o.Isometry3d(np.identity(3), arr)
self.add_edge([ind, index], diff)
print('num vertices:', len(super().vertices()))
print('num edges: ', len(super().edges()))
def create_measure(self):
t = self.position
roll_angle = np.deg2rad(self.rotation[0])
pitch_angle = np.deg2rad(self.rotation[1])
yaw_angle = np.deg2rad(self.rotation[2])
R_roll = g.rotation_from_axis_angle(np.array([0, 1, 0]), roll_angle)
R_pitch = g.rotation_from_axis_angle(np.array([1, 0, 0]), pitch_angle)
R_yaw = g.rotation_from_axis_angle(np.array([0, 0, 1]), yaw_angle)
R = np.matmul(R_roll, np.matmul(R_pitch, R_yaw))
self.measure = g2o.Isometry3d(R, t)
def handle_duckiebot_message(self,
node_id0,
node_id1,
data,
time_stamp,
pose_error=None):
"""Processes a message containing the pose of an object seen by a
Duckiebot and adds an edge to the graph. Note: we assume that a
Duckiebot cannot see the April tag of another Duckiebot, so no
adjustment based on the object seen is needed.
Args:
node_id0: ID of the object (Duckiebot) that sees the April tag of the
other object.
node_id1: ID of the object whose April tag is seen by the Duckiebot.
transform: Transform contained in the ROS message.
time_stamp: Timestamp associated to the ROS message.
"""
transform = get_transform_from_data(data)
space_dev = self.default_variance["duckiebot"]["position_deviation"]
angle_dev = self.default_variance["duckiebot"]["angle_deviation"]
if (pose_error != None):
measure_information = create_info_matrix(space_dev * pose_error,
angle_dev * pose_error)
else:
measure_information = create_info_matrix(space_dev, angle_dev)
# Get type of the object that sees the other object, for a sanity check.
type_of_object_seeing = node_id0.split("_")[0]
if (type_of_object_seeing == "duckiebot"):
# The pose needs to be adjusted to take into account the relative
# pose of the camera on the Duckiebot w.r.t. to the base frame of
# the Duckiebot.
t = [0.075, 0.0, 0.025]
# This angle is an estimate of the angle by which the plastic
# support that holds the camera is tilted.
x_angle = -102
z_angle = -90
x_angle = np.deg2rad(x_angle)
z_angle = np.deg2rad(z_angle)
R_x = g.rotation_from_axis_angle(np.array([1, 0, 0]), x_angle)
R_z = g.rotation_from_axis_angle(np.array([0, 0, 1]), z_angle)
R = np.matmul(R_z, R_x) # verified
H_base_to_camera = g2o.Isometry3d(R, t)
transform = H_base_to_camera * transform
# Add edge to the graph.
# return self.resampler.handle_duckiebot_edge(node_id0, node_id1, transform, time_stamp, measure_information)
else:
print("This should not be here! %s " % node_id0)
def update(self, i, left_img, right_img, timestamp):
# Feature extraction takes 0.12s
origin = g2o.Isometry3d()
left_frame = Frame(i, origin, self.cam, self.params, left_img,
timestamp)
right_frame = Frame(i, self.cam.compute_right_camera_pose(origin),
self.cam, self.params, right_img, timestamp)
frame = StereoFrame(left_frame, right_frame)
if i == 0:
self.initialize(frame)
return
# All code in this functions below takes 0.05s
self.current = frame
predicted_pose, _ = self.motion_model.predict_pose(frame.timestamp)
frame.update_pose(predicted_pose)
# Get mappoints and measurements take 0.013s
local_mappoints = self.get_local_map_points(frame)
print(local_mappoints)
if len(local_mappoints) == 0:
print('Nothing in local_mappoints! Exiting.')
exit()
measurements = frame.match_mappoints(local_mappoints)
# local_maplines = self.get_local_map_lines(frame)
# line_measurements = frame.match_maplines(local_maplines)
# Refined pose takes 0.02s
try:
pose = self.refine_pose(frame.pose, self.cam, measurements)
frame.update_pose(pose)
self.motion_model.update_pose(frame.timestamp, pose.position(),
pose.orientation())
tracking_is_ok = True
except:
tracking_is_ok = False
print('tracking failed!!!')
if tracking_is_ok and self.should_be_keyframe(frame, measurements):
# Keyframe creation takes 0.03s
self.create_new_keyframe(frame)
