How can I get the world positions of map points in System.cc?

[rakshith95]

I want to save the Keyframes, poses, keypoints, and corresponding 3D map points positions from System.cc at the end of my mapping. I do the following:

    std::vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);

    std::cout<<"\n"<<vpKFs.size()<<"\n";
    // std::vector<MapPoint*> mps = mpMap->GetAllMapPoints();

    for(size_t i=0; i<vpKFs.size(); i++)
        {
        // long unsigned int kf_id = vpKFs[i]->mnId;
        
        std::vector<MapPoint*> threeD_mappoints = vpKFs[i]->GetMapPointMatches();
        
        const std::vector<cv::KeyPoint> twoD_keypoints = vpKFs[i]->mvKeysUn;

        for(size_t j=0; j<threeD_mappoints.size(); j++)
            cv::Mat worldPos = threeD_mappoints[j]->GetWorldPos();

However, the program crashes when it runs this part of the code; I assume this is due to some mutex locking, but I don't have any experience with it. Could someone tell me why this would cause a problem, or how to circumvent it?

[thebirdgr]

in ros_rgbd.cc i get World to camera pose from cv::Mat T_, R_, t_ ;

T_ = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());