WAIMapStorage.cpp File Reference

#include <WAIMapStorage.h>
#include <Profiler.h>

Include dependency graph for WAIMapStorage.cpp:

Go to the source code of this file.

Functions
void	buildMatching (std::vector< WAIKeyFrame * > &kfs, std::map< WAIKeyFrame *, std::map< size_t, size_t >> &KFmatching)
void	saveKeyFrames (std::vector< WAIKeyFrame * > &kfs, std::map< WAIKeyFrame *, std::map< size_t, size_t >> &KFmatching, cv::FileStorage &fs, std::string imgDir, bool saveBOW)
void	saveMapPoints (std::vector< WAIMapPoint * > mpts, std::map< WAIKeyFrame *, std::map< size_t, size_t >> &KFmatching, cv::FileStorage &fs)

Function Documentation

buildMatching()

void buildMatching	(	std::vector< WAIKeyFrame * > &	kfs,
		std::map< WAIKeyFrame *, std::map< size_t, size_t >> &	KFmatching
	)

Definition at line 54 of file WAIMapStorage.cpp.

{
    for (int i = 0; i < kfs.size(); ++i)
    {
        WAIKeyFrame* kf = kfs[i];
        if (kf->isBad())
            continue;
        // if (kf->mBowVec.data.empty())
        //     continue;
 
        std::vector<WAIMapPoint*> mps = kf->GetMapPointMatches();
        std::map<size_t, size_t>  matching;
 
        size_t id = 0;
        for (int j = 0; j < mps.size(); j++)
        {
            if (mps[j] != nullptr)
            {
                matching.insert(std::pair<size_t, size_t>(j, id));
                id++;
            }
        }
        KFmatching.insert(std::pair<WAIKeyFrame*, std::map<size_t, size_t>>(kf, matching));
    }
}

saveKeyFrames()

void saveKeyFrames	(	std::vector< WAIKeyFrame * > &	kfs,
		std::map< WAIKeyFrame *, std::map< size_t, size_t >> &	KFmatching,
		cv::FileStorage &	fs,
		std::string	imgDir,
		bool	saveBOW
	)

Definition at line 81 of file WAIMapStorage.cpp.

{
    // start sequence keyframes
    fs << "KeyFrames"
       << "[";
    for (int i = 0; i < kfs.size(); ++i)
    {
        WAIKeyFrame* kf = kfs[i];
        if (kf->isBad())
            continue;
        if (kf->mBowVec.data.empty())
            continue;
 
        fs << "{";         // new map keyFrame
                           // add id
        fs << "id" << (int)kf->mnId;
        if (kf->mnId != 0) // kf with id 0 has no parent
            fs << "parentId" << (int)kf->GetParent()->mnId;
        else
            fs << "parentId" << -1;
 
        // loop edges: we store the id of the connected kf
        auto loopEdges = kf->GetLoopEdges();
        if (loopEdges.size())
        {
            std::vector<int> loopEdgeIds;
            for (auto loopEdgeKf : loopEdges)
            {
                loopEdgeIds.push_back(loopEdgeKf->mnId);
            }
            fs << "loopEdges" << loopEdgeIds;
        }
 
        // world w.r.t camera
        fs << "Tcw" << kf->GetPose();
 
        if (KFmatching.size() > 0)
        {
            cv::Mat                         descriptors;
            const std::map<size_t, size_t>& matching = KFmatching[kf];
            descriptors.create((int)matching.size(), 32, CV_8U);
            std::vector<cv::KeyPoint> keypoints(matching.size());
            for (int j = 0; j < kf->mvKeysUn.size(); j++)
            {
                auto it = matching.find(j);
                if (it != matching.end())
                {
                    kf->mDescriptors.row(j).copyTo(descriptors.row(it->second));
                    keypoints[it->second] = kf->mvKeysUn[j];
                }
            }
            fs << "featureDescriptors" << descriptors;
            fs << "keyPtsUndist" << keypoints;
        }
        else
        {
            fs << "featureDescriptors" << kf->mDescriptors;
            fs << "keyPtsUndist" << kf->mvKeysUn;
        }
 
        if (saveBOW)
        {
            WAIBowVector&      bowVec = kf->mBowVec;
            std::vector<int>   wordsId;
            std::vector<float> tfIdf;
            for (auto it = bowVec.getWordScoreMapping().begin(); it != bowVec.getWordScoreMapping().end(); it++)
            {
                wordsId.push_back(it->first);
                tfIdf.push_back(it->second);
            }
 
            fs << "BowVectorWordsId" << wordsId;
            fs << "TfIdf" << tfIdf;
        }
 
        // scale factor
        fs << "scaleFactor" << kf->mfScaleFactor;
        // number of pyriamid scale levels
        fs << "nScaleLevels" << kf->mnScaleLevels;
        fs << "K" << kf->mK;
 
        fs << "nMinX" << kf->mnMinX;
        fs << "nMinY" << kf->mnMinY;
        fs << "nMaxX" << kf->mnMaxX;
        fs << "nMaxY" << kf->mnMaxY;
 
#if 0
        std::vector<int>          bestCovisibleKeyFrameIds;
        std::vector<int>          bestCovisibleWeights;
        std::vector<WAIKeyFrame*> bestCovisibles = kf->GetBestCovisibilityKeyFrames(20);
        for (WAIKeyFrame* covisible : bestCovisibles)
        {
            if (covisible->isBad())
                continue;
            int weight = kf->GetWeight(covisible);
            if (weight)
            {
                bestCovisibleKeyFrameIds.push_back(covisible->mnId);
                bestCovisibleWeights.push_back(weight);
            }
        }
 
        fs << "bestCovisibleKeyFrameIds" << bestCovisibleKeyFrameIds;
        fs << "bestCovisibleWeights" << bestCovisibleWeights;
#endif
 
        fs << "}"; // close map
 
        // save the original frame image for this keyframe
        if (imgDir != "")
        {
            cv::Mat imgColor;
            if (!kf->imgGray.empty())
            {
                std::stringstream ss;
                ss << imgDir << "kf" << (int)kf->mnId << ".jpg";
 
                cv::cvtColor(kf->imgGray, imgColor, cv::COLOR_GRAY2BGR);
                cv::imwrite(ss.str(), imgColor);
 
                // if this kf was never loaded, we still have to set the texture path
                kf->setTexturePath(ss.str());
            }
        }
    }
    fs << "]"; // close sequence keyframes
}

