search

uoip / g2opy

Python binding of SLAM graph optimization framework g2o
662 stars 175 forks source link

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

Open HongYegg opened 9 months ago

HongYegg commented 9 months ago

`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. 1

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 commented 5 months ago

https://github.com/miquelmassot/g2o-python/issues/27#issuecomment-1962427438