AlejandroSilvestri / osmap

Save and load orb-slam2 maps
84 stars 17 forks source link

When loading a map segmentation fault (core dumped) #8

Closed Alexandre34 closed 3 years ago

Alexandre34 commented 5 years ago

Hi, This is linked with #4 and #5

I try to load a previously generated map using osmap, the compilation works but the execution stops when trying to load a map. Here is the console log with verbose on.

ORB Extractor Parameters: 
- Number of Features: 3000
- Scale Levels: 8
- Scale Factor: 1.2
- Initial Fast Threshold: 16
- Minimum Fast Threshold: 6
Local Mapping RELEASE
System Reseting
Reseting Local Mapper... done
Reseting Loop Closing... done
Reseting Database... done
Local Mapping STOP
Mappoints loaded: 9242
Keyframes loaded: 51
Loading features from myFirstMap.features ...
Features loaded: 143074
Rebuilding map:
Processing 51 keyframes 
pKF: 0xf5b1250
Segmentation fault (core dumped)

I tried the fix given in #5 (adding two lines in Osmap.cpp), but it gives me the following error when compiling.

/home/ubuntu/ORB_SLAM2_GRID_MAP/src/Osmap.cpp:928:35: error: ‘pKF’ was not declared in this scope
   const_cast<std::vector<float>&>(pKF->mvuRight) = vector<float>(n,-1.0f);
                                   ^
/home/ubuntu/ORB_SLAM2_GRID_MAP/src/Osmap.cpp:928:66: error: ‘n’ was not declared in this scope
   const_cast<std::vector<float>&>(pKF->mvuRight) = vector<float>(n,-1.0f);
                                                                  ^

I also tried to put pauseThread = false, but without luck. Here is my main.

int main(int argc, char **argv)
{
    if(argc != 4)
    {
        cerr << endl << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_sequence" << endl;
        return 1;
    }

    // Retrieve paths to images
    vector<string> vstrImageFilenames;
    vector<double> vTimestamps;
    string strFile = string(argv[3])+"/rgb.txt";
    LoadImages(strFile, vstrImageFilenames, vTimestamps);

    int nImages = vstrImageFilenames.size();

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

    // Construct the osmap object, can be right after SLAM construction
    ORB_SLAM2::Osmap osmap = ORB_SLAM2::Osmap(SLAM);

    // Activate verbose for debugging purposes
    osmap.verbose = true;

    // Loading the map
    osmap.mapLoad("myFirstMap.yaml", false);

    // Vector for tracking time statistics
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);

    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;
    cout << "Images in the sequence: " << nImages << endl << endl;

    // Main loop
    cv::Mat im;

    for(int ni=0; ni<nImages; ni++)
    {
        // Read image from file
        im = cv::imread(string(argv[3])+"/"+vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];

        if(im.empty())
        {
            cerr << endl << "Failed to load image at: "
                 << string(argv[3]) << "/" << vstrImageFilenames[ni] << endl;
            return 1;
        }

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif

        // Pass the image to the SLAM system
        SLAM.TrackMonocular(im,tframe);

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif

        double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

        vTimesTrack[ni]=ttrack;

        // Wait to load the next frame
        double T=0;
        if(ni<nImages-1)
            T = vTimestamps[ni+1]-tframe;
        else if(ni>0)
            T = tframe-vTimestamps[ni-1];

        if(ttrack<T)
            usleep((T-ttrack)*1e6);
    }

    // Stop all threads
    SLAM.Shutdown();

    // Tracking time statistics
    sort(vTimesTrack.begin(),vTimesTrack.end());
    float totaltime = 0;
    for(int ni=0; ni<nImages; ni++)
    {
        totaltime+=vTimesTrack[ni];
    }
    cout << "-------" << endl << endl;
    cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
    cout << "mean tracking time: " << totaltime/nImages << endl;

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

    // save the pointcloud
    SLAM.CreatePCD("pointcloud.pcd");

    // When you already has a map to save
    //osmap.mapSave("myFirstMap");

    //view the pointcloud
    //SLAM.ViewCloud("Pointcloud.pcd");

    return 0;
}