saveMapPoints()

void saveMapPoints	(	std::vector< WAIMapPoint * >	mpts,
		std::map< WAIKeyFrame *, std::map< size_t, size_t >> &	KFmatching,
		cv::FileStorage &	fs
	)

Definition at line 213 of file WAIMapStorage.cpp.

{
    // start map points sequence
    fs << "MapPoints"
       << "[";
    for (int i = 0; i < mpts.size(); ++i)
    {
        WAIMapPoint* mpt = mpts[i];
        // TODO: ghm1: check if it is necessary to removed points that have no reference keyframe OR can we somehow update the reference keyframe in the SLAM
        if (mpt->isBad() || mpt->refKf()->isBad())
            continue;
 
        fs << "{"; // new map for MapPoint
                   // add id
        fs << "id" << (int)mpt->mnId;
        // add position
        fs << "mWorldPos" << mpt->GetWorldPos();
        // save keyframe observations
        auto        observations = mpt->GetObservations();
        vector<int> observingKfIds;
        vector<int> corrKpIndices; // corresponding keypoint indices in observing keyframe
 
        if (!KFmatching.empty())
        {
            for (auto it : observations)
            {
                WAIKeyFrame* kf    = it.first;
                size_t       kpIdx = it.second;
                if (!kf || kf->isBad() || kf->mBowVec.data.empty())
                    continue;
 
                if (KFmatching.find(kf) == KFmatching.end())
                {
                    std::cout << "observation not found in kfmatching" << std::endl;
                    continue;
                }
 
                const std::map<size_t, size_t>& matching = KFmatching[kf];
                auto                            mit      = matching.find(kpIdx);
                if (mit != matching.end())
                {
                    observingKfIds.push_back(kf->mnId);
                    corrKpIndices.push_back(mit->second);
                }
            }
        }
        else
        {
            for (auto it : observations)
            {
                if (!it.first->isBad())
                {
                    observingKfIds.push_back(it.first->mnId);
                    corrKpIndices.push_back(it.second);
                }
            }
        }
 
        fs << "observingKfIds" << observingKfIds;
        fs << "corrKpIndices" << corrKpIndices;
 
        fs << "mfMaxDistance" << mpt->GetMaxDistance();
        fs << "mfMinDistance" << mpt->GetMinDistance();
        fs << "mNormalVector" << mpt->GetNormal();
        fs << "mDescriptor" << mpt->GetDescriptor();
 
        fs << "refKfId" << (int)mpt->refKf()->mnId;
        fs << "}"; // close map
    }
    fs << "]";
}