ISAM2 with Smart Factors null ptr segfault when using pose3 priors

While testing a simple ISAM2 + smart factors graph optimization, I encountered a null ptr segfault inside ISAM2's optimizeWildfireNode. The backtrace is below:

#!

ISAM2Example_SmartFactor: /usr/include/boost/smart_ptr/shared_ptr.hpp:734: typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = gtsam::ISAM2Clique; typename boost::detail::sp_member_access<T>::type = gtsam::ISAM2Clique*]: Assertion `px != 0' failed.

Thread 1 "ISAM2Example_Sm" received signal SIGABRT, Aborted.
0x00007ffff4063860 in raise () from /usr/lib/libc.so.6
(gdb) bt
#0  0x00007ffff4063860 in raise () from /usr/lib/libc.so.6
#1  0x00007ffff4064ec9 in abort () from /usr/lib/libc.so.6
#2  0x00007ffff405c0bc in __assert_fail_base () from /usr/lib/libc.so.6
#3  0x00007ffff405c133 in __assert_fail () from /usr/lib/libc.so.6
#4  0x00007ffff74da805 in boost::shared_ptr<gtsam::ISAM2Clique>::operator-> (this=0x7fffffffc130)
    at /usr/include/boost/smart_ptr/shared_ptr.hpp:734
#5  0x00007ffff74dca6d in gtsam::internal::optimizeWildfireNode<gtsam::ISAM2Clique> (clique=..., threshold=0.001, changed=..., 
    replaced=..., delta=..., count=@0x7fffffffc568: 0) at /home/alex/sources/gtsam/gtsam/nonlinear/ISAM2-inl.h:163
#6  0x00007ffff74daea6 in gtsam::optimizeWildfireNonRecursive<gtsam::ISAM2Clique> (root=..., threshold=0.001, keys=..., delta=...)
    at /home/alex/sources/gtsam/gtsam/nonlinear/ISAM2-inl.h:277
#7  0x00007ffff74d8acb in gtsam::ISAM2::Impl::UpdateGaussNewtonDelta (roots=..., replacedKeys=..., delta=..., 
    wildfireThreshold=0.001) at /home/alex/sources/gtsam/gtsam/nonlinear/ISAM2-impl.cpp:291
#8  0x00007ffff74efd24 in gtsam::ISAM2::updateDelta (this=0x7fffffffd630, forceFullSolve=false)
    at /home/alex/sources/gtsam/gtsam/nonlinear/ISAM2.cpp:983
#9  0x00007ffff74f030c in gtsam::ISAM2::getDelta (this=0x7fffffffd630) at /home/alex/sources/gtsam/gtsam/nonlinear/ISAM2.cpp:1053
#10 0x00007ffff74f0081 in gtsam::ISAM2::calculateEstimate (this=0x7fffffffd630)
    at /home/alex/sources/gtsam/gtsam/nonlinear/ISAM2.cpp:1027
#11 0x00005555557657d5 in main (argc=1, argv=0x7fffffffe218) at /home/alex/sources/gtsam/examples/ISAM2Example_SmartFactor.cpp:93

The issue seems to be that parent is null:

#!

(gdb) up 5
#5  0x00007ffff74dca6d in gtsam::internal::optimizeWildfireNode<gtsam::ISAM2Clique> (clique=..., threshold=0.001, changed=..., 
    replaced=..., delta=..., count=@0x7fffffffc568: 0) at /home/alex/sources/gtsam/gtsam/nonlinear/ISAM2-inl.h:163
163               clique->solnPointers_.insert(std::make_pair(key, parent->solnPointers_.at(key)));
(gdb) print parent
$1 = {px = 0x0, pn = {pi_ = 0x0}}

I've also attached the source code for the program. The graph is a sequence of poses, each with priors and a measurement of a single landmark using a smart factor. The same program works if the pose prior factors are replaced with pose between factors (see the commented line).

Is ISAM2 not designed to work with a graph with only priors? Switching to ISAM (the original) seems to work, so I thought this might be a bug in ISAM2's implementation.

Another unrelated issue I ran into, is that with GTSAM_EXTRA_CONSISTENCY_CHECKS set to true and cacheLinearizedFactors set to true, the program fails the assertion on line 235 of ISAM2.cpp. Do some of the assumptions that the assertions make fail when using smart factors?

Thanks!

Comments (4)