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

[adthoms]

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:

• ais2klinik.g2o (Note: line 2 of the file FIX 0 should be ignored when reading g2o file)
• cubicle.g2o
• g2o50k.g2o
• g2o100k.g2o
• rim.g2o

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]

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]

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