I also put the yaml file if you are interested.

%YAML:1.0
---
mappointsFile: "myFirstMap.mappoints"
nMappoints: 9242
keyframesFile: "myFirstMap.keyframes"
nKeyframes: 51
featuresFile: "myFirstMap.features"
nFeatures: 143074
Options: 2
Options descriptions: [ FEATURES_FILE_NOT_DELIMITED ]
cameraMatrices:
   - { fx:6.8279998779296875e+02, fy:6.8650000000000000e+02,
       cx:3.4239999389648438e+02, cy:1.7730000305175781e+02 }

Do you have an idea on how I could solve this problem? Thanks

AlejandroSilvestri commented 5 years ago

@Alexandre34

Those two lines you tried to add are already in the code, here.

They were added in March 29. Is your code older? If so, please download its last version.

We have a problem in rebuild() I'm not able to reproduce. Thanks to your verbose output I have more data to track it down. If I can get rid of this seg fault problem, I will tell you.

You can try to visualize the map with Osmap Viewer, or open the file with the standalone example provided with osmap, not tied to orb-slam2. Let me know if you try some of them, and if the seg fault happens there too or not.

Alexandre34 commented 5 years ago

Hi @AlejandroSilvestri Thank you for your help, I tried installing Osmap viewer but I have an issue with viz.hpp not being found. I think I need to install the contrib modules as you said in the readme.

I would also like to point out that when I execute multiple times the loading map code, I always get a different verbose code. pKF: 0x1047b250 pKF: 0xfde4250

Maybe it's a memory issue, because my recording is quite large (generated from a 2 min VGA video at 15 fps). Thanks

AlejandroSilvestri commented 5 years ago

@Alexandre34, those are memory addresses, may always be different. Their only purpose is to print out pKF is not NULL before seg fault, because there's no safe way to print info after seg fault.

AlejandroSilvestri commented 5 years ago

@Alexandre34 ,

Thanks to your detailed info I know there is a problem rebuilding the very first keyframe, but I need more info to go further. I updated osmap verboseness, I would appreciate if you can recompile, retry and post me the output.

The only updated file is Osmap.cpp .

Thank you

Alexandre34 commented 5 years ago

Hi @AlejandroSilvestri Here is the output with the updated file

ORB Extractor Parameters: 
- Number of Features: 3000
- Scale Levels: 8
- Scale Factor: 1.2
- Initial Fast Threshold: 16
- Minimum Fast Threshold: 6
Local Mapping RELEASE
System Reseting
Reseting Local Mapper... done
Reseting Loop Closing... done
Reseting Database... done
Local Mapping STOP
Mappoints loaded: 9242
Keyframes loaded: 51
Loading features from myFirstMap.features ...
Features loaded: 143074
Rebuilding map:
Processing 51 keyframes 
pKF: 0x10710250
pKF->mnId: 1
pKF->mbNotErase: 0
Segmentation fault (core dumped)
mmajewsk commented 5 years ago

I have simmilar problem:

ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza.
This program comes with ABSOLUTELY NO WARRANTY;
This is free software, and you are welcome to redistribute it
under certain conditions. See LICENSE.txt.

Input sensor was set to: Monocular

Loading ORB Vocabulary. This could take a while...
Vocabulary loaded!

Camera Parameters: 
- fx: 280.329
- fy: 278.756
- cx: 157.359
- cy: 125.254
- k1: -0.346145
- k2: 1.96797
- k3: -2.08042
- p1: -0.003468
- p2: 0.000668
- fps: 15
- color order: BGR (ignored if grayscale)

ORB Extractor Parameters: 
- Number of Features: 1200
- Scale Levels: 8
- Scale Factor: 1.2
- Initial Fast Threshold: 20
- Minimum Fast Threshold: 7

