mapdetector.cpp

#include "mapdetector.h"

namespace juefx{

MapDetector::Ptr mapDetectorInstance;
MapDetector::MapDetector()
{

}

void MapDetector::Reset()
{
    mapDetectorInstance.reset();
}

MapDetector::Ptr MapDetector::Instance()
{
    if(!mapDetectorInstance){
        mapDetectorInstance.reset(new MapDetector());
    }
    return mapDetectorInstance;
}

void MapDetector::HandleMapFolder(const string &mapFolder)
{
    vector<string> mapDirs = GetMapDirs(mapFolder);
    ConfigMaps(mapDirs);
}

vector<string> MapDetector::GetCloseMap(const Vector3d &blh, float distance)
{
    vector<string> ret;
    Isometry3d unuse;
    for(auto poseGuesser : poseGuesserVec_){
        float dis = poseGuesser->GetMapPoseByBlh(blh, unuse);
        if(dis < distance){
            ret.emplace_back(poseGuesser->GetMapName());
        }
    }
    return ret;
}

vector<string> MapDetector::GetMapDirs(const string &mapFolder)
{
    vector<string> ret;
    if(is_directory(mapFolder)){
        directory_iterator dir_iter(mapFolder);
        directory_iterator end_iter;
        for(; dir_iter != end_iter; dir_iter++) {
            string taskDir = dir_iter->path().string();
            if(!is_directory(taskDir)){
                continue;
            }
            string baseDir = taskDir + "/voxels/";
            if(!is_directory(baseDir)){
                continue;
            }
            ret.push_back(baseDir);
        }
    }
    return ret;
}

void MapDetector::ConfigMaps(const vector<string> &mapDirs)
{
    for(const string& mapDir : mapDirs){
        string trajPath = mapDir + "/traj.txt";
        string gpsTrajPath = mapDir + "/gps_traj.txt";
        if(!exists(trajPath)){
            continue;
        }
        if(!exists(gpsTrajPath)){
            continue;
        }
        PoseGuesser::Ptr poseGuesser(new PoseGuesser);
        poseGuesser->SetMapName(mapDir);
        if(poseGuesser->LoadSlamTraj(trajPath, gpsTrajPath)){
            poseGuesserVec_.emplace_back(poseGuesser);
        }
    }
}


} // end of namespace