search

UZ-SLAMLab / ORB_SLAM3

ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM
GNU General Public License v3.0
6.62k stars 2.57k forks source link

g2o Assertion `_sizePoses > 0 && "allocating with wrong size"' failed. #259

Open PfeifferMicha opened 3 years ago

PfeifferMicha commented 3 years ago

I quite regularly get this crash:

block_solver.hpp:75: void g2o::BlockSolver::resize(int, int, int, int, int) [with Traits = g2o::BlockSolverTraits<6, 3>]: Assertion `_sizePoses > 0 && "allocating with wrong size"' failed.

It seems to happen whenever the system got lost, created a new map and then re-detects the first map and wants to perform a merge.

Not sure if I should report this on g2o's issue tracker or here.

Backtrace below:

orb_slam3_stereo: /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/Thirdparty/g2o/g2o/core/block_solver.hpp:75: void g2o::BlockSolver<Traits>::resize(int*, int, int*, int, int) [with Traits = g2o::BlockSolverTraits<6, 3>]: Assertion `_sizePoses > 0 && "allocating with wrong size"' failed.

Thread 7 "orb_slam3_stere" received signal SIGABRT, Aborted.
[Switching to Thread 0x7fffd9104700 (LWP 1605993)]
__GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
50      ../sysdeps/unix/sysv/linux/raise.c: No such file or directory.
(gdb) bt
#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
#1  0x00007ffff6c80859 in __GI_abort () at abort.c:79
#2  0x00007ffff6c80729 in __assert_fail_base
    (fmt=0x7ffff6e16588 "%s%s%s:%u: %s%sAssertion `%s' failed.\n%n", assertion=0x7ffff7f55f90 "_sizePoses > 0 && \"allocating with wrong size\"", file=0x7ffff7f55e78 "/home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/Thirdparty/g2o/g2o/core/block_solver.hpp", line=75, function=<optimized out>)
    at assert.c:92
#3  0x00007ffff6c91f36 in __GI___assert_fail
    (assertion=0x7ffff7f55f90 "_sizePoses > 0 && \"allocating with wrong size\"", file=0x7ffff7f55e78 "/home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/Thirdparty/g2o/g2o/core/block_solver.hpp", line=75, function=0x7ffff7f57f28 "void g2o::BlockSolver<Traits>::resize(int*, int, int*, int, int) [with Traits = g2o::BlockSolverTraits<6, 3>]") at assert.c:101
#4  0x00007ffff7e3a836 in g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::resize(int*, int, int*, int, int) ()
    at /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/lib/liborb_slam3.so
#5  0x00007ffff7e3bd48 in g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::buildStructure(bool) ()
    at /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/lib/liborb_slam3.so
#6  0x00007ffff6c2cfc7 in g2o::OptimizationAlgorithmLevenberg::solve(int, bool) ()
    at /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/Thirdparty/g2o/lib/libg2o.so
#7  0x00007ffff6c25dbc in g2o::SparseOptimizer::optimize(int, bool) ()
    at /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/Thirdparty/g2o/lib/libg2o.so
#8  0x00007ffff7dd50a1 in ORB_SLAM3::Optimizer::LocalBundleAdjustment(ORB_SLAM3::KeyFrame*, std::vector<ORB_SLAM3::KeyFrame*, std::allocator<ORB_SLAM3::KeyFrame*> >, std::vector<ORB_SLAM3::KeyFrame*, std::allocator<ORB_SLAM3::KeyFrame*> >, bool*) ()
    at /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/lib/liborb_slam3.so
#9  0x00007ffff7d41e96 in ORB_SLAM3::LoopClosing::MergeLocal() () at /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/lib/liborb_slam3.so
#10 0x00007ffff7d45198 in ORB_SLAM3::LoopClosing::Run() () at /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/lib/liborb_slam3.so
#11 0x00007ffff708dd84 in  () at /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#12 0x00007ffff7570609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#13 0x00007ffff6d7d293 in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95

Additional Info:

