VINS-Mono代码学习记录（七）----processImage( )

以你之姓@ 2023-06-12 09:11 13阅读 0赞

#### 一、重定位帧的处理 ####

在process( )函数中，在处理完IMU之后，接着就是对重定位帧的设置，代码片段如下：

//[4] 设置重定位帧setReloFrame( )
                sensor_msgs::PointCloudConstPtr relo_msg = NULL;//去看传感器数据的definition
                while (!relo_buf.empty())
                {
                    relo_msg = relo_buf.front();// 返回队首元素的值
                    relo_buf.pop();//删除队列首元素
                }
                if (relo_msg != NULL)
                {
                    vector<Vector3d> match_points;
                    double frame_stamp = relo_msg->header.stamp.toSec();
                    //遍历relo_msg中的points特征点
                    for (unsigned int i = 0; i < relo_msg->points.size(); i++)
                    {
                        Vector3d u_v_id;
                        u_v_id.x() = relo_msg->points[i].x;
                        u_v_id.y() = relo_msg->points[i].y;
                        u_v_id.z() = relo_msg->points[i].z;
                        match_points.push_back(u_v_id);
                    }
                    //[重定位帧的平移向量T的x,y,z，旋转四元数w,x,y,z和索引值]
    
                    Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);//平移
                    Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);//四元数表示的旋转
                    Matrix3d relo_r = relo_q.toRotationMatrix();
                    int frame_index;
                    frame_index = relo_msg->channels[0].values[7];//索引
                    estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
                }

relo\_buf用来接收relo\_msg，从relo\_msg中取出点push到match\_points,再取出帧的时间戳、帧的id、旋转以及平移量传递给估计器的setReloFrame( )函数。

void Estimator::setReloFrame(double _frame_stamp, int _frame_index, vector<Vector3d> &_match_points, Vector3d _relo_t, Matrix3d _relo_r)
    {
        //[1]将传入的各参数进行赋值给重定位相关的变量
        relo_frame_stamp = _frame_stamp;
        relo_frame_index = _frame_index;
        match_points.clear();
        match_points = _match_points;
        prev_relo_t = _relo_t;
        prev_relo_r = _relo_r;
    
        //【2】遍历滑动窗口，将当前传入的重定位帧的时间戳和滑动窗口中的进行对比
        for(int i = 0; i < WINDOW_SIZE; i++)
        {
            if(relo_frame_stamp == Headers[i].stamp.toSec())
            {
                relo_frame_local_index = i;//记录滑窗中是那一帧重定位
                relocalization_info = 1;//时间戳相等,重定位标志置1
                for (int j = 0; j < SIZE_POSE; j++)//SIZE_POSE=7
                    relo_Pose[j] = para_Pose[i][j];// 把两个匹配的帧的位置和旋转四元数存储起来
            }
        }
    }

#### 二、processImage（ ）函数 ####

先来看张图：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODUwNDI3Ng_size_16_color_FFFFFF_t_70]  
代码部分为：

