VINS-Mono代码解读——状态估计器流程 estimator 写在初始化和非线性优化前

前言

本文主要介绍VINS的状态估计器模块（estimator），主要在代码中/vins_estimator节点的相关部分实现。

这个模块可以说是VINS的最核心模块，从论文的内容上来说，里面的内容包括了VINS的估计器初始化、基于滑动窗口的非线性优化实现紧耦合，即论文第五章（V. ESTIMATOR INITIALIZATION）第六章（VI. TIGHTLY-COUPLED MONOCULAR VIO）。此外还包括了关键帧的选择，即论文第四章（IV. MEASUREMENT PREPROCESSING A. Vision Processing Front-end） 的部分内容。

该模块的代码放在文件夹vins_estimator中，可以看到，除了上述内容外，还包括有外参标定、可视化等其他功能的实现，内容实在是太多了！所以本文主要是对vins_estimator文件夹内每个文件的代码功能进行简单整理，并从estimator_node.cpp开始，对状态估计器的具体流程进行代码解读，初始化以及紧耦合的理论知识和具体实现将放在以后进行详细说明。

其中论文中对于关键帧的选择（论文IV A部分）：
两个关键帧选择标准：
1、与上一个关键帧的平均视差。如果在当前帧和最新关键帧之间跟踪的特征点的平均视差超出某个特定阈值，则将该帧视为新的关键帧。
2、跟踪质量。如果跟踪的特征数量低于某一阈值，则将此帧视为新的关键帧。这个标准是为了避免跟踪特征完全丢失。
具体在bool FeatureManager::addFeatureCheckParallax()中实现。

---

流程图

---

代码实现

vins_estimator文件夹

• factor：
  • imu_factor.h：IMU残差、雅可比
  • intergration_base.h：IMU预积分
  • marginalization.cpp/.h：边缘化
  • pose_local_parameterization.cpp/.h：局部参数化
  • projection_factor.cpp/.h：视觉残差
• initial：
  • initial_alignment.cpp/.h：视觉和IMU校准(陀螺仪偏置、尺度、重力加速度和速度)
  • initial_ex_rotation.cpp/.h：相机和IMU外参标定
  • initial_sfm.cpp/.h：纯视觉SFM、三角化、PNP
  • solve_5pts.cpp/.h：5点法求基本矩阵得到Rt
• utility：
  • CameraPoseVisualization.cpp/.h：相机位姿可视化
  • tic_toc.h：记录时间
  • utility.cpp/.h：各种四元数、矩阵转换
  • visualization.cpp/.h：各种数据发布
• estimator.cpp/.h：紧耦合的VIO状态估计器实现
• estimator_node.cpp：ROS 节点函数，回调函数
• feature_manager.cpp/.h：特征点管理，三角化，关键帧等
• parameters.cpp/.h：读取参数

输入输出

输入：
1、IMU的角速度和线加速度，即订阅了IMU发布的topic：IMU_TOPIC="/imu0"
2、图像追踪的特征点，即订阅了feature_trackers模块发布的topic：“/feature_tracker/feature"
3、复位信号，即订阅了feature_trackers模块发布的topic：“/feature_tracker/restart"
4、重定位的匹配点，即订阅了pose_graph模块发布的topic：“/pose_graph/match_points"

输出：
1、在线程void process()中给RVIZ发送里程计信息PQV、关键点三维坐标、相机位姿、点云信息、IMU到相机的外参、重定位位姿等

pubOdometry(estimator, header);//"odometry"
pubKeyPoses(estimator, header);//"key_poses"
pubCameraPose(estimator, header);//"camera_pose"
pubPointCloud(estimator, header);//"history_cloud"
pubTF(estimator, header);//"extrinsic"
pubKeyframe(estimator);//"keyframe_point"、"keyframe_pose"
if (relo_msg != NULL)
	pubRelocalization(estimator);//"relo_relative_pose"

2、在回调函数void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)发布最新的由IMU直接递推得到的PQV

if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
            pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);//"imu_propagate"

---

estimator_node.cpp

函数	功能
void predict	从IMU测量值imu_msg和上一个PVQ递推得到当前PVQ
void update()	得到窗口最后一个图像帧的imu项[P,Q,V,ba,bg,a,g]，对imu_buf中剩余imu_msg进行PVQ递推
getMeasurements()	对imu和图像数据进行对齐并组合
void imu_callback	imu回调函数，将imu_msg存入imu_buf，递推IMU的PQV并发布"imu_propagate”
void feature_callback	feature回调函数，将feature_msg放入feature_buf
void restart_callback	restart回调函数，收到restart消息时清空feature_buf和imu_buf，估计器重置，时间重置
void relocalization_callback	relocalization回调函数，将points_msg放入relo_buf
void process()	VIO的主线程
int main()	程序入口

---

程序入口 int main(int argc, char **argv)

1、ROS初始化、设置句柄

ros::init(argc, argv, "vins_estimator");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

2、读取参数，设置状态估计器参数

readParameters(n);
estimator.setParameter();

