def add_vertex(self, id, x_estimate, fixed=False):
v_se2 = g2o.VertexSE2()
v_se2.set_id(id)
se2 = g2o.SE2(x_estimate, 0, 0)
v_se2.set_estimate(se2)
v_se2.set_fixed(fixed)
self.optimizer.add_vertex(v_se2)
def add_vertex(self, id, pose, typeit, fixed=False):
if typeit == 1: v_se2 = g2o.VertexSE2() # pose
else: v_se2 = g2o.VertexPointXY() # landmark
v_se2.set_id(id)
v_se2.set_estimate(pose)
v_se2.set_fixed(fixed)
super(GraphOptimization, self).add_vertex(v_se2)
print("add vertex", id)
def add_vertex(self, id, pose, is_landmark, fixed=False):
if is_landmark:
v_se2 = g2o.VertexPointXY()
else:
v_se2 = g2o.VertexSE2()
v_se2.set_id(id)
v_se2.set_estimate(pose)
v_se2.set_fixed(fixed)
super(PoseGraphOptimization, self).add_vertex(v_se2)
def add_pose_vertex(self, se2, fixed=False):
id = self.get_next_vertex_id() # increment global pose id counter
v_se2 = g2o.VertexSE2()
v_se2.set_id(id) # assign vertex id
v_se2.set_estimate(
se2) # pass the se2 param (x, y, theta) as the estimate
# v_se2.set_fixed(True if self.last_pose_vertex_id == -1 else fixed) # fix the first node
v_se2.set_fixed(fixed)
if self.last_pose_vertex_id == -1: # store the first pose vertex id for later use during error plotting
self.first_pose_vertex_id = id
self.last_pose_vertex_id = id # update last pose vertex id
super(PoseGraphOptimization,
self).add_vertex(v_se2) # register the new pose vertex
return v_se2, id # return the new pose vertex and its id
def add_vertex(self, id, pose, fixed=False):
v_se2 = g2o.VertexSE2()
v_se2.set_id(id)
v_se2.set_estimate(pose)
v_se2.set_fixed(fixed)
super(PoseGraphOptimization, self).add_vertex(v_se2)
def add_vertex(self, id, pose, fixed=False):
v_se2 = g2o.VertexSE2()
v_se2.set_id(id)
v_se2.set_estimate(pose)
v_se2.set_fixed(fixed)
super().add_vertex(v_se2)