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 add_edge(self, vertices, measurement, information):
edge = g2o.EdgeSE3()
for i, v in enumerate(vertices):
if isinstance(v, int):
v = self.vertex(v)
edge.set_vertex(i, v)
edge.set_measurement(measurement)
edge.set_information(information)
super(GraphOptimizer, self).add_edge(edge)
def add_edge(self, vertices, measurement,
information=np.identity(6),
robust_kernel=None):
edge = g2o.EdgeSE3()
for i, v in enumerate(vertices):
if isinstance(v, int):
v = self.vertex(v)
edge.set_vertex(i, v)
edge.set_measurement(measurement) # relative pose
edge.set_information(information)
if robust_kernel is not None:
edge.set_robust_kernel(robust_kernel)
super().add_edge(edge)
def add_edge(self, v0,v1, measurement=None,
information=np.identity(6),
robust_kernel = None): #95% CI
edge = g2o.EdgeSE3()
# edge = g2o.EdgeProjectXYZ2UV()
edge.set_vertex(0, v0)
edge.set_vertex(1, v1)
if measurement is None:
measurement = g2o.Isometry3d(g2o.Quaternion(0,0,0,1),np.array([0,1,0]))
edge.set_measurement(measurement) # relative pose
edge.set_information(information)
# edge.set_parameter_id(0, 0)
robust_kernel=g2o.RobustKernelHuber(np.sqrt(5.991))#5.991
if robust_kernel is not None:
edge.set_robust_kernel(robust_kernel)
super().add_edge(edge)
def add_edge(self,
vertices,
measurement=None,
information=np.identity(6),
robust_kernel=None):
edge = g2o.EdgeSE3()
for i, v in enumerate(vertices):
if isinstance(v, int):
v = self.vertex(v)
edge.set_vertex(i, v)
if measurement is None:
measurement = (edge.vertex(0).estimate().inverse() *
edge.vertex(1).estimate())
edge.set_measurement(measurement)
edge.set_information(information)
if robust_kernel is not None:
edge.set_robust_kernel(robust_kernel)
super().add_edge(edge)
def add_edge(self,
vertices,
measurement,
information=np.identity(6),
robust_kernel=None):
"""
Adds an edge between two vertices to an optimizer object.
vertices: array of length two containing the id's of the vertices you wish to link
measurement: g2o.Isometry3d object corresponding to the translation between vertices
"""
edge = g2o.EdgeSE3()
for i, v in enumerate(vertices):
if isinstance(v, int):
v = self.vertex(v)
edge.set_vertex(i, v)
edge.set_measurement(measurement) # relative pose
edge.set_information(information)
if robust_kernel is not None:
edge.set_robust_kernel(robust_kernel)
super().add_edge(edge)
def add_edge(self,
vertices,
measurement,
information=np.identity(6),
robust_kernel=None):
"""Add edge
Args:
vertices (list): two vertices / vertex index
measurement (g2o.Isometry3d): relative pose T_ij
"""
edge = g2o.EdgeSE3()
for i, v in enumerate(vertices):
if isinstance(v, int):
v = self.vertex(v)
edge.set_vertex(i, v)
edge.set_measurement(measurement) # relative pose
edge.set_information(information)
if robust_kernel is not None:
edge.set_robust_kernel(robust_kernel)
super().add_edge(edge)
def create_edge(self, row: pd.Series) -> g2o.EdgeSE3:
euler_angles = row[['euler_x', 'euler_y', 'euler_z']].values
translation = row[['t_x', 't_y', 't_z']].values
rotation_matrix = convert_euler_angles_to_rotation_matrix(euler_angles)
measurement = self.create_pose(rotation_matrix, translation)
edge = g2o.EdgeSE3()
edge.set_measurement(measurement)
euler_angles_std = row[['euler_x_confidence', 'euler_y_confidence', 'euler_z_confidence']].values
translation_std = row[['t_x_confidence', 't_y_confidence', 't_z_confidence']].values
covariance = get_covariance_matrix_from_euler_uncertainty(translation_std, euler_angles_std)
covariance = convert_euler_uncertainty_to_quaternion_uncertainty(euler_angles, covariance)
information = np.linalg.pinv(covariance)
information = np.delete(information, 3, axis=0)
information = np.delete(information, 3, axis=1)
edge.set_information(information)
edge.set_vertex(0, self.optimizer.vertex(int(row['from_index'])))
edge.set_vertex(1, self.optimizer.vertex(int(row['to_index'])))
return edge
def graph_to_optimizer(self):
"""Convert a :class: graph to a :class: g2o.SparseOptimizer. Only the edges and vertices fields need to be
filled out.
Returns:
A :class: g2o.SparseOptimizer that can be optimized via its optimize class method.
"""
optimizer = g2o.SparseOptimizer()
optimizer.set_algorithm(g2o.OptimizationAlgorithmLevenberg(
g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())))
if self.is_sparse_bundle_adjustment:
for i in self.vertices:
if self.vertices[i].mode == VertexType.TAGPOINT:
vertex = g2o.VertexSBAPointXYZ()
vertex.set_estimate(self.vertices[i].estimate[:3])
else:
vertex = g2o.VertexSE3Expmap()
vertex.set_estimate(pose_to_se3quat(self.vertices[i].estimate))
vertex.set_id(i)
vertex.set_fixed(self.vertices[i].fixed)
optimizer.add_vertex(vertex)
cam_idx = 0
for i in self.edges:
if self.edges[i].corner_ids is None:
edge = g2o.EdgeSE3Expmap()
for j, k in enumerate([self.edges[i].startuid,
self.edges[i].enduid]):
edge.set_vertex(j, optimizer.vertex(k))
edge.set_measurement(pose_to_se3quat(self.edges[i].measurement))
edge.set_information(self.edges[i].information)
optimizer.add_edge(edge)
else:
# Note: we only use the focal length in the x direction since: (a) that's all that g2o supports and
# (b) it is always the same in ARKit (at least currently)
cam = g2o.CameraParameters(self.edges[i].camera_intrinsics[0],
self.edges[i].camera_intrinsics[2:], 0)
cam.set_id(cam_idx)
optimizer.add_parameter(cam)
for corner_idx, corner_id in enumerate(self.edges[i].corner_ids):
edge = g2o.EdgeProjectPSI2UV()
edge.resize(3)
edge.set_vertex(0, optimizer.vertex(corner_id))
edge.set_vertex(1, optimizer.vertex(self.edges[i].startuid))
edge.set_vertex(2, optimizer.vertex(self.edges[i].enduid))
edge.set_information(self.edges[i].information)
edge.set_measurement(self.edges[i].measurement[corner_idx * 2:corner_idx * 2 + 2])
edge.set_parameter_id(0, cam_idx)
if self.use_huber:
edge.set_robust_kernel(g2o.RobustKernelHuber(self.huber_delta))
optimizer.add_edge(edge)
cam_idx += 1
else:
for i in self.vertices:
vertex = g2o.VertexSE3()
vertex.set_id(i)
vertex.set_estimate(pose_to_isometry(self.vertices[i].estimate))
vertex.set_fixed(self.vertices[i].fixed)
optimizer.add_vertex(vertex)
for i in self.edges:
edge = g2o.EdgeSE3()
for j, k in enumerate([self.edges[i].startuid,
self.edges[i].enduid]):
edge.set_vertex(j, optimizer.vertex(k))
edge.set_measurement(pose_to_isometry(self.edges[i].measurement))
edge.set_information(self.edges[i].information)
edge.set_id(i)
optimizer.add_edge(edge)
return optimizer
def add_edge(self,
vertex0_id,
vertex1_id,
measure,
measure_information=None,
robust_kernel_value=None):
'''
Helper function to add an edge
vertex 0 and 1 are valid IDs of vertex inside optimizer.
measure is a Isometry3d that map the transform from vertex0 to vertex1
'''
## TODO
# robust kernels? They are made to reduce the impact of outliers.
# They have to be specified separatly for each edge, with a value
# example : robust_kernel = g2o.RobustKernelHuber(np.sqrt(5.991))
# edge.set_robust_kernel(robust_kernel)
##
# print("add_edge")
# print(optimizer.vertices())
if vertex0_id in self.optimizer.vertices():
with self.lock:
if vertex1_id not in self.optimizer.vertices():
vc1 = g2o.VertexSE3()
vc1.set_id(vertex1_id)
vc1.set_estimate(
self.optimizer.vertex(vertex0_id).estimate() * measure)
vc1.set_fixed(False)
self.optimizer.add_vertex(vc1)
self.set_of_new_vertex.add(vc1)
edge = g2o.EdgeSE3()
edge.set_vertex(0, self.optimizer.vertex(vertex0_id))
edge.set_vertex(1, self.optimizer.vertex(vertex1_id))
edge.set_measurement(measure)
if measure_information is not None:
edge.set_information(measure_information)
if robust_kernel_value is not None:
robust_kernel = g2o.RobustKernelHuber(robust_kernel_value)
edge.set_robust_kernel(robust_kernel)
self.optimizer.add_edge(edge)
# Adding edge to set of new edge to update initialization
self.set_of_new_edges.add(edge)
else:
if vertex1_id in self.optimizer.vertices():
with self.lock:
vc0 = g2o.VertexSE3()
vc0.set_id(vertex0_id)
vc0.set_estimate(
self.optimizer.vertex(vertex1_id).estimate() *
measure.inverse())
vc0.set_fixed(False)
self.optimizer.add_vertex(vc0)
self.set_of_new_vertex.add(vc0)
edge = g2o.EdgeSE3()
edge.set_vertex(0, self.optimizer.vertex(vertex0_id))
edge.set_vertex(1, self.optimizer.vertex(vertex1_id))
edge.set_measurement(measure)
if (measure_information):
edge.set_information(measure_information)
# edge.set_information(np.eye(6) * 2)
finished = self.optimizer.add_edge(edge)
self.set_of_new_edges.add(edge)
else:
with self.lock:
vc0 = g2o.VertexSE3()
vc0.set_id(vertex0_id)
vc0.set_fixed(False)
self.optimizer.add_vertex(vc0)
self.set_of_new_vertex.add(vc0)
self.add_edge(vertex0_id,
vertex1_id,
measure,
robust_kernel_value=robust_kernel_value)
