def add_edge(self, fromVertex, toVertex, dx, information=np.identity(3)):
edge = g2o.EdgeSE2()
for i, v in enumerate([fromVertex, toVertex]):
if isinstance(v, int):
v = self.optimizer.vertex(v)
edge.set_vertex(i, v)
edge.set_measurement(g2o.SE2(dx, 0, 0)) # relative pose
edge.set_information(information)
self.optimizer.add_edge(edge)
def add_edge(self, vertices, measurement,
information=np.identity(3),
robust_kernel=None):
edge = g2o.EdgeSE2()
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(PoseGraphOptimization, self).add_edge(edge)
def add_edge(self, vertices, measurement, typeit, robust_kernel=None):
if typeit == 1:
edge = g2o.EdgeSE2() # between poses
information = np.identity(3)
else:
edge = g2o.EdgeSE2PointXY() # between pose and landmark
information = np.identity(2)
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(GraphOptimization, self).add_edge(edge)
print("add edge", vertices)
def add_edge(self,
vertices,
measurement,
is_between_pose,
robust_kernel=None):
if is_between_pose:
information = np.identity(3)
edge = g2o.EdgeSE2() # between pose nodes
else:
information = np.identity(2)
edge = g2o.EdgeSE2PointXY() # between pose node and landmark node
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(PoseGraphOptimization, self).add_edge(edge)