视觉SLAM笔记（48） 局部地图

    #ifndef MAPPOINT_H
    #define MAPPOINT_H
    
    #include "myslam/common_include.h"
    
    namespace myslam
    {
    class Frame;
    class MapPoint
    {
    public:
        typedef shared_ptr<MapPoint> Ptr;
        unsigned long      id_;           // ID
        static unsigned long factory_id_;    
        bool        good_;                // 是否是好点
        Vector3d    pos_;                 // 世界坐标系上的坐标
        Vector3d    norm_;                // 观察方向法线
        Mat         descriptor_;          // 描述符匹配
    
        list<Frame*>    observed_frames_; // 观察时间
    
        int         matched_times_;       // 匹配时间
        int         visible_times_;       // 一帧时间
    
        MapPoint();
        MapPoint(
            unsigned long id,
            const Vector3d& position,
            const Vector3d& norm,
            Frame* frame = nullptr,
            const Mat& descriptor = Mat()
        );
    
        inline cv::Point3f getPositionCV() const {
            return cv::Point3f(pos_(0, 0), pos_(1, 0), pos_(2, 0));
        }
    
        // 建立MapPoint
        static MapPoint::Ptr createMapPoint();
        static MapPoint::Ptr createMapPoint(
            const Vector3d& pos_world,
            const Vector3d& norm_,
            const Mat& descriptor,
            Frame* frame);
    };
    }
    
    #endif // MAPPOINT_H
    
    
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
    
    AI助手

    void VisualOdometry::addKeyFrame()
    {
    if (map_->keyframes_.empty())
    {
        // 第一个关键帧，添加所有3d点到地图
        for (size_t i = 0; i < keypoints_curr_.size(); i++)
        {
            double d = curr_->findDepth(keypoints_curr_[i]);
            if (d < 0)
                continue;
            Vector3d p_world = ref_->camera_->pixel2world(
                Vector2d(keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y), curr_->T_c_w_, d
            );
            Vector3d n = p_world - ref_->getCamCenter();
            n.normalize();
            MapPoint::Ptr map_point = MapPoint::createMapPoint(
                p_world, n, descriptors_curr_.row(i).clone(), curr_.get()
            );
            map_->insertMapPoint(map_point);
        }
    }
    
    map_->insertKeyFrame(curr_);
    ref_ = curr_;
    }
    
    
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
    
    AI助手

    void VisualOdometry::optimizeMap()
    {
    // 除去几乎看不见的和不可见的点
    for (auto iter = map_->map_points_.begin(); iter != map_->map_points_.end(); )
    {
        if (!curr_->isInFrame(iter->second->pos_))
        {
            iter = map_->map_points_.erase(iter);
            continue;
        }
        float match_ratio = float(iter->second->matched_times_) / iter->second->visible_times_;
        if (match_ratio < map_point_erase_ratio_)
        {
            iter = map_->map_points_.erase(iter);
            continue;
        }
    
        double angle = getViewAngle(curr_, iter->second);
        if (angle > M_PI / 6.)
        {
            iter = map_->map_points_.erase(iter);
            continue;
        }
        if (iter->second->good_ == false)
        {
            // 试着对这个地图点进行三角测量
        }
        iter++;
    }
    
    if (match_2dkp_index_.size() < 100)
        addMapPoints();
    if (map_->map_points_.size() > 1000)
    {
        // 地图太大，删除一些
        map_point_erase_ratio_ += 0.05;
    }
    else
        map_point_erase_ratio_ = 0.1;
    cout << "map points: " << map_->map_points_.size() << endl;
    }
    
    
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
    
    AI助手

    void VisualOdometry::featureMatching()
    {
    boost::timer timer;
    vector<cv::DMatch> matches;
    // 在map中选择候选项
    Mat desp_map;
    vector<MapPoint::Ptr> candidate;
    for (auto& allpoints : map_->map_points_)
    {
        MapPoint::Ptr& p = allpoints.second;
        // 检查p是否在当前帧图像中
        if (curr_->isInFrame(p->pos_))
        {
            // 添加到候选
            p->visible_times_++;
            candidate.push_back(p);
            desp_map.push_back(p->descriptor_);
        }
    }
    
    matcher_flann_.match(desp_map, descriptors_curr_, matches);
    // 选择最佳匹配
    float min_dis = std::min_element(
        matches.begin(), matches.end(),
        [](const cv::DMatch& m1, const cv::DMatch& m2)
    {
        return m1.distance < m2.distance;
    })->distance;
    
    match_3dpts_.clear();
    match_2dkp_index_.clear();
    for (cv::DMatch& m : matches)
    {
        if (m.distance < max<float>(min_dis*match_ratio_, 30.0))
        {
            match_3dpts_.push_back(candidate[m.queryIdx]);
            match_2dkp_index_.push_back(m.trainIdx);
        }
    }
    cout << "good matches: " << match_3dpts_.size() << endl;
    cout << "match cost time: " << timer.elapsed() << endl;
    }
    
    
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
    
    AI助手