New Segfault during calculateBestEstimate

[marcusabate]

When running VIO with loop closure detector, sometimes when a loop closure is detected there is a segfault during RobustPGO's output. Specifically, I add loop closures in the following way:

nfg.add(gtsam::BetweenFactor<gtsam::Pose3>(
    factor.ref_key_, factor.cur_key_, factor.ref_Pose_cur_, factor.noise_));

pgo_->update(nfg);

Then, when loops are detected I want to return the full trajectory. First I want a specific element from the values of the pgo so that I can compute an error transform. This is done like so:

gtsam::Pose3 w_Pose_Bkf_optimal =
      pgo_->calculateBestEstimate().at<gtsam::Pose3>(
          W_Pose_Bkf_estimates_.size() - 1);

W_Pose_Bkf_estimates_ is guaranteed to be of same size as pgo_'s values.

I get this error (in gdb):

Thread 1 "lcd_ros_node" received signal SIGSEGV, Segmentation fault.
0x00007ffff5f63610 in vtable for gtsam::BetweenFactor<gtsam::Pose3> ()
   from /home/marcus/code/VIO/build/libSparkVio.so
(gdb) backtrace
#0  0x00007ffff5f63610 in vtable for gtsam::BetweenFactor<gtsam::Pose3> ()
    at /home/marcus/code/VIO/build/libSparkVio.so
#1  0x00007ffff565d30d in gtsam::Values::tryInsert(unsigned long, gtsam::Value const&) ()
    at /usr/local/lib/libgtsam.so.4
#2  0x00007ffff565d64d in gtsam::Values::insert(unsigned long, gtsam::Value const&) ()
    at /usr/local/lib/libgtsam.so.4
#3  0x00007ffff565d811 in gtsam::Values::insert(gtsam::Values const&) () at /usr/local/lib/libgtsam.so.4
#4  0x00007ffff565da5d in gtsam::Values::Values(gtsam::Values const&) () at /usr/local/lib/libgtsam.so.4
#5  0x00007ffff5ade609 in GenericSolver::calculateBestEstimate() (this=<optimized out>)
    at /usr/local/include/RobustPGO/GenericSolver.h:46
#6  0x00007ffff5ade609 in VIO::LoopClosureDetector::getWPoseMap() const (this=0x5555561f9290)
    at /home/marcus/code/VIO/src/LoopClosureDetector.cpp:773
#7  0x00007ffff5ade609 in VIO::LoopClosureDetector::spinOnce(std::shared_ptr<VIO::LoopClosureDetectorInputPayload> const&) (this=0x5555561f9290, input=std::shared_ptr<VIO::LoopClosureDetectorInputPayload> (use count 1, weak count 0) = {...}) at /home/marcus/code/VIO/src/LoopClosureDetector.cpp:180
#8  0x000055555562dc59 in VIO::LCD_ros::spin() ()
#9  0x000055555561d5e1 in main ()

So during calculateBestEstimate there is an issue. But this simply returns the class gtsam::Values values member, so there shouldn't be anything wrong there. The backtrace suggests it is a problem in the gtsam library. @yunzc have you seen this before?

[marcusabate]

It's worth clarifying that this isn't happening during the update step. The factors are all added properly. The problem comes when I make a call to RobustPGO::calculateBestEstimate or RobustPGO::calculateEstimate. These functions are simple getters for the values_ field of the GenericSolver base class.

I'm not sure why returning values_ should involve a call to gtsam::Values::tryInsert.

[yunzc]

I will look into this more but note that .size() might be giving issues. When you call .at(arg) the argument should be the gtsam key instead of the index (see https://borg.cc.gatech.edu/sites/edu.borg/html/a00261.html)

[marcusabate]

It looks like simply calling RobustPGO::calculateEstimate() is what is causing the issue, not size() or even at<gtsam::Pose3>(). But RobustPGO:: calculateEstimate() simply returns the class member so I'm not sure what is going wrong, unless the copy constructor for gtsam::Values is doing something strange.

[yunzc]

weird hmm since we are also using this a lot in another project and never had any issues with seg faults