VINS-Mono阅读笔记一

VINS-Mono阅读笔记一
标签： vslam IMU

1. 概要

---

本阅读笔记将围绕vins-mono的ros功能包展开介绍，第二部分介绍camera_model的相机球体模型，抽出其中的鱼眼相机模型进行详细分析；eature_model模块介绍前端的特征点检测算法及光流跟踪模块；vis_estimator介绍算法的后端主体部分，涉及相机、IMU外参数标定，状态初始化，数据对齐，非线性优化等内容；pose_graph介绍图优化相关的内容。

2. camera_model

---

vins-mono支持的相机格式为等距投影模型的鱼眼相机模型（与opencv的鱼眼模型一致），针孔相机模型及全景相机模型，下面将比较vins鱼眼相机模型中的代码与opencv中的鱼眼相机模型代码，并作详细分析。　 图2.1显示了等距模型的鱼眼相机成像模型，$P_c$为相机坐标系下的点，成像平面A垂直于光轴Z且与球体相切。绿点$P_{undistorted}$为$P_c$归一化后的3D点，前两维坐标即为畸变矫正归一化后的像素坐标点。黄点为入射光线与单位球体的交点，其与球体上顶点构成的弧线向成像平面等距投影后的蓝点$P_{distorted}$即为畸变图像归一化后的像素坐标点。下面代码为vins中的等距鱼眼模型代码：

    template<typename T>
　　// 畸变系数拟合
    T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta)
    {
   
        // k1 = 1
        return theta +
               k2 * theta * theta * theta +
               k3 * theta * theta * theta * theta * theta +
               k4 * theta * theta * theta * theta * theta * theta * theta +
               k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta;
    }
    
    // 将2D畸变原图坐标转为投影平面的3D坐标（矫正过程）
    void EquidistantCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const
    {
   
        // Lift points to normalised plane
        Eigen::Vector2d p_u;
        p_u << m_inv_K11 * p(0) + m_inv_K13,
               m_inv_K22 * p(1) + m_inv_K23;
    
        // Obtain a projective ray
        double theta, phi;
        backprojectSymmetric(p_u, theta, phi);
    
        P(0) = sin(theta) * cos(phi);
        P(1) = sin(theta) * sin(phi);
        P(2) = cos(theta);
    }
    
    // 将射线上的3D点投影到成像平面（投影过程）
    void EquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const
    {
   
        double theta = acos(P(2) / P.norm());
        double phi = atan2(P(1), P(0));
    
        Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi));
    
        // Apply generalised projection matrix
        p << mParameters.mu() * p_u(0) + mParameters.u0(),
             mParameters.mv() * p_u(1) + mParameters.v0();
    }

下图为OPENCV对鱼眼相机模型的描述，采用同vins相同的等距投影模型，其中$x^{‘}$与$y^{‘}$为上述代码中的p_u向量，即：归一化的畸变平面像素坐标$P_{distorted}$。其计算公式利用图2.1中黄色、绿色三角形相似计算而得，同vins中EquidistantCamera::spaceToPlane函数利用$\varphi$计算一致。
　　

3. feature_model

---

feature_model功能包入口函数为feature_tracker_node.cpp中的main函数，其代码如下，主函数在img_callback回调中将检测到的特征角点发布出去。
　　该功能包订阅了IMAGE_TOPIC（/cam0/image_raw），并创建了三个发布的话题：
　　pub_img–发布feature topic
　　pub_match–发布feature_img topic
　　pub_restart–发布restart topic

3.1 代码流程图

　　　　　　　　　　　 图3.1 feature_model节点代码流程图

3.2 各关键代码段分析

3.2.1 img_callback()

下面就img_callback回调函数中的关键代码详细分析：

   //若当前帧时间与前一帧时间差<1s或时间错位则重新开始第一帧
    if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)
    {
   
        ROS_WARN("image discontinue! reset the feature tracker!");
        first_image_flag = true; 
        last_image_time = 0;
        pub_count = 1;
        std_msgs::Bool restart_flag;
        restart_flag.data = true;
        pub_restart.publish(restart_flag);
        return;
    }

vins并非将每一帧检测到的特征点都发送，而是控制以不大于10Hz的频率

    // 节点发布频率控制 < 10Hz
    if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
    {
   
        PUB_THIS_FRAME = true;
        // reset the frequency control, FREQ = 10
        // 控制特征点的发布频率<10Hz,否则发布flag置零        
        if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
        {
   
            first_image_time = img_msg->header.stamp.toSec();
            pub_count = 0;
        }
    }
    else
        PUB_THIS_FRAME = false;

