def add_landmark_edge(self, vertices,
robust_kernel=None):
edge = g2o.EdgeSE2PointXY()
for i, v in enumerate(vertices):
if isinstance(v, int):
v = self.vertex(v)
edge.set_vertex(i, v)
if robust_kernel is not None:
edge.set_robust_kernel(robust_kernel)
super(PoseGraphOptimization, self).add_edge(edge)
def add_landmark_edge(self,
vertices,
xy,
information=np.identity(2),
robust_kernel=None):
edge = g2o.EdgeSE2PointXY()
for i, v in enumerate(vertices):
if isinstance(v, int): # vertex id provided; retrieve vertex
v = self.vertex(v)
edge.set_vertex(i, v)
edge.set_measurement(xy) # 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_landmark_edge(self,
vertex_id,
landmark_id,
dx,
information=np.identity(2)):
edge = g2o.EdgeSE2PointXY()
for i, v in enumerate([vertex_id, landmark_id]):
if isinstance(v, int):
v = self.optimizer.vertex(v)
edge.set_vertex(i, v)
edge.set_measurement([dx, 0])
edge.set_information(information)
self.optimizer.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)