miquelmassot
Examples don't work
I have tried some examples of pose graph optimization, and found out there are several functions not working. Such as
optimizer.load()
in https://github.com/RainerKuemmerle/g2o/blob/pymem/python/examples/simple_optimize.py; and
optimizer.optimize()
in https://github.com/RainerKuemmerle/g2o/tree/pymem/python.
or optimizer.save()
So I guess the project hasn't been finished? Thank you for your kind effort anything, looking forward to the next update.
I think I can reproduce what is happening here: You are using the pip installation of g2o-python? There g2o itself is build with static libraries. By this we miss the factory initialization required for load/save to work. Hence, simple_optimize does not work. Created a PR in g2o to hopefully address this issue.
Thanks a lot! I am using the pip installed g2o-python. Besides load/save, there is also a problem of SparseOptimizer.optimize() not working when I try to create and optimize my own pose graph, with all chi2 being 0. I think this has been addressed here https://github.com/uoip/g2opy/issues/79. I don't know if it's related to the same problem. Thank you anyway for the reply.
We need to specify information matrices. I will see, if we should provide identity by default instead to avoid this issue in the future.
Following modified code from the above issue should work.
import numpy as np
import g2o
optimizer = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE2(g2o.LinearSolverEigenSE2())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
optimizer.set_algorithm(solver)
pose1 = g2o.VertexSE2()
pose1.set_id(0)
pose1.set_estimate(g2o.SE2(0, 0, 0))
pose1.set_fixed(True)
optimizer.add_vertex(pose1)
pose2 = g2o.VertexSE2()
pose2.set_id(1)
pose2.set_estimate(g2o.SE2(1, 1, np.pi/2))
optimizer.add_vertex(pose2)
pose3 = g2o.VertexSE2()
pose3.set_id(2)
pose3.set_estimate(g2o.SE2(6, 3, np.pi))
optimizer.add_vertex(pose3)
point1 = g2o.VertexPointXY()
point1.set_id(3)
point1.set_estimate(np.array([0, 3]))
optimizer.add_vertex(point1)
point2 = g2o.VertexPointXY()
point2.set_id(4)
point2.set_estimate(np.array([2, 3]))
optimizer.add_vertex(point2)
point3 = g2o.VertexPointXY()
point3.set_id(5)
point3.set_estimate(np.array([0, 6]))
optimizer.add_vertex(point3)
edge1_pose_pose = g2o.EdgeSE2()
edge1_pose_pose.set_vertex(0, pose1)
edge1_pose_pose.set_vertex(1, pose2)
edge1_pose_pose.set_measurement(g2o.SE2(4, 3, np.pi/2))
edge1_pose_pose.set_information(np.identity(3))
optimizer.add_edge(edge1_pose_pose)
edge2_pose_pose = g2o.EdgeSE2()
edge2_pose_pose.set_vertex(0, pose2)
edge2_pose_pose.set_vertex(1, pose3)
edge2_pose_pose.set_measurement(g2o.SE2(0, -2, np.pi/2))
edge2_pose_pose.set_information(np.identity(3))
optimizer.add_edge(edge2_pose_pose)
edge1_pose_point = g2o.EdgeSE2PointXY()
edge1_pose_point.set_vertex(0, pose1)
edge1_pose_point.set_vertex(1, point1)
edge1_pose_point.set_measurement(np.array([0, 3]))
edge1_pose_point.set_information(np.identity(2))
optimizer.add_edge(edge1_pose_point)
edge2_pose_point = g2o.EdgeSE2PointXY()
edge2_pose_point.set_vertex(0, pose1)
edge2_pose_point.set_vertex(1, point2)
edge2_pose_point.set_measurement(np.array([2, 3]))
edge2_pose_point.set_information(np.identity(2))
optimizer.add_edge(edge2_pose_point)
edge3_pose_point = g2o.EdgeSE2PointXY()
edge3_pose_point.set_vertex(0, pose1)
edge3_pose_point.set_vertex(1, point3)
edge3_pose_point.set_measurement(np.array([0, 6]))
edge3_pose_point.set_information(np.identity(2))
optimizer.add_edge(edge3_pose_point)
edge4_pose_point = g2o.EdgeSE2PointXY()
edge4_pose_point.set_vertex(0, pose2)
edge4_pose_point.set_vertex(1, point1)
edge4_pose_point.set_measurement(np.array([0, 2]))
edge4_pose_point.set_information(np.identity(2))
optimizer.add_edge(edge4_pose_point)
edge5_pose_point = g2o.EdgeSE2PointXY()
edge5_pose_point.set_vertex(0, pose2)
edge5_pose_point.set_vertex(1, point2)
edge5_pose_point.set_measurement(np.array([0, 4]))
edge5_pose_point.set_information(np.identity(2))
optimizer.add_edge(edge5_pose_point)
edge6_pose_point = g2o.EdgeSE2PointXY()
edge6_pose_point.set_vertex(0, pose2)
edge6_pose_point.set_vertex(1, point3)
edge6_pose_point.set_measurement(np.array([3, 4]))
edge6_pose_point.set_information(np.identity(2))
optimizer.add_edge(edge6_pose_point)
edge7_pose_point = g2o.EdgeSE2PointXY()
edge7_pose_point.set_vertex(0, pose3)
edge7_pose_point.set_vertex(1, point1)
edge7_pose_point.set_measurement(np.array([6, 0]))
edge7_pose_point.set_information(np.identity(2))
optimizer.add_edge(edge7_pose_point)
edge8_pose_point = g2o.EdgeSE2PointXY()
edge8_pose_point.set_vertex(0, pose3)
edge8_pose_point.set_vertex(1, point2)
edge8_pose_point.set_measurement(np.array([4, 0]))
edge8_pose_point.set_information(np.identity(2))
optimizer.add_edge(edge8_pose_point)
edge9_pose_point = g2o.EdgeSE2PointXY()
edge9_pose_point.set_vertex(0, pose3)
edge9_pose_point.set_vertex(1, point3)
edge9_pose_point.set_measurement(np.array([6, -3]))
edge9_pose_point.set_information(np.identity(2))
optimizer.add_edge(edge9_pose_point)
optimizer.initialize_optimization()
optimizer.set_verbose(True)
optimizer.optimize(100)
print(optimizer.vertex(2).estimate().translation())
print(optimizer.vertex(4).estimate())