map.h

浏览该文件的文档.

 
#pragma once
 
#include "camera.h"
#include "common_include.h"
#include "config.h"
 
NAMESPACE_BEGIN
struct MapPoint;
 
struct Feature {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef std::shared_ptr<Feature> Ptr;
 
    static Ptr create() {
        Ptr feature(new Feature);
        return feature;
    }
 
    std::weak_ptr<MapPoint> m_mapPoint; 
    Vec2d m_position;                   
};
 
struct MapPoint {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef std::shared_ptr<MapPoint> Ptr;
 
    static Ptr create() {
        static std::size_t pointNum = 0;
        Ptr point(new MapPoint);
        point->m_id = pointNum++;
        return point;
    }
 
    bool m_isOutlier = false; 
    int m_featureNum = 0;     
    std::size_t m_id;         
    Vec3d m_position;         
};
 
class Frame {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef std::shared_ptr<Frame> Ptr;
 
    Frame(cv::Mat leftIMG, cv::Mat rightIMG)
        : m_leftIMG(std::move(leftIMG))
        , m_rightIMG(std::move(rightIMG)) {
        static std::size_t frameNum = 0;
        m_id = frameNum++;
    }
 
    std::vector<Feature::Ptr> &getLeftFeatures() { return m_leftFeatures; }
    std::vector<Feature::Ptr> &getRightFeatures() { return m_rightFeatures; }
 
    int initKeyFrame(const Camera::Ptr &camera);
 
    static double distance(const Frame &left, const Frame &right);
 
    SE3d &pose() { return m_pose; }
    const SE3d &pose() const { return m_pose; }
 
private:
    std::vector<Feature::Ptr> m_leftFeatures;  
    std::vector<Feature::Ptr> m_rightFeatures; 
    SE3d m_pose;                               
 
public:
    std::size_t m_keyID = -1;  
    std::size_t m_id;          
    cv::Mat m_leftIMG;         
    cv::Mat m_rightIMG;        
    bool m_isKeyFrame = false; 
};
 
class Map {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    typedef std::shared_ptr<Map> Ptr;
    typedef std::unordered_map<std::size_t, Frame::Ptr> FramesType;
    typedef std::unordered_map<std::size_t, MapPoint::Ptr> PointsType;
 
    Map(const Map &other) = delete;
    Map &operator=(const Map &other) = delete;
 
    void insertMapPoints(const std::vector<MapPoint::Ptr> &mapPoints) {
        for (auto &point : mapPoints)
            m_points.insert(std::make_pair(point->m_id, point));
    }
 
    void insertKeyFrame(const Frame::Ptr &keyFrame);
 
    static Ptr getInstance() {
        static Ptr map(new Map);
        return map;
    }
 
    FramesType &getActiveFrames() { return m_activeFrames; }
 
    PointsType &getActivePoints() { return m_activePoints; }
 
    FramesType &getAllFrames() { return m_frames; }
 
    PointsType &getAllPoints() { return m_points; }
 
    void removeOldFrame(const Frame::Ptr &keyFrame);
 
    void cleanMap();
 
private:
    Map() = default;
    FramesType m_frames;       
    PointsType m_points;       
    FramesType m_activeFrames; 
    PointsType m_activePoints; 
 
public:
    std::mutex m_mutex;             
    std::condition_variable m_cond; 
    bool m_isReady = false;         
};
 
NAMESPACE_END