消息节点发布的内容

        sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
        sensor_msgs::ChannelFloat32 id_of_point;   // 特征点ID
        sensor_msgs::ChannelFloat32 u_of_point;    // 特征点坐标u
        sensor_msgs::ChannelFloat32 v_of_point;     // 特征点坐标v
        sensor_msgs::ChannelFloat32 velocity_x_of_point;  // 光流速度x
        sensor_msgs::ChannelFloat32 velocity_y_of_point;  // 光流速度y

        feature_points->header = img_msg->header;  // 图像头部，含时间戳
        feature_points->header.frame_id = "world";

        vector<set<int>> hash_ids(NUM_OF_CAM);
        for (int i = 0; i < NUM_OF_CAM; i++)
        {
   
            auto &un_pts = trackerData[i].cur_un_pts;// 畸变矫正归一化2D点
            auto &cur_pts = trackerData[i].cur_pts; // 原图畸变2D点
            auto &ids = trackerData[i].ids;         // 特征点ID号标识
            auto &pts_velocity = trackerData[i].pts_velocity; // 前后帧特征点光流速度
            for (unsigned int j = 0; j < ids.size(); j++)
            {
   
                if (trackerData[i].track_cnt[j] > 1)
                {
   
                    int p_id = ids[j];
                    hash_ids[i].insert(p_id);
                    geometry_msgs::Point32 p;
                    p.x = un_pts[j].x;
                    p.y = un_pts[j].y;
                    p.z = 1;

                    feature_points->points.push_back(p);
                    id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
                    u_of_point.values.push_back(cur_pts[j].x);
                    v_of_point.values.push_back(cur_pts[j].y);
                    velocity_x_of_point.values.push_back(pts_velocity[j].x);
                    velocity_y_of_point.values.push_back(pts_velocity[j].y);
                }
            }
        }
        feature_points->channels.push_back(id_of_point);
        feature_points->channels.push_back(u_of_point);
        feature_points->channels.push_back(v_of_point);
        feature_points->channels.push_back(velocity_x_of_point);
        feature_points->channels.push_back(velocity_y_of_point);

3.2.2 readImage()

readImage函数代码分析如下，

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
   
    cv::Mat img;
    TicToc t_r;
    cur_time = _cur_time;
    // 亮度均衡开启开关
    if (EQUALIZE)
    {
   
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        TicToc t_c;
        clahe->apply(_img, img);
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    }
    else
        img = _img;

    // 第一帧时为空，否则forw_img为当前帧图像
    if (forw_img.empty())
    {
   
        prev_img = cur_img = forw_img = img;
    }
    else
    {
   
        forw_img = img;
    }

    forw_pts.clear();

    // 如果已经存在前帧光流特征
    if (cur_pts.size() > 0)
    {
   
        TicToc t_o;
        vector<uchar> status;
        vector<float> err;
        //光流跟踪
        // cur_img/前一帧图像  forw_img/当前帧图像 status/ 1:跟踪成功 0:跟踪失败
        cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);

        // 遍历角点，过滤在图像外的点
        for (int i = 0; i < int(forw_pts.size()); i++)
            if (status[i] && !inBorder(forw_pts[i]))
                status[i] = 0;

        // 根据status状态更新所有点及ID         
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(ids, status);
        reduceVector(cur_un_pts, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
    }

    // 将所有跟踪的角点跟踪次数累加
    for (auto &n : track_cnt)
        n++;

    // 满足发布频率条件下
    if (PUB_THIS_FRAME)
    {
   
        // ransac 过滤外点
        rejectWithF();
        ROS_DEBUG("set mask begins");
        TicToc t_m;
        // 为跟踪到的特征点添加模板，防止二次检测
        setMask();
        ROS_DEBUG("set mask costs %fms", t_m.toc());

        ROS_DEBUG("detect feature begins");
        TicToc t_t;
        // 若跟踪角点< 150则重新检测其他区域的角点
        int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
        if (n_max_cnt > 0)
        {
   
            if(mask.empty())
                cout << "mask is empty " << endl;
            if (mask.type() != CV_8UC1)
                cout << "mask type wrong " << endl;
            if (mask.size() != forw_img.size())
                cout << "wrong size " << endl;
            cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
        }
        else
            n_pts.clear();
        ROS_DEBUG("detect feature costs: %fms", t_t.toc());

        ROS_DEBUG("add feature begins");
        TicToc t_a;
        addPoints();
        ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
    }

    // 将图片数据、角点存入前一帧
    prev_img = cur_img;
    prev_pts = cur_pts;
    prev_un_pts = cur_un_pts;
    cur_img = forw_img;
    cur_pts = forw_pts;
    // 畸变矫正，计算光流速度
    undistortedPoints();
    prev_time = cur_time;
}

3.2.3 rejectWithF()

rejectWithF()函数利用F矩阵和ransac过滤外点,ransac距离阈值为1像素;此外，所有的特征点畸变矫正到统一固定的图像尺寸，防止因为尺寸差异导致ransac距离阈值无统一的尺度。

void FeatureTracker::rejectWithF()
{
   
    if (forw_pts.size() >= 8)
    {
   
        ROS_DEBUG("FM ransac begins");
        TicToc t_f;
        vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
   
            // 将畸变原点投影到投影平面（矫正），归一化后转化为 FOCAL_LENGTH =460 ，偏移量为固定值的图像上
            Eigen::Vector3d tmp_p;
            m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());

            m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z