search

mit-acl / dpgo

Distributed Pose Graph Optimization
MIT License
177 stars 26 forks source link

An error occurred, after replacing it with my own g2o file. #4

Closed lqy668honor closed 2 months ago

lqy668honor commented 3 months ago

Thank you for your great work. I encountered the following error while trying to run ./bin/single-robot-example using my own g2o file:

Running local pose graph optimization...
=========================RTRNewton=========================
i:0,f:-nan,|gf|:-nan,
X_{1} WAS REJECTED.
i:1,f:nan,df/f:-nan,|gf|:-nan,time:8.44e-02,nf:2,ng:1,nR:1,nH:51,rho:nan,radius:1.000e+02,tCGstatus:MAXITER,innerIter:50,
X_{2} WAS REJECTED.
i:2,f:nan,df/f:-nan,|gf|:-nan,time:1.67e-01,nf:3,ng:1,nR:2,nH:102,rho:nan,radius:1.000e+02,tCGstatus:MAXITER,innerIter:50,
X_{3} WAS REJECTED.
i:3,f:nan,df/f:-nan,|gf|:-nan,time:2.49e-01,nf:4,ng:1,nR:3,nH:153,rho:nan,radius:1.000e+02,tCGstatus:MAXITER,innerIter:50,
Iter:3,f:nan,|gf|:-nan,|gf|/|gf0|:-nan,time:2.50e-01,nf:4,ng:1,nR:3,nH:153,
Cost = -nan

May I ask what the reason is and how can I fix it?

yuluntian commented 3 months ago

Hi, thanks for your interest in this work. Could you confirm that the code works with one of the provided g2o files? If you upload the custom g2o file I am also happy to take a look.

lqy668honor commented 3 months ago

Thanks for your reply. I ran the code according to readme.md and got the following results:

lqy@AP201:~/dpgo/build$ ./bin/single-robot-example ../data/smallGrid3D.g2o 
Single robot pose-graph optimization demo. 
WARNING: Logging before InitGoogleLogging() is written to STDERR
I0611 23:38:36.230044 862862 PGOAgent.cpp:224] Computing local odometry initialization.
I0611 23:38:36.230399 862862 PGOAgent.cpp:344] Robot 0 initializes in global frame!
Running local pose graph optimization...
=========================RTRNewton=========================
i:0,f:7.807e+02,|gf|:2.376e+02,
X_{1} WAS ACCEPTED. Rho: 0.954505, Acceptance_Rho: 0.100000
i:1,f:5.185e+02,df/f:5.058e-01,|gf|:4.829e+01,time:5.41e-03,nf:2,ng:2,nR:1,nH:9,rho:9.55e-01,radius:1.000e+02,tCGstatus:LCON,innerIter:7,
X_{2} WAS ACCEPTED. Rho: 1.003916, Acceptance_Rho: 0.100000
i:2,f:5.127e+02,df/f:1.115e-02,|gf|:4.529e+00,time:8.70e-03,nf:3,ng:3,nR:2,nH:17,rho:1.00e+00,radius:1.000e+02,tCGstatus:LCON,innerIter:6,
X_{3} WAS ACCEPTED. Rho: 1.000007, Acceptance_Rho: 0.100000
i:3,f:5.127e+02,df/f:7.995e-05,|gf|:3.452e-01,time:1.24e-02,nf:4,ng:4,nR:3,nH:26,rho:1.00e+00,radius:1.000e+02,tCGstatus:LCON,innerIter:7,
Iter:3,f:5.127e+02,|gf|:3.452e-01,|gf|/|gf0|:1.453e-03,time:1.24e-02,nf:4,ng:4,nR:3,nH:26,
Cost = 1025.4

My g2o file have been packed. data.zip.Thank you very much for taking the time to help me!

yuluntian commented 2 months ago

The output on smallGrid3D.g2o looks correct. After looking into the custom g2o file, I think the problem is caused by the missing information matrix of each edge. To see what I mean, you can compare each EDGE_SE3:QUAT line in smallGrid3D.g2o versus the custom g2o file. The extra fields in smallGrid3D.g2o specify each edge's information matrix (intuitively, how accurate each measurement is)

lqy668honor commented 2 months ago

That's the problem. I've solved it. Thank you!