3、发布用于RVIZ显示的Topic，本模块具体发布的内容详见输入输出

registerPub(n);

4、订阅IMU、feature、restart、match_points的topic，执行各自回调函数

ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);
ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);

5、创建VIO主线程

std::thread measurement_process{process};

---

4个回调函数

这里需要注意的一点是：节点estimator，以及创建了一个process，必须考虑多线程安全问题：
1、队列imu_buf、feature_buf、relo_buf是被多线程共享的，因而在回调函数将相应的msg放入buf或进行pop时，需要设置互斥锁m_buf，在操作前lock()，操作后unlock()。其他互斥锁同理。
2、在feature_callback和imu_callback中还设置了条件锁，在完成将msg放入buf的操作后唤醒作用于process线程中的获取观测值数据的函数。

std::condition_variable con;
con.notify_one();

3、在imu_callback中还通过lock_guard的方式构造互斥锁m_state，它能在构造时加锁，析构时解锁。

std::lock_guard<std::mutex> lg(m_state);

---

VIO主线程 void process()

通过while (true)不断循环，主要功能包括等待并获取measurements，计算dt，然后执行以下函数：
stimator.processIMU()进行IMU预积分
estimator.setReloFrame()设置重定位帧
estimator.processImage()处理图像帧：初始化，紧耦合的非线性优化
其中measurements的数据格式可以表示为：(IMUs, img_msg)s s表示容器（vector）

1、 等待上面两个接收数据完成就会被唤醒，在执行getMeasurements()提取measurements时互斥锁m_buf会锁住，此时无法接收数据。
getMeasurements()的作用是对imu和图像数据进行对齐并组合，之后会具体分析

std::unique_lock<std::mutex> lk(m_buf);
con.wait(lk, [&]
	{
	return (measurements = getMeasurements()).size() != 0;
	});
lk.unlock();

2、对measurements中的每一个measurement (IMUs,IMG)组合进行操作
for (auto &measurement : measurements)

2.1、对于measurement中的每一个imu_msg，计算dt并执行processIMU()。
processIMU()实现了IMU的预积分，通过中值积分得到当前PQV作为优化初值

estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));              

2.2、在relo_buf中取出最后一个重定位帧，拿出其中的信息并执行setReloFrame()

// set relocalization frame
sensor_msgs::PointCloudConstPtr relo_msg = NULL;
while (!relo_buf.empty())
{
	relo_msg = relo_buf.front();
    relo_buf.pop();
}
if (relo_msg != NULL)
{
	/*...*/
    estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
}

2.3、建立每个特征点的(camera_id,[x,y,z,u,v,vx,vy])s的map，索引为feature_id

map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
for (unsigned int i = 0; i < img_msg->points.size(); i++)
{
	int v = img_msg->channels[0].values[i] + 0.5;
    int feature_id = v / NUM_OF_CAM;
    int camera_id = v % NUM_OF_CAM;
    /* double x,y,z,p_u,p_v,velocity_x,velocity_y     */
    ROS_ASSERT(z == 1);//判断是否归一化了    Eigen::Matrix xyz_uv_velocity;
    xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
    image[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
}

2.4、处理图像，这里实现了视觉与IMU的初始化以及非线性优化的紧耦合

estimator.processImage(image, img_msg->header);

2.5、向RVIZ发布里程计信息、关键位姿、相机位姿、点云和TF关系，这部分在之前输入输出已经介绍了

2.6、更新IMU参数[P,Q,V,ba,bg,a,g]，注意线程安全

m_buf.lock();
m_state.lock();
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
	update();//更新IMU参数[P,Q,V,ba,bg,a,g]
m_state.unlock();
m_buf.unlock();

---

std::vector, sensor_msgs::PointCloudConstPtr>> getMeasurements()

该函数的主要功能是对imu和图像数据进行对齐并组合，返回的是(IMUs, img_msg)s，即图像帧所对应的所有IMU数据，并将其放入一个容器vector中。
IMU和图像帧的对应关系在新版的代码中有变化：对图像帧j，每次取完imu_buf中所有时间戳小于它的imu_msg，以及第一个时间戳大于图像帧时间戳的imu_msg （这里还需要加上同步时间存在的延迟td）。
因此在新代码中，每个大于图像帧时间戳的第一个imu_msg是被两个图像帧共用的，而产生的差别在processIMU()前进行了对应的处理。
（这一部分我不知道自己理解的对不对，若有错误请指出！）

img:    i -------- j  -  -------- k
imu:    - jjjjjjjj - j+k kkkkkkkk -

std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>>
getMeasurements()
{
    std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;

    while (true)
    {
        //直到把缓存中的图像特征数据或者IMU数据取完，才能够跳出此函数
        if (imu_buf.empty() || feature_buf.empty())
            return measurements;

        //对齐标准：IMU最后一个数据的时间要大于第一个图像特征数据的时间
        if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td))
        {
            //ROS_WARN("wait for imu, only should happen at the beginning");
            sum_of_wait++;
            return measurements;
        }

        //对齐标准：IMU第一个数据的时间要小于第一个图像特征数据的时间
        if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td))
        {
            ROS_WARN("throw img, only should happen at the beginning");
            feature_buf.pop();
            continue;
        }

        sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();
        feature_buf.pop();

        std::vector<sensor_msgs::ImuConstPtr> IMUs;

        //图像数据(img_msg)，对应多组在时间戳内的imu数据,然后塞入measurements
        while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)
        {
            //emplace_back相比push_back能更好地避免内存的拷贝与移动
            IMUs.emplace_back(imu_buf.front());
            imu_buf.pop();
        }
        
        //这里把下一个imu_msg也放进去了,但没有pop
        //因此当前图像帧和下一图像帧会共用这个imu_msg
        IMUs.emplace_back(imu_buf.front());
        if (IMUs.empty())
            ROS_WARN("no imu between two image");

        measurements.emplace_back(IMUs, img_msg);
    }
    return measurements;
}

