search

mit-acl / dpgo

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

[DPGO_solver.cpp:294] Check failed: m.p1 == src #3

Open adthoms opened 2 months ago

adthoms commented 2 months ago

Summary: In executing single-robot-example and multi-robot-example on datasets in dpgo/data/, a check fails within DPGO_solver.cpp and returns

F0523 17:45:44.194057 295076 DPGO_solver.cpp:294] Check failed: m.p1 == src
*** Check failure stack trace: ***
Aborted (core dumped)

This error has been observed on the following datasets:

Reproducing Error:

  1. Change directory to build

    cd ~/catkin_ws/src/dpgo/build

  2. Execute single-robot-example, where dataset.g2o is the name of a file from the list provided in the bug summary section.

    ./bin/single-robot-example ../data/dataset.g2o

    the program returns

    alex@alex:~/catkin_ws/src/dpgo/build$ ./bin/single-robot-example ../data/ais2klinik.g2o
Single robot pose-graph optimization demo. 
WARNING: Logging before InitGoogleLogging() is written to STDERR
I0523 17:46:07.459980 295336 PGOAgent.cpp:224] Computing local odometry initialization.
F0523 17:46:07.477582 295336 DPGO_solver.cpp:294] Check failed: m.p1 == src 
*** Check failure stack trace: ***
Aborted (core dumped)

  3. Execute multi-robot-example, where dataset.g2o is the name of a file from the list provided in the bug summary section.

    ./bin/multi-robot-example 5 ../data/data_sets.g2o

    the program returns

    alex@alex:~/catkin_ws/src/dpgo/build$ ./bin/multi-robot-example 3 ../data/ais2klinik.g2o
Multi-robot pose graph optimization example. 
Simulating 3 robots.
Loaded dataset from file ../data/ais2klinik.g2o.
WARNING: Logging before InitGoogleLogging() is written to STDERR
I0523 17:38:51.861912 290697 PGOAgent.cpp:224] Computing local odometry initialization.
I0523 17:38:51.873019 290697 PGOAgent.cpp:344] Robot 0 initializes in global frame!
I0523 17:38:51.876274 290697 PGOAgent.cpp:224] Computing local odometry initialization.
F0523 17:38:51.876972 290697 DPGO_solver.cpp:294] Check failed: m.p1 == src 
*** Check failure stack trace: ***
Aborted (core dumped)
adthoms commented 2 months ago

Bug: The datasets listed in the bug Summary section have discontinuous odometry measurements. The check that fails in DPGO_solver.cpp does not explicitly state this, though correctly checks for continuous odometry measurements.

Fix: Add missing odometry measurements to datasets (using slerp interpolation) and include a descriptive error message when check fails in DPGO_solver.cpp.

@yuluntian I will create a PR that resolves this bug and link it to this issue.

yuluntian commented 2 months ago

@adthoms Thanks for raising this issue and the suggested fix sounds good. I'm happy to review and help merge this PR.