【SLAM】视觉SLAM十四讲-设计前端v0.2

说在前面

• 书籍参考：《视觉SLAM十四讲》高翔著
• 书籍代码地址：Github
• 数据集：TUM
• 环境搭建：【SLAM】在WSL中搭建环境(Linux子系统)
• 其他说明：学习记录

代码理解

• VO

  • 代码仅包括前端的实现，并且使用的方法为两两帧的视觉里程计；
  • 大致流程
    1. 捕获第一帧，将该帧设置为关键帧以及参考帧，使用ORB方法提取该帧的关键点并计算描述子，将这些关键点通过相机内参计算出对应的相机坐标系中的3D坐标(由针孔相机模型，三角形相似，参见here)，同时记录每个关键点对应的描述子。
    2. 捕获下一帧，同样提取关键点、计算描述子，然后与参考帧进行特征匹配(这里使用的是暴力匹配方法)，根据匹配成功的点对使用PnP方法来估计相机姿态。
       (问题：PnP来估计时的3D坐标应该要是世界坐标系的坐标，但是代码中用的却是相机坐标系，不知道哪里出了问题…)
       (猜想：可能和书上$T_{cw}=T_{cr}{T}_{rw}$这个左乘关系有关；)
       (原因：将第一帧作为世界坐标系，假设第二帧的姿态是良好的，那么第二帧就是相对于第一帧的姿态，然后又将第二帧作为世界坐标系，通过第二三帧计算姿态，也就是相对于第二帧的姿态，那么第二个姿态$\cdot$第一个姿态也就是第三帧相对于第一帧的姿态。应该是这个意思，我还是太菜了/(ㄒoㄒ)/ )
    3. 判断当前计算的相机姿态是否符合预设的阈值；
       若符合，则当前帧的相机姿态=当前帧计算得到的相机姿态$\cdot$参考帧的相机姿态(即$T_{cw}=T_{cr}{T}_{rw}$)，并将当前帧作为参考帧计算关键点的3D坐标；然后还需判断当前帧是否为关键帧；
       若不符合，说明该帧丢失，统计丢失帧总数，当丢失帧数目达到预设阈值，算法结束。
    4. 返回第二步

• Code

  • Config类
    用于读取default.yml文件中的配置参数。实际使用的时候如果出问题（无法读取，出现段错误），建议改一下。
  • Camera类
    相机内参以及坐标系变换。由于用到了Config类，也注意一下。
  • Map类、Mappoint类
    这俩存在感暂时不高。
  • Frame类
    帧。
  • VisualOdometry类
    VO。最关键的一个类，主要流程在该类中实现。
    成员变量

        VOState     state_;     // 当前VO状态，包括INITIALIZING、OK、LOST
        Map::Ptr    map_;       // 记录所有帧以及特征点
        Frame::Ptr  ref_;       // 参考帧
        Frame::Ptr  curr_;      // 当前帧 
        
        cv::Ptr<cv::ORB> orb_;  // OpenCV提供的ORB检测
        vector<cv::Point3f>     pts_3d_ref_;        // 参考帧中的3D点
        vector<cv::KeyPoint>    keypoints_curr_;    // 当前帧中的关键点，ORB提取
        Mat                     descriptors_curr_;  // 当前帧的关键点对应的描述子
        Mat                     descriptors_ref_;   // 参考帧的描述子
        vector<cv::DMatch>      feature_matches_;   // 特征匹配对，存储特诊匹配的结果
        
        SE3 T_c_r_estimated_;  // 当前帧的姿态估计
        int num_inliers_;        // number of inlier features in icp
        int num_lost_;           // 丢失帧计数
        
        // parameters 
        int num_of_features_;   // ORB参数
        double scale_factor_;   // ORB参数
        int level_pyramid_;     // ORB参数
        float match_ratio_;      // 特征匹配使用的参数
        int max_num_lost_;      // 预设的丢失帧阈值
        int min_inliers_;       // 用于评估姿态估计
        
        double key_frame_min_rot;   // 用于判断是否为关键帧
        double key_frame_min_trans; // 用于判断是否为关键帧
    

    成员函数
    bool addFrame( Frame::Ptr frame );
    主要函数，实现了VO的基本流程；

    bool VisualOdometry::addFrame ( Frame::Ptr frame )
    {
        switch ( state_ )
        {
        //VO类在初始化的时候，状态设置为INITIALIZING，所以第一帧必定进入INITIALIZING分支
        case INITIALIZING:
        {
            state_ = OK;
            curr_ = ref_ = frame;
            map_->insertKeyFrame ( frame );
            // 提取当前帧的关键点
            extractKeyPoints();
            //计算当前帧的描述子
            computeDescriptors();
            // 计算关键点对应的3D坐标
            setRef3DPoints();
            break;
        }
        case OK:
        {
            curr_ = frame;
            extractKeyPoints();
            computeDescriptors();
            //与参考帧进行特征匹配
            featureMatching();
            //姿态估计
            poseEstimationPnP();
            if ( checkEstimatedPose() == true ) // a good estimation
            {
                curr_->T_c_w_ = T_c_r_estimated_ * ref_->T_c_w_;  // T_c_w = T_c_r*T_r_w 
                ref_ = curr_;
                setRef3DPoints();
                num_lost_ = 0;
                if ( checkKeyFrame() == true ) // is a key-frame
                {
                    addKeyFrame();
                }
            }
            else // bad estimation due to various reasons
            {
                num_lost_++;
                if ( num_lost_ > max_num_lost_ )
                {
                    state_ = LOST;
                }
                return false;
            }
            break;
        }
        case LOST:
        {
            cout<<"vo has lost."<<endl;
            break;
        }
        }
    
        return true;
    }
    

    void VisualOdometry::setRef3DPoints()
    计算参考帧中特征点的3d坐标；（可以看到用的是pixel2camera()这个函数，转换成了相机坐标系中的点？）

    void VisualOdometry::setRef3DPoints()
    {
        // select the features with depth measurements 
        pts_3d_ref_.clear();
        descriptors_ref_ = Mat();
        for ( size_t i=0; i<keypoints_curr_.size(); i++ )
        {
            double d = ref_->findDepth(keypoints_curr_[i]);               
            if ( d > 0)
            {
                Vector3d p_cam = ref_->camera_->pixel2camera(
                    Vector2d(keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y), d
                );
                pts_3d_ref_.push_back( cv::Point3f( p_cam(0,0), p_cam(1,0), p_cam(2,0) ));
                descriptors_ref_.push_back(descriptors_curr_.row(i));
            }
        }
    }
    

    void extractKeyPoints();
    void computeDescriptors();
    void featureMatching();
    这几个函数就是OpenCV中一些常用的使用，见opencv featrue2d module

运行结果

改进：单目版本

• 方式

  • 由于单目相机没有深度信息，我们可以假设特征点的深度均为1（详见《视觉SLAM十四讲》7.4.1 讨论）

• 运行结果

  • 由于WSL不能直接访问摄像头，可以使用摄像头录制一段视频(运动尽量平稳、低速)，然后使用OpenCV读取视频文件，记得修改default.yml文件中的相机内参。(或者直接使用TUM数据集，将setRef3DPoints函数中的深度d改为1)