---

estimator.cpp/.h

构建了一个estimator类，这次我们主要讨论流程问题，因而暂时只分析一下processImage()

方法	功能
void Estimator::setParameter()	设置部分参数
void Estimator::clearState()	清空或初始化滑动窗口中所有的状态量
void Estimator::processIMU()	处理IMU数据，预积分
void Estimator::processImage()	处理图像特征数据
bool Estimator::initialStructure()	视觉的结构初始化
bool Estimator::visualInitialAlign()	视觉惯性联合初始化
bool Estimator::relativePose()	判断两帧有足够视差30且内点数目大于12则可进行初始化，同时得到R和T
void Estimator::solveOdometry()	VIO非线性优化求解里程计
void Estimator::vector2double()	vector转换成double数组，因为ceres使用数值数组
void Estimator::double2vector()	数据转换，vector2double的相反过程
bool Estimator::failureDetection()	检测系统运行是否失败
void Estimator::optimization()	基于滑动窗口的紧耦合的非线性优化，残差项的构造和求解
void Estimator::slideWindow()	滑动窗口法
void Estimator::setReloFrame()	重定位操作

void Estimator::processImage()

1、addFeatureCheckParallax()添加之前检测到的特征点到feature容器list中，计算每一个点跟踪的次数，以及它的视差并通过检测两帧之间的视差决定是否作为关键帧。
param[in] frame_count 窗口内帧的个数
param[in] image 某帧所有特征点的[camera_id,[x,y,z,u,v,vx,vy]]构成的map,索引为feature_id
param[in] td 相机和IMU同步校准得到的时间差

    if (f_manager.addFeatureCheckParallax(frame_count, image, td))
        marginalization_flag = MARGIN_OLD;//=0
    else
        marginalization_flag = MARGIN_SECOND_NEW;//=1

2、 将图像数据、时间、临时预积分值存到图像帧类中

   
    ImageFrame imageframe(image, header.stamp.toSec());
    imageframe.pre_integration = tmp_pre_integration;
    
    all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));

3、更新临时预积分初始值

    
    tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

4、判断是否需要进行外参标定

    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;
            }
        }
    }

5、solver_flag==INITIAL 进行初始化
5.1、确保有足够的frame参与初始化，有外参，且当前帧时间戳大于初始化时间戳+0.1秒

5.2、执行视觉惯性联合初始化

result = initialStructure();
initial_timestamp = header.stamp.toSec();

5.3、初始化成功则进行一次非线性优化，不成功则进行滑窗操作

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();

6、solver_flag==NON_LINEAR进行非线性优化

6.1、执行非线性优化具体函数solveOdometry()

6.2、检测系统运行是否失败，若失败则重置估计器

if (failureDetection())//失败
        {
            ROS_WARN("failure detection!");
            failure_occur = 1;
            clearState();
            setParameter();
            ROS_WARN("system reboot!");
            return;
        }

6.3、执行窗口滑动函数slideWindow();

6.4、去除估计失败的点并发布关键点位置

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]);

---

void update()

这个函数在非线性优化时才会在process()中被调用
1、从估计器中得到滑动窗口中最后一个图像帧的imu更新项[P,Q,V,ba,bg,a,g]

    latest_time = current_time;
    tmp_P = estimator.Ps[WINDOW_SIZE];
    tmp_Q = estimator.Rs[WINDOW_SIZE];
    tmp_V = estimator.Vs[WINDOW_SIZE];
    tmp_Ba = estimator.Bas[WINDOW_SIZE];
    tmp_Bg = estimator.Bgs[WINDOW_SIZE];
    acc_0 = estimator.acc_0;
    gyr_0 = estimator.gyr_0;

2、对imu_buf中剩余的imu_msg进行PVQ递推
（因为imu的频率比图像频率要高很多，在getMeasurements(）将图像和imu时间对齐后，imu_buf中还会存在imu数据）

	queue<sensor_msgs::ImuConstPtr> tmp_imu_buf = imu_buf;
    for (sensor_msgs::ImuConstPtr tmp_imu_msg; !tmp_imu_buf.empty(); tmp_imu_buf.pop())
        predict(tmp_imu_buf.front());

---