Hello, why can’t I get optimized results? Can you help me improve the code?

[HongYegg]

`import numpy as np
import g2o

optimizer = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE2(g2o.LinearSolverCholmodSE2())
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))
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))
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]))
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]))
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]))
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]))
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]))
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]))
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]))
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]))
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]))
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())
`
This is the problem I want to solve.

Below is the console output：

/home/why/anaconda3/envs/g2opy/bin/python /home/why/PycharmProjects/g2opy/myg2o.py
iteration= 0 chi2= 0.000000 time= 4.0684e-05 cumTime= 4.0684e-05 edges= 11 schur= 0 lambda= 1456597823539583947129309713666812946101870135966217720567140156314062197717880343734495709738343057914640350155485593730049698930455020468865037460369357458472516483337890880902153031012130488320.000000 levenbergIter= 1
[6. 3.]
[2. 3.]

Process finished with exit code 0

Thank you so much

[RainerKuemmerle]

miquelmassot/g2o-python#27 (comment)