frontend.h

浏览该文件的文档.

 
#pragma once
#include "camera.h"
#include "common_include.h"
#include "config.h"
#include "dataset.h"
#include "g2o_types.h"
#include "map.h"
#include "viewer.h"
 
NAMESPACE_BEGIN
 
enum class TrackStatus { INIT_TRACKING, TRACK_GOOD, TRACK_BAD, TRACK_LOST, TRACK_ERROR };
void printPose(const SE3d &pose);
 
class Frontend {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef std::shared_ptr<Frontend> Ptr;
    Frontend(const Frontend &) = delete;
    Frontend &operator=(const Frontend &) = delete;
 
    static Ptr getInstance() {
        static Ptr frontend(new Frontend);
        return frontend;
    }
 
    void run();
 
    void addFrame(const Frame::Ptr &frame);
 
    bool triangulate(const std::vector<SE3d> &poses, const std::vector<Vec2d> &pixelPoints,
                     Vec3d &worldPoint);
 
    int triNewPoint(Frame::Ptr &frame, std::vector<MapPoint::Ptr> &newPoints);
 
    void setCamera(const Camera::Ptr &camera) { m_camera = camera; }
 
    void setMap(const Map::Ptr &map) { m_map = map; }
 
    void setDataset(const Dataset::Ptr &dataset) { m_dataset = dataset; }
 
    void setViewer(const Viewer::Ptr &viewer) { m_viewer = viewer; }
 
    int trackCurrFrame();
 
    void initTrack();
 
    int estimatePose();
 
    int detectOutlier(std::vector<Feature::Ptr> &features, std::vector<PoseEdge *> &edges,
                      g2o::SparseOptimizer &optimizer, PoseVertex *vertex);
 
    void track() {
        int trackNum = trackCurrFrame();
        SPDLOG_INFO("==================================================================");
        SPDLOG_INFO("参考帧: {:06}\t当前帧: {:06} 追踪成功的特征点数目为: {:03}", m_refFrame->m_id,
                    m_currFrame->m_id, trackNum);
        int inlinerNum = estimatePose();
 
        m_diffPose = m_currFrame->pose() * m_refFrame->pose().inverse();
        SPDLOG_INFO("对当前帧: {:06} 进行位姿估计 非异常点数目为: {:03} 当前帧位姿为:",
                    m_currFrame->m_id, inlinerNum);
        printPose(m_currFrame->pose());
        if (inlinerNum < Config::MIN_MAPPOINTS_NUM_FOR_TRACK) {
            initTrack();
        }
        SPDLOG_INFO("==================================================================\n");
    }
 
private:
    Frontend() = default;
    Frame::Ptr m_refFrame;  
    Frame::Ptr m_currFrame; 
    bool m_isStop = false;  
    Camera::Ptr m_camera;   
    Dataset::Ptr m_dataset; 
    Map::Ptr m_map;         
    SE3d m_diffPose;        
    Viewer::Ptr m_viewer;   
    TrackStatus m_status = TrackStatus::INIT_TRACKING; 
};
 
NAMESPACE_END