void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)
    {
      //image的数据类型分别表示feature_id,camera_id,点的x,y,z坐标，u,v坐标，在x,y方向上的跟踪速度
        ROS_DEBUG("new image coming ------------------------------------------");
        ROS_DEBUG("Adding feature points %lu", image.size());
    
        //[1]根据视差来决定marg掉哪一帧，如果次新帧是关键帧，marg掉最老帧，如果次新帧不是关键帧,marg掉次新帧
        if (f_manager.addFeatureCheckParallax(frame_count, image, td))//添加特征并检测视差
            marginalization_flag = MARGIN_OLD;//MARFIN_OLD=0
        else
            marginalization_flag = MARGIN_SECOND_NEW;//MARGIN_SECOND_NEW=1
    
        ROS_DEBUG("this frame is--------------------%s", marginalization_flag ? "reject" : "accept");
        ROS_DEBUG("%s", marginalization_flag ? "Non-keyframe" : "Keyframe");
        ROS_DEBUG("Solving %d", frame_count);
        ROS_DEBUG("number of feature: %d", f_manager.getFeatureCount());//计算滑窗内被track过的特征点的数量
        Headers[frame_count] = header;
    
        //【2】将图像数据、时间、临时预积分值存储到图像帧类中,ImageFrame这个类的定义在initial_alignment.h中
        ImageFrame imageframe(image, header.stamp.toSec());
        imageframe.pre_integration = tmp_pre_integration;
        all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));
        tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
    
        //[3]如果ESTIMATE_EXTRINSIC == 2表示需要在线估计imu和camera之间的外参数
        if(ESTIMATE_EXTRINSIC == 2)
        {
            ROS_INFO("calibrating extrinsic param, rotation movement is needed");
            if (frame_count != 0)
            {
                vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
                Matrix3d calib_ric;
                if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
                {
                    ROS_WARN("initial extrinsic rotation calib success");
                    ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
                    ric[0] = calib_ric;
                    RIC[0] = calib_ric;
                    ESTIMATE_EXTRINSIC = 1;
                }
            }
        }
    
        //[4]判断是初始化还是非线性优化
        if (solver_flag == INITIAL)//初始化
        {
            if (frame_count == WINDOW_SIZE)
            {
                bool result = false;
                if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
                {
                   result = initialStructure();
                   initial_timestamp = header.stamp.toSec();
                }
                if(result)
                {
                    solver_flag = NON_LINEAR;
                    solveOdometry();
                    slideWindow();
                    f_manager.removeFailures();
                    ROS_INFO("Initialization finish!");
                    last_R = Rs[WINDOW_SIZE];
                    last_P = Ps[WINDOW_SIZE];
                    last_R0 = Rs[0];
                    last_P0 = Ps[0];
                    
                }
                else
                    slideWindow();
            }
            else
                frame_count++;
        }
        else//非线性优化
        {
            TicToc t_solve;
            solveOdometry();
            ROS_DEBUG("solver costs: %fms", t_solve.toc());
    
            if (failureDetection())
            {
                ROS_WARN("failure detection!");
                failure_occur = 1;
                clearState();
                setParameter();
                ROS_WARN("system reboot!");
                return;
            }
    
            TicToc t_margin;
            slideWindow();
            f_manager.removeFailures();
            ROS_DEBUG("marginalization costs: %fms", t_margin.toc());
            // prepare output of VINS
            key_poses.clear();
            for (int i = 0; i <= WINDOW_SIZE; i++)
                key_poses.push_back(Ps[i]);
    
            last_R = Rs[WINDOW_SIZE];
            last_P = Ps[WINDOW_SIZE];
            last_R0 = Rs[0];
            last_P0 = Ps[0];
        }
    }

#### 三、bool FeatureManager::addFeatureCheckParallax（ ）函数 ####

在processImage( )函数中，又调用了很多别的函数：按顺序来看一下吧，但是有些自己也没读明白。  
这个addFeatureCheckParallax（ ）函数的功能就是将传入的特征点加入feature这个vector中，并且根据视差判断当前帧是不是关键帧来决定marg掉最老帧还是次新帧。

bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
    {
        //image数据类型解释：feature_id,camera_id，p.x,p.y,p.z,u,v,u_velocity,v_velocity
        ROS_DEBUG("input feature: %d", (int)image.size());
        ROS_DEBUG("num of feature: %d", getFeatureCount());//计算滑窗内被track过的特征点的数量
        double parallax_sum = 0;//视差总和
        int parallax_num = 0;//满足条件的特征点数量
        last_track_num = 0;//被跟踪点的个数
    
        //[1]遍历图像image中所有的特征点，和已经记录了特征点的容器feature中进行比较
        for (auto &id_pts : image)
        {
            FeaturePerFrame f_per_fra(id_pts.second[0].second, td);//7*1 x,y,z,u,v,u的速度，v的速度
    
            int feature_id = id_pts.first;//获取特征点的id
            //在feature中查找该feature_id的feature是否存在
            auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
                              {
                return it.feature_id == feature_id;
                              });
    
            if (it == feature.end())//这里的feature的数据类型: list<FeaturePerId> feature;
            {
               //没有找到该feature的id，则把特征点放入feature的list容器中
                feature.push_back(FeaturePerId(feature_id, frame_count));
                feature.back().feature_per_frame.push_back(f_per_fra);//list链表.back()表示返回最后一个元素
            }
            else if (it->feature_id == feature_id)
            {
    /**
     * 如果找到了相同ID特征点，就在其FeaturePerFrame内增加此特征点在此帧的位置以及其他信息，
     * it的feature_per_frame容器中存放的是该feature能够被哪些帧看到，存放的是在这些帧中该特征点的信息
     * 所以，feature_per_frame.size的大小就表示有多少个帧可以看到该特征点
     * */
                it->feature_per_frame.push_back(f_per_fra);
                last_track_num++;//特征点被跟踪的次数+1
            }
        }
    
        if (frame_count < 2 || last_track_num < 20)//如果上一次track的点少于20 就添加点 第一个frame_count表示滑窗内的帧数
            return true;//marg最老帧
    
    //[2]遍历每一个feature，计算能被当前帧和其前两帧共同看到的特征点视差
        for (auto &it_per_id : feature)
        {
    
            if (it_per_id.start_frame <= frame_count - 2 &&
                it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)
            {
                //计算特征点it_per_id在倒数第二帧和倒数第三帧之间的视差，并求所有视差的累加和
                parallax_sum += compensatedParallax2(it_per_id, frame_count);//开始补偿视差啦！
                parallax_num++;//满足条件的特征点数量+1
            }
        }
    
        if (parallax_num == 0)
        {
            return true;//marg最老帧
        }
        else
        {
            //视差总和除以参与计算视差的特征点个数，表示每个特征点的平均视差值
            ROS_DEBUG("parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);
            ROS_DEBUG("current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);
            return parallax_sum / parallax_num >= MIN_PARALLAX;
            //当平均视差大于等于最小视差，返回true，marg最老帧，当平均视差小于最小视差,marg掉次新帧
        }
    }

当平均视差小于最小视差，我想大概表示的是新来的帧和前两帧挨得很近，图像的信息很相似，所以可以去掉相邻的次新帧。  
在addFeatureCheckParallax（ ）这个函数中，又继续调用了compensatedParallax2（ ）来计算视差。

double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)
    {
        //传入第一个参数为滑窗内的每一个feature，第二参数表示当前帧的帧数
        //check the second last frame is keyframe or not刺新帧是否为关键帧
        //parallax betwwen seconde last frame and third last frame 
        const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame];//右边指啥到底？
        const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[frame_count - 1 - it_per_id.start_frame];
    
        double ans = 0;
        Vector3d p_j = frame_j.point;
    
        double u_j = p_j(0);
        double v_j = p_j(1);
        //这里为啥没有先归一化?
    
        Vector3d p_i = frame_i.point;
        Vector3d p_i_comp;
    
        //int r_i = frame_count - 2;
        //int r_j = frame_count - 1;
        //p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] * ric[camera_id_i] * p_i;
        p_i_comp = p_i;
        double dep_i = p_i(2);
        double u_i = p_i(0) / dep_i;
        double v_i = p_i(1) / dep_i;
        double du = u_i - u_j, dv = v_i - v_j;//这跟下面de_comp dv_comp不是一样的吗？
    
        double dep_i_comp = p_i_comp(2);
        double u_i_comp = p_i_comp(0) / dep_i_comp;
        double v_i_comp = p_i_comp(1) / dep_i_comp;
        double du_comp = u_i_comp - u_j, dv_comp = v_i_comp - v_j;
    
        ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));
    
        return ans;
    }

没太看明白。

processImage( )中接下来的处理判断是否需要进行imu和camera之间的外参数估计，这一部分主要的内容在initial文件夹里,下次再更。。。要去看演唱会啦啦，回来再更吧！

演唱会回来了。。后遗症持续中，无心学习。。。

代码：

//[3]如果ESTIMATE_EXTRINSIC == 2表示需要在线估计imu和camera之间的外参数
        if(ESTIMATE_EXTRINSIC == 2)
        {
            ROS_INFO("calibrating extrinsic param, rotation movement is needed");
            if (frame_count != 0)
            {
                vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
                Matrix3d calib_ric;
                if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
                {
                    ROS_WARN("initial extrinsic rotation calib success");
                    ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
                    ric[0] = calib_ric;
                    RIC[0] = calib_ric;
                    ESTIMATE_EXTRINSIC = 1;
                }
            }
        }

上段代码中涉及两个函数f\_manager.getCorresponding( )和initial\_ex\_rotation.CalibrationExRotation( ),  
getCorresponding( )提供vector<pair<Vector3d, Vector3d>> 类型的corres给CalibrationExRotation( )函数，CalibrationExRotation( )求解出相机和imu的旋转Qbc,  
啊，理论知识借用深蓝学院的两张ppt来介绍：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODUwNDI3Ng_size_16_color_FFFFFF_t_70 1]![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODUwNDI3Ng_size_16_color_FFFFFF_t_70 2]svd分解出的V矩阵的最后一列即为Qbc的解。

bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
    {
        frame_count++;
        Rc.push_back(solveRelativeR(corres));//求解两帧之间的相对位姿
        Rimu.push_back(delta_q_imu.toRotationMatrix());
        Rc_g.push_back(ric.inverse() * delta_q_imu * ric);//求出来的是相机上一帧到下一帧的旋转，此表达式由公式推导得出，理论上跟Rc相等
    
        Eigen::MatrixXd A(frame_count * 4, 4);//这一部分需要看理论知识
        A.setZero();
        int sum_ok = 0;
        for (int i = 1; i <= frame_count; i++)
        {
            Quaterniond r1(Rc[i]);
            Quaterniond r2(Rc_g[i]);
    
            double angular_distance = 180 / M_PI * r1.angularDistance(r2);//求角度误差
            ROS_DEBUG(
                "%d %f", i, angular_distance);
    
            double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;//求核函数值
            ++sum_ok;
            Matrix4d L, R;
    
            double w = Quaterniond(Rc[i]).w();//取四元数的实部！！相机的旋转取左L
            Vector3d q = Quaterniond(Rc[i]).vec();//取四元数的虚部
            L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
            L.block<3, 1>(0, 3) = q;
            L.block<1, 3>(3, 0) = -q.transpose();
            L(3, 3) = w;
    
            Quaterniond R_ij(Rimu[i]);//imu的旋转取右R
            w = R_ij.w();
            q = R_ij.vec();
            R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
            R.block<3, 1>(0, 3) = q;
            R.block<1, 3>(3, 0) = -q.transpose();
            R(3, 3) = w;
    
            A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);//加上huber权重
        }
    
        JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);//svd分解
        Matrix<double, 4, 1> x = svd.matrixV().col(3);
        Quaterniond estimated_R(x);//上面估计的R为imu到相机的旋转
        ric = estimated_R.toRotationMatrix().inverse();
        //cout << svd.singularValues().transpose() << endl;
        //cout << ric << endl;
        Vector3d ric_cov;
        ric_cov = svd.singularValues().tail<3>();
        if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
        {
            calib_ric_result = ric;
            return true;
        }
        else
            return false;
    }

其中的solveRelativeR( )求解两帧图像之间的旋转，注释如下…

Matrix3d InitialEXRotation::solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres)//求两帧之间的旋转及平移
    {
        //如果点对数大于等于9，利用对极约束来求解相对运动
        if (corres.size() >= 9)
        {
            //[1]构建cv::Point2f类型的数据
            vector<cv::Point2f> ll, rr;
            for (int i = 0; i < int(corres.size()); i++)
            {
                ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));//取点的x,y坐标push进vector中
                rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
            }
            //【2】求解出本质矩阵并分解求出旋转和平移
            cv::Mat E = cv::findFundamentalMat(ll, rr);
            cv::Mat_<double> R1, R2, t1, t2;
            decomposeE(E, R1, R2, t1, t2);
    
            //[3]加强条件判断
            if (determinant(R1) + 1.0 < 1e-09)
            {
                E = -E;
                decomposeE(E, R1, R2, t1, t2);
            }
    
            //[4]选出更合适的旋转矩阵
            double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2));
            double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2));
            cv::Mat_<double> ans_R_cv = ratio1 > ratio2 ? R1 : R2;
    
            //【5】为啥要这样做？取转置
            Matrix3d ans_R_eigen;
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    ans_R_eigen(j, i) = ans_R_cv(i, j);
            return ans_R_eigen;
        }
        //如果不满足条件，返回单位矩阵
        return Matrix3d::Identity();
    }

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODUwNDI3Ng_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20191111203427607.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODUwNDI3Ng==,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODUwNDI3Ng_size_16_color_FFFFFF_t_70 1]: https://img-blog.csdnimg.cn/2019112014485256.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODUwNDI3Ng==,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODUwNDI3Ng_size_16_color_FFFFFF_t_70 2]: https://img-blog.csdnimg.cn/20191120144904915.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODUwNDI3Ng==,size_16,color_FFFFFF,t_70