ORBSLAM2地图保存

#include <fstream>
#include "Converter.h"

public: 
   void Save(const string &filename);
    void SaveMapPoint(ofstream& f, MapPoint* mp);
    void SaveKeyFrame( ofstream& f, KeyFrame* kf );
    void GetMapPointsIdx();
protected:
    std::map<MapPoint*,unsigned long int> mmpnMapPointsIdx;

static cv::Mat toCvMat(const std::vector<float>& v);

 void SaveMap(const string &filename); 

void Map::Save ( const string& filename )
{
    //Print the information of the saving map
    cerr<<"Map.cc :: Map Saving to "<<filename <<endl;
    ofstream f;
    f.open(filename.c_str(), ios_base::out|ios::binary);

    //Number of MapPoints
    unsigned long int nMapPoints = mspMapPoints.size();
    f.write((char*)&nMapPoints, sizeof(nMapPoints) );
    //Save MapPoint sequentially
    for ( auto mp: mspMapPoints ){
        //Save MapPoint
        SaveMapPoint( f, mp );
        // cerr << "Map.cc :: Saving map point number: " << mp->mnId << endl;
    }

    //Print The number of MapPoints
    cerr << "Map.cc :: The number of MapPoints is :"<<mspMapPoints.size()<<endl;
        

    //Grab the index of each MapPoint, count from 0, in which we initialized mmpnMapPointsIdx  
    GetMapPointsIdx(); 

    //Print the number of KeyFrames
    cerr <<"Map.cc :: The number of KeyFrames:"<<mspKeyFrames.size()<<endl;
    //Number of KeyFrames
    unsigned long int nKeyFrames = mspKeyFrames.size();
    f.write((char*)&nKeyFrames, sizeof(nKeyFrames));

    //Save KeyFrames sequentially
    for ( auto kf: mspKeyFrames )
        SaveKeyFrame( f, kf );

    for (auto kf:mspKeyFrames )
    {
        //Get parent of current KeyFrame and save the ID of this parent
        KeyFrame* parent = kf->GetParent();
        unsigned long int parent_id = ULONG_MAX;
        if ( parent )
            parent_id = parent->mnId;
        f.write((char*)&parent_id, sizeof(parent_id));

        //Get the size of the Connected KeyFrames of the current KeyFrames
        //and then save the ID and weight of the Connected KeyFrames
        unsigned long int nb_con = kf->GetConnectedKeyFrames().size();
        f.write((char*)&nb_con, sizeof(nb_con));
        for ( auto ckf: kf->GetConnectedKeyFrames())
        {
            int weight = kf->GetWeight(ckf);
            f.write((char*)&ckf->mnId, sizeof(ckf->mnId));
            f.write((char*)&weight, sizeof(weight));
        }
    }

    // Save last Frame ID
    // SaveFrameID(f);

    f.close();
    cerr<<"Map.cc :: Map Saving Finished!"<<endl;
}
void Map::SaveMapPoint( ofstream& f, MapPoint* mp)
{   
    //Save ID and the x,y,z coordinates of the current MapPoint
    f.write((char*)&mp->mnId, sizeof(mp->mnId));
    cv::Mat mpWorldPos = mp->GetWorldPos();
    f.write((char*)& mpWorldPos.at<float>(0),sizeof(float));
    f.write((char*)& mpWorldPos.at<float>(1),sizeof(float));
    f.write((char*)& mpWorldPos.at<float>(2),sizeof(float));
}

void Map::SaveKeyFrame( ofstream& f, KeyFrame* kf )
{
    //Save the ID and timesteps of current KeyFrame
    f.write((char*)&kf->mnId, sizeof(kf->mnId));
    // cout << "saving kf->mnId = " << kf->mnId <<endl;
    f.write((char*)&kf->mTimeStamp, sizeof(kf->mTimeStamp));
    //Save the Pose Matrix of current KeyFrame
    cv::Mat Tcw = kf->GetPose();

    ////Save the rotation matrix
    // for ( int i = 0; i < Tcw.rows; i ++ )
    // {
    //     for ( int j = 0; j < Tcw.cols; j ++ )
    //     {
    //         f.write((char*)&Tcw.at<float>(i,j), sizeof(float));
    //         //cerr<<"Tcw.at<float>("<<i<<","<<j<<"):"<<Tcw.at<float>(i,j)<<endl;
    //     }
    // }

    //Save the rotation matrix in Quaternion
    std::vector<float> Quat = Converter::toQuaternion(Tcw);
    for ( int i = 0; i < 4; i ++ )
        f.write((char*)&Quat[i],sizeof(float));
    //Save the translation matrix
    for ( int i = 0; i < 3; i ++ )
        f.write((char*)&Tcw.at<float>(i,3),sizeof(float));




    //Save the size of the ORB features current KeyFrame
    //cerr<<"kf->N:"<<kf->N<<endl;
    f.write((char*)&kf->N, sizeof(kf->N));
    //Save each ORB features
    for( int i = 0; i < kf->N; i ++ )
    {
        cv::KeyPoint kp = kf->mvKeys[i];
        f.write((char*)&kp.pt.x, sizeof(kp.pt.x));
        f.write((char*)&kp.pt.y, sizeof(kp.pt.y));
        f.write((char*)&kp.size, sizeof(kp.size));
        f.write((char*)&kp.angle,sizeof(kp.angle));
        f.write((char*)&kp.response, sizeof(kp.response));
        f.write((char*)&kp.octave, sizeof(kp.octave));

        //Save the Descriptors of current ORB features
        f.write((char*)&kf->mDescriptors.cols, sizeof(kf->mDescriptors.cols)); //kf->mDescriptors.cols is always 32 here.
        for (int j = 0; j < kf->mDescriptors.cols; j ++ )
            f.write((char*)&kf->mDescriptors.at<unsigned char>(i,j), sizeof(char));

        //Save the index of MapPoints that corresponds to current ORB features
        unsigned long int mnIdx;
        MapPoint* mp = kf->GetMapPoint(i);
        if (mp == NULL  )
            mnIdx = ULONG_MAX;
        else
            mnIdx = mmpnMapPointsIdx[mp];

        f.write((char*)&mnIdx, sizeof(mnIdx));
    }

    // Save BoW for relocalization.
    // f.write((char*)&kf->mBowVec, sizeof(kf->mBowVec));
}
void Map::GetMapPointsIdx()
{
    unique_lock<mutex> lock(mMutexMap);
    unsigned long int i = 0;
    for ( auto mp: mspMapPoints )
    {
        mmpnMapPointsIdx[mp] = i;
        i += 1;
    }
}

cv::Mat Converter::toCvMat(const std::vector<float>& v)
{
    Eigen::Quaterniond q;
    q.x() = v[0];
    q.y() = v[1];
    q.z() = v[2];
    q.w() = v[3];

    Eigen::Matrix<double,3,3> eigMat(q);
    cv::Mat M = toCvMat(eigMat);
    return M;
}

void System::SaveMap(const string &filename)
{
    mpMap->Save(filename);
}

SLAM.SaveMap("/home/ORB_SLAM2/Examples/Stereo/map.bin");
//注意地址要填自己的地址