Ubuntu 20.04, running the ROS example. Changes I made:

Update:

I dug a little deeper and found out that the error happens when all vertices are set "marginalized" by calling setMarginalized() earlier. In this case, _sizePoses will be 0, which is not allowed when _doshur is true. So it looks like the issue above arises because no vertices are optimizable (?) in my current situation. I'll keep digging why that happens...

PfeifferMicha commented 3 years ago

Update: I haven't figured out why this happens, but I can work around the issue by adding the following code:

    // Make sure we have vertices to optimize!
    bool allVerticesMarginalized = true;
    for (size_t i = 0; i < optimizer.indexMapping().size(); ++i) {
        g2o::OptimizableGraph::Vertex* v = optimizer.indexMapping()[i];
        if (! v->marginalized()){
            allVerticesMarginalized = false;
            break;
        }
    }
    if( allVerticesMarginalized )
        return;

I placed this just before both calls to "optimize" in src/Optimizer.cc in the following function

void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector<KeyFrame*> vpAdjustKF, vector<KeyFrame*> vpFixedKF, bool *pbStopFlag)

Note that there are three versions of LocalBundleAdjustment, I only placed the code in this one because the other ones never crashed for me (but I don't know if they were ever called at all).

This code makes sure that at least some vertices are not marginalized, which will make sure the assertion error never triggers. If all vertices are marginalized, I understand this to mean "no vertex should be optimized" in which case I abort the optimization process.

As to why this happens, I am still not sure. The above worked for me but I'm not sure if my workaround is the correct action to be taken, or whether this error is just a symptom of something else that's going wrong.

ghost commented 3 years ago

I had the same exact issue, thank you for posting your workaround, it worked for me!

Same setup:

after adding the check mentioned above the maps are finally able to merge and tracking works !

PfeifferMicha commented 3 years ago

Cool, I'm glad it helped someone!

I was wondering if maybe this was related to the data somehow? I have short sequences and sometimes a bad frame rate. Do you always achieve the 30 FPS? Another issue may be that after getting lost and creating a new map, it very quickly tried to perform the merge - maybe this new map wasn't big enough yet. These are shots in the dark, though, I haven't delved deeper into the code - but maybe we can find a pattern?

ghost commented 3 years ago

I achieve really good performance (30fps) on a real-time stream from a camera using the Intel Realsense D455 camera (stereo-inertial), I will try and create a pull request with the sample.

I am not sure why it happens but both of your guesses seems reasonable.

Mukidashi commented 3 years ago

I identified cause of the bug. Shortly, connections between nodes of a graph of BA is not correctly specified.

In src/Optimizer.cc file, 


 void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector<KeyFrame*> vpAdjustKF, vector<KeyFrame*> vpFixedKF, bool *pbStopFlag) 
 {

  -----------------------------------------

    // Set non fixed Keyframe vertices
    set<KeyFrame*> spAdjustKF(vpAdjustKF.begin(), vpAdjustKF.end());
    for(KeyFrame* pKFi : vpAdjustKF)
    {
        if(pKFi->isBad() || pKFi->GetMap() != pCurrentMap)
            continue;

        //Wrong line
        // In case of commit# 093173baccc70930c87c64bce5b54e5664d6535f, this is line 5657. 
        // pKFi->mnBALocalForKF = pMainKF->mnId; 
        //Correct one
        pKFi->mnBALocalForMerge = pMainKF->mnId;

  -----------------------------------------

 }

This correction make the connection between free Keyframe nodes and Mappoint nodes and g2o solver correctly recognize free variables that are not marginalized.

Sorry for poor English.

I quite regularly get this crash:

block_solver.hpp:75: void g2o::BlockSolver::resize(int, int, int, int, int) [with Traits = g2o::BlockSolverTraits<6, 3>]: Assertion `_sizePoses > 0 && "allocating with wrong size"' failed.

It seems to happen whenever the system got lost, created a new map and then re-detects the first map and wants to perform a merge.

Not sure if I should report this on g2o's issue tracker or here.

Backtrace below:

orb_slam3_stereo: /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/Thirdparty/g2o/g2o/core/block_solver.hpp:75: void g2o::BlockSolver<Traits>::resize(int*, int, int*, int, int) [with Traits = g2o::BlockSolverTraits<6, 3>]: Assertion `_sizePoses > 0 && "allocating with wrong size"' failed.

Thread 7 "orb_slam3_stere" received signal SIGABRT, Aborted.
[Switching to Thread 0x7fffd9104700 (LWP 1605993)]
__GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
50      ../sysdeps/unix/sysv/linux/raise.c: No such file or directory.
(gdb) bt
#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
#1  0x00007ffff6c80859 in __GI_abort () at abort.c:79
#2  0x00007ffff6c80729 in __assert_fail_base
    (fmt=0x7ffff6e16588 "%s%s%s:%u: %s%sAssertion `%s' failed.\n%n", assertion=0x7ffff7f55f90 "_sizePoses > 0 && \"allocating with wrong size\"", file=0x7ffff7f55e78 "/home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/Thirdparty/g2o/g2o/core/block_solver.hpp", line=75, function=<optimized out>)
    at assert.c:92
#3  0x00007ffff6c91f36 in __GI___assert_fail
    (assertion=0x7ffff7f55f90 "_sizePoses > 0 && \"allocating with wrong size\"", file=0x7ffff7f55e78 "/home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/Thirdparty/g2o/g2o/core/block_solver.hpp", line=75, function=0x7ffff7f57f28 "void g2o::BlockSolver<Traits>::resize(int*, int, int*, int, int) [with Traits = g2o::BlockSolverTraits<6, 3>]") at assert.c:101
#4  0x00007ffff7e3a836 in g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::resize(int*, int, int*, int, int) ()
    at /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/lib/liborb_slam3.so
#5  0x00007ffff7e3bd48 in g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::buildStructure(bool) ()
    at /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/lib/liborb_slam3.so
#6  0x00007ffff6c2cfc7 in g2o::OptimizationAlgorithmLevenberg::solve(int, bool) ()
    at /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/Thirdparty/g2o/lib/libg2o.so
#7  0x00007ffff6c25dbc in g2o::SparseOptimizer::optimize(int, bool) ()
    at /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/Thirdparty/g2o/lib/libg2o.so
#8  0x00007ffff7dd50a1 in ORB_SLAM3::Optimizer::LocalBundleAdjustment(ORB_SLAM3::KeyFrame*, std::vector<ORB_SLAM3::KeyFrame*, std::allocator<ORB_SLAM3::KeyFrame*> >, std::vector<ORB_SLAM3::KeyFrame*, std::allocator<ORB_SLAM3::KeyFrame*> >, bool*) ()
    at /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/lib/liborb_slam3.so
#9  0x00007ffff7d41e96 in ORB_SLAM3::LoopClosing::MergeLocal() () at /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/lib/liborb_slam3.so
#10 0x00007ffff7d45198 in ORB_SLAM3::LoopClosing::Run() () at /home/user/Projects/ROS/catkin_ws_noetic/src/orb_slam3/lib/liborb_slam3.so
#11 0x00007ffff708dd84 in  () at /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#12 0x00007ffff7570609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#13 0x00007ffff6d7d293 in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95

Additional Info:

Ubuntu 20.04, running the ROS example. Changes I made:

  • I recompiled both g2o and ORB SLAM 3 without "-march=native", as this would crash on my system (see: Debug build: systematic crash raulmur/ORB_SLAM2#341)
  • Added some publishers to the ROS example, nothing that should influence the SLAM itsel
  • Upgrade to OpenCV 4, by adding a few includes

Update:

I dug a little deeper and found out that the error happens when all vertices are set "marginalized" by calling setMarginalized() earlier. In this case, _sizePoses will be 0, which is not allowed when _doshur is true. So it looks like the issue above arises because no vertices are optimizable (?) in my current situation. I'll keep digging why that happens...