Local Mapping RELEASE
System Reseting
GLib-GIO-Message: 16:36:20.806: Using the 'memory' GSettings backend.  Your settings will not be saved or shared with other applications.
Reseting Local Mapper... done
Reseting Loop Closing... done
Reseting Database... done
Local Mapping STOP
Mappoints loaded: 2003
Keyframes loaded: 29
Loading features from myFirstMap.features ...
Features loaded: 29298
Rebuilding map:
Processing 29 keyframes 
pKF: 0x55ab9ce7de60
pKF->mnId: 1
pKF->mbNotErase: 0
Segmentation fault (core dumped)
AlejandroSilvestri commented 5 years ago

Nice! (sorry) The problem happens when computing Bag of Words. I'll dig in. Thank you for the info.

mmajewsk commented 5 years ago

@AlejandroSilvestri that is my thought as well, this line:

https://github.com/AlejandroSilvestri/osmap/blob/11edeba00e78821878eeed4daff57031bc41a238/src/Osmap.cpp#L518

Is doing something funny.

mmajewsk commented 5 years ago

actually, it seems that KeyFrames are reconstructed improperly, and in https://github.com/raulmur/ORB_SLAM2/blob/master/src/KeyFrame.cc#L66 the arguments for the call of transform are somehow eiher not read properly, or empty.

mmajewsk commented 5 years ago

@AlejandroSilvestri

LOGV(pKF->mpORBvocabulary->size()); 

before line 518 referenced early yields seg fault. This probably means that mpORBvocabulary is not initialised properly.

AlejandroSilvestri commented 5 years ago

@mmajewsk , @Alexandre34

I believe mmajewsk is right. It seems the problem is you are loading a map before mpTracker->mCurrentFrame initialization. I'll have to be explicit about this on readme. You must initialize this before loading the map.

You have two ways to initialize mpTracker->mCurrentFrame:

1- Call SLAM.TrackMonocular (or stereo or rgbd, whatever you want). 2- Initialize it yourself, as shown here. It can be complex if you don't have access to mpTracker and all arguments Frame constructor needs.

mpTracker->mState NO_IMAGES_YET means mCurrentFrame is not initialized.

I have to add this check to osmap, and may be a quick initialization if needed.

Please tell me if this works.

Alexandre34 commented 5 years ago

Hi @AlejandroSilvestri and @mmajewsk Thank you for all your help. I have modified the loading map code so that it first calls TrackMonocular before launching mapLoad. This time, the mapLoad function works and the map is being displayed, but only for a fraction of second. When it tries to to load again the images in the main loop, the program crashes (segmentation fault). Here is my modified file.

int main(int argc, char **argv)
{
    if(argc != 4)
    {
        cerr << endl << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_sequence" << endl;
        return 1;
    }

    // Retrieve paths to images
    vector<string> vstrImageFilenames;
    vector<double> vTimestamps;
    string strFile = string(argv[3])+"/rgb.txt";
    LoadImages(strFile, vstrImageFilenames, vTimestamps);

    int nImages = vstrImageFilenames.size();

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

    // Construct the osmap object, can be right after SLAM construction
    ORB_SLAM2::Osmap osmap = ORB_SLAM2::Osmap(SLAM);

    // Activate verbose for debugging purposes
    osmap.verbose = true;

    // Load only first image to initialize TrackMonocular
    cv::Mat im;
    im = cv::imread(string(argv[3])+"/"+vstrImageFilenames[0],CV_LOAD_IMAGE_UNCHANGED);
    double tframe = vTimestamps[0];

    SLAM.TrackMonocular(im,tframe);

    // Loading the map
    osmap.mapLoad("myFirstMap.yaml", false);

    // Vector for tracking time statistics
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);    

    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;
    cout << "Images in the sequence: " << nImages << endl << endl;

    // Main loop
    //cv::Mat im;

    for(int ni=1; ni<nImages; ni++)
    {
        // Read image from file
        im = cv::imread(string(argv[3])+"/"+vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];

        if(im.empty())
        {
            cerr << endl << "Failed to load image at: "
                 << string(argv[3]) << "/" << vstrImageFilenames[ni] << endl;
            return 1;
        }

    #ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    #else
        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
    #endif

        cout << "Before TrackMonocular" << endl;        

        // Pass the image to the SLAM system
        SLAM.TrackMonocular(im,tframe);
        cout << "After TrackMonocular" << endl;
    #ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    #else
        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
    #endif

        double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

        vTimesTrack[ni]=ttrack;

        // Wait to load the next frame
        double T=0;
        if(ni<nImages-1)
            T = vTimestamps[ni+1]-tframe;
        else if(ni>0)
            T = tframe-vTimestamps[ni-1];

        if(ttrack<T)
            usleep((T-ttrack)*1e6);
    }

    // Stop all threads
    SLAM.Shutdown();

    // Tracking time statistics
    sort(vTimesTrack.begin(),vTimesTrack.end());
    float totaltime = 0;
    for(int ni=0; ni<nImages; ni++)
    {
        totaltime+=vTimesTrack[ni];
    }
    cout << "-------" << endl << endl;
    cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
    cout << "mean tracking time: " << totaltime/nImages << endl;

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

    // save the pointcloud
    SLAM.CreatePCD("pointcloud.pcd");

    // When you already has a map to save
    //osmap.mapSave("myFirstMap");

    //view the pointcloud
    //SLAM.ViewCloud("Pointcloud.pcd");

    return 0;
}

From what I can understand, it crashes when trying to load TrackMonocular for the first time in the loop.

pMP: 0x1198f5e0
pMP->mnId: 82089
pMP: 0x1198fb70
pMP->mnId: 82090
pMP: 0x11990100
pMP->mnId: 82092
pMP: 0x11990690
pMP->mnId: 82094
pMP: 0x11990c20
pMP->mnId: 82097

-------
Start processing sequence ...
Images in the sequence: 1782

Before TrackMonocular
Segmentation fault (core dumped)

I've also tried to add the mapLoad function at the end of the main loop (making sure that it is only executed only once), but with the same result. What I'm trying to use your program for is that after loading the map from a previous recording, pictures are being sent to the system and by using the localization mode of ORB-SLAM2, it should be able to localize itself in the saved map. So I need to load pictures into the system after the map is loaded (I don't know if this is possible with your version of the program).

Thanks

Alexandre34 commented 5 years ago

Follow up on the previous comment.

It seems it crashes here : https://github.com/raulmur/ORB_SLAM2/blob/f2e6f51cdc8d067655d90a78c06261378e07e8f3/src/System.cc#L260

Then, in Tracking.cc, it crashes here : https://github.com/raulmur/ORB_SLAM2/blob/f2e6f51cdc8d067655d90a78c06261378e07e8f3/src/Tracking.cc#L262

So I don't think that calling TrackMonocular before solves completely the issue. It seems that it is the same mCurrentFrame initialization issue that you have raised.

mmajewsk commented 5 years ago

@Alexandre34 Bro.

osmap.mapLoad("myFirstMap.yaml", false, false);

My problem continued the same way as yours did, until I used those arguments. I don't even exactly know what was the problem, i just tried to setting it this way.

P.S. My code just loads the map after the first loop

Alexandre34 commented 5 years ago

Yes that works ! Thank you very much ! Seems that setting pauseThreads to false solves the issue. I think this has already been discussed in #4 and I know I tried it before when I had a similar problem but it didn't do anything at the time. @AlejandroSilvestri Maybe you can update the readme?

Thanks

mmajewsk commented 5 years ago

@Alexandre34 Altough there seems to be some other issue that gives segfault on some of my datasets, that contain similar environments.

Alexandre34 commented 5 years ago

@mmajewsk So I've been experimenting with other datasets, and indeed I have the same problems as you. The other datasets come from the same camera (same calibration and approximately the same number of frames). I've been able to track the segmentation fault to the function Tracking::Track() at the very end.

// Store frame pose information to retrieve the complete camera trajectory afterwards.
    cout << "Track" << endl;
    if(!mCurrentFrame.mTcw.empty())
    {
        cout << "if" << endl;
        cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
        mlRelativeFramePoses.push_back(Tcr);
        mlpReferences.push_back(mpReferenceKF);
        mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
        mlbLost.push_back(mState==LOST);
    }
    else
    {
        cout << "else" << endl;
        // This can happen if tracking is lost
        mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
        mlpReferences.push_back(mlpReferences.back());
        mlFrameTimes.push_back(mlFrameTimes.back());
        mlbLost.push_back(mState==LOST);
    }

With the good dataset, the program goes into the if statement and everything is alright. With the others however it goes in the else statement and it crashes here. mlRelativeFramePoses.push_back(mlRelativeFramePoses.back()); It also crashes at the very first frame.

Seems like in some datasets the system gets lost in the very first frame.

mmajewsk commented 5 years ago

@Alexandre34 Yup, I confirm this. Im bypassing this error by skipping inital frames, until the ones that have chance to recognise the position in regards of the loaded map. What is weird is that if it recognises the track, and then loses it, it does not give you seg fault. I think this issue should remain open, untill this is solved.

AlejandroSilvestri commented 5 years ago

@Alexandre34 , @mmajewsk

Thank you for your help and interest. This debug is more than enlightening. I myself don't generate trajectory, I whipped out those code lines from my orb-slam2 modified version called os1, and that's why I wasn't able to reproduce the error. I'm sorry I didn't see that.

As @mmajewsk said, the cause of the error is in mlRelativeFramePoses.back(). At this point, if you opened the map before generating some minimum map, mlRelativeFramePoses is empty, and there is no .back() element to retrieve. Same happens on the next two lines.

So, you can safely delete 8 lines from the else clause, and it won't register trajectory while it's lost. Or, more elegantly, after loading you may add one dummy element to each list to avoid the seg fault error, like this:

mlRelativeFramePoses.push_back(cv::Mat::eye(4,4,CV_32F));
mlpReferences.push_back(static_cast<KeyFrame*>(NULL));
mlFrameTimes.push_back(0.0);
mlbLost.push_back(true);

You can access these lists from System SLAM like SLAM.mpTracker.mlRelativeFramePoses .

I hope it works. If you can confirm it, I'll put this patch on readme.

Keep in mind that the map file doesn't contain the trajectory originally traveled. If you need to invent a trajectory over a loaded map, you can travel the spanning tree and push_back keyframes' poses, from the last keyframe iterating over parents until the first keyframe, the one without parent.

Alexandre34 commented 5 years ago

@AlejandroSilvestri Thanks for your quick response, I can confirm both these methods work with all my datasets. Because the first one modifies the ORB-SLAM2 source, I suggest putting the second one in the readme. I added these lines to the main, after loading the map and before the loop.

// Adding empty elements to lists to avoid segmentation fault when tracking is lost at the beginning of the loading
SLAM.mpTracker->mlRelativeFramePoses.push_back(cv::Mat::eye(4,4,CV_32F));
SLAM.mpTracker->mlpReferences.push_back(NULL);
SLAM.mpTracker->mlFrameTimes.push_back(0.0);
SLAM.mpTracker->mlbLost.push_back(true);

The first frames are slightly not in the original trajectory, which means that the system says "Track lost. Trying to relocalize." then after moving the camera a bit the system is able to find itself in the map. I will continue to try with other datasets, especially in cases when it can't localize itself at all.

AlejandroSilvestri commented 5 years ago

@Alexandre34, I'm glad to hear that, thank you for your feedback. I believe this issue is resolved.

I'm adding instructions to readme.

Those empty elements we added put a point in the trajectory in the very origin of the map reference system. After relocalization trajectory moves to the real position. In your case the origin is slightly not in the trajectory, but it can be worse in other maps.

When the system initializes the map, the localization of the first keyframe is designed as the origin of the map reference system, but this keyframe's localization can change with a loop closure, leaving the origin in a meaningless place.

AlejandroSilvestri commented 5 years ago

I added the code to osmap.mapLoad.

lakshay-narula commented 5 years ago

Hello @AlejandroSilvestri. Thanks for the bug fix. I think you meant to add the above code snippet after the if(pauseThreads) block. As it stands, if pauseThreads is true, the tracker is reset and the dummy values you added are lost, again causing a segmentation fault.

AlejandroSilvestri commented 5 years ago

Done. You are right! Thank you.