Vins-Mono 阅读笔记——estimator
vins_estimator 概述
基本上VINS里面绝大部分功能都在这个package下面，包括IMU数据的处理(前端)，初始化(我觉得可能属于是前端)，滑动窗口(后端)，非线性优化(后端)，关键帧的选取(部分内容)(前端)。我第一次看的时候，总是抱有一个疑问，就是为什么把这么多内容全都放在这一个node里面。为了回答这个问题，那么首先先搞清楚vins_estimator里面分别具体都是什么，为什么要有这些数据结构/函数，这些函数是怎样工作的。
ROS package组成
这个package下面主要以下文件：
factor——主要用于非线性优化对各个参数块和残差块的定义，VINS采用的是ceres，所以这部分需要对一些状态量和因子进行继承和重写。
initial——主要用于初始化，VINS采用的初始化策略是先SfM进行视觉初始化，再与IMU进行松耦合。
estimator.cpp——vins_estimator需要的所有函数都放在这里，是一个鸿篇巨制。
estimator_node.cpp——vins_estimator的入口，是一个ROS的node，实际上运行的是这个cpp文件。
feature_manager.cpp——负责管理滑窗内的所有特征点。
parameters.cpp——读取参数，是一个辅助函数。
utility——里面放着用于可视化的函数和tictok计时器。
然后就是要注意CmakeLists.txt和package.xml文件的写法，这两个文件也是相当于套公式，写错了就不能实现正常的ROS功能了。
main()入口
1、ROS初始化，设置句柄
// 1、ROS初始化、设置句柄
ros::init(argc, argv, "vins_estimator");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
2、读取参数，设置状态估计器参数
// 2、读取参数，设置状态估计器参数
readParameters(n);
estimator.setParameter();
3、发布用于RVIZ显示的topic
registerPub(n);
4、订阅imu_topic，执行回调imu_callback
ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
if (imu_msg->header.stamp.toSec() <= last_imu_t)
{
ROS_WARN("imu message in disorder!");
return;
}
last_imu_t = imu_msg->header.stamp.toSec();
m_buf.lock();
imu_buf.push(imu_msg);
m_buf.unlock();
con.notify_one();
last_imu_t = imu_msg->header.stamp.toSec();
{
std::lock_guard<std::mutex> lg(m_state);
predict(imu_msg);
std_msgs::Header header = imu_msg->header;
header.frame_id = "world";
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
}
}
该回调主要干了3件事情：
- 1、往imu_buf里面放imu数据
- queue<sensor_msgs::ImuConstPtr> imu_buf;
- 2、IMU预积分得到当前时刻的PVQ
- predict(imu_msg)
- 3、如果当前处于非线性优化阶段的话，需要把第二件事计算得到的PVQ发布到rviz里去，见utility/visualization.cpp的pubLatestOdometry()函数
5、订阅/feature_tracker/feature，执行feature_callback
ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
// feature回调函数，将feature_msg放入feature_buf
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
if (!init_feature)
{
//skip the first detected feature, which doesn't contain optical flow speed
init_feature = 1;
return;
}
m_buf.lock();
feature_buf.push(feature_msg);
m_buf.unlock();
con.notify_one();
}
这一部分接收的是feature_tracker_node发布的在cur帧的所有特征点的信息。
feature_callback就只干了一件事，就是把cur帧的所有特征点放到feature_buf里，同样需要上锁。注意，cur帧的所有特征点都是整合在一个数据里的，也就是sensor_msgs::PointCloudConstPtr &feature_msg。
6、订阅/feature_tracker/restart，执行restart_callback
ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);
7、订阅/pose_graph/match_points，执行relocalization_callback
ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);
8、创建主线程process
std::thread measurement_process{process};
这一部分是最重要的，包含了VINS绝大部分内容和最难的内容。
8.1 对imu和图像数据进行对齐
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
std::unique_lock<std::mutex> lk(m_buf);
con.wait(lk, [&]{return (measurements = getMeasurements()).size() != 0; });
lk.unlock();
:::success
数据结构: measurements
1、首先，measurements他自己就是一个vector；
2、对于measurements中的每一个measurement，又由2部分组成；
3、第一部分，由sensor_msgs::ImuConstPtr组成的vector；
4、第二部分，一个sensor_msgs::PointCloudConstPtr；
5、这两个sensor_msgs见3.1-6部分介绍。
6、为什么要这样配对(一个PointCloudConstPtr配上若干个ImuConstPtr)？
因为IMU的频率比视觉帧的发布频率要高，所以说在这里，需要把一个视觉帧和之前的一串IMU帧的数据配对起来。
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
:::
容器measurements有了，接下来就是配对IMU和视觉帧的数据，放到容器里去。配对过程也需要上锁，而且是一个条件锁。作者在这里用了一个lambda表达式，也就是说，在return里面的部分是false时，保持上锁状态，继续配对数据；如果return里面时true，说明配对完成，释放锁，measurements完成了，以供后续使用。
接下来分析一下measurements()具体的功能：
// 对imu和图像数据进行对齐并组合（根据时间戳，挑选当前帧和上一帧的IMU数据，用于后续IMU积分）
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)
{
if (imu_buf.empty() || feature_buf.empty())
return measurements;
// imu *******
// image *****
// 这就是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 ****
// image ******
// 这种只能扔掉一些image帧
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;
}
// 此时就保证了图像前一定有imu数据
sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();
feature_buf.pop();
// 一般第一帧不会严格对齐，但是后面就都会对齐，当然第一帧也不会用到
std::vector<sensor_msgs::ImuConstPtr> IMUs;
//imu *****
//image ***
//图像前一定有IMU数据
while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)
{
IMUs.emplace_back(imu_buf.front());
imu_buf.pop();
}
// 保留图像时间戳后一个imu数据，但不会从buffer中扔掉
// imu * *
// image *
IMUs.emplace_back(imu_buf.front());
if (IMUs.empty())
ROS_WARN("no imu between two image");
//每一项的格式
// imu ****** *
// image *
measurements.emplace_back(IMUs, img_msg);
}
return measurements;
}
8.2 对IMU数据进行处理
// 2、对measurements中的每一个measurement(IMUs,IMG)组合进行操作
for (auto &measurement : measurements)
{
auto img_msg = measurement.second;
double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
// 遍历imu
for (auto &imu_msg : measurement.first)
{
double t = imu_msg->header.stamp.toSec();
double img_t = img_msg->header.stamp.toSec() + estimator.td;
if (t <= img_t)
{
if (current_time < 0)
current_time = t;
double dt = t - current_time; // 当前imu信息与上一个imu信息之间的时间差
ROS_ASSERT(dt >= 0);
current_time = t;
dx = imu_msg->linear_acceleration.x;
dy = imu_msg->linear_acceleration.y;
dz = imu_msg->linear_acceleration.z;
rx = imu_msg->angular_velocity.x;
ry = imu_msg->angular_velocity.y;
rz = imu_msg->angular_velocity.z;
// 时间差和imu数据送进去
// 2.1、对于measurement中的每一个imu_msg，计算dt并执行processIMU()。
//processIMU()实现了IMU的预积分，通过中值积分得到当前PQV作为优化初值
estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
//printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);
}
else // 这就是针对最后一个imu数据，需要做一个简单的线性插值
{
double dt_1 = img_t - current_time;
double dt_2 = t - img_t;
current_time = img_t;
ROS_ASSERT(dt_1 >= 0);
ROS_ASSERT(dt_2 >= 0);
ROS_ASSERT(dt_1 + dt_2 > 0);
double w1 = dt_2 / (dt_1 + dt_2);
double w2 = dt_1 / (dt_1 + dt_2);
dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
//printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz);
}
}
processIMU()
:::success
/这一段作用就是就是给以下数据提供初始值/初始化：
pre_integrations[frame_count]
dt_buf[frame_count]
linear_acceleration_buf[frame_count]
angular_velocity_buf[frame_count]
Rs[frame_count]
PS[frame_count]
Vs[frame_count]
- TODO 关于frame_count的更新，目前只在process_img里的solver_flag == INITIAL这里看到?
-
*/
:::
void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
if (!first_imu)
{
first_imu = true;
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
// 滑窗中保留11帧，frame_count表示现在处理第几帧，一般处理到第11帧时就保持不变了
// 由于预积分是帧间约束，因此第1个预积分量实际上是用不到的
if (!pre_integrations[frame_count])
{
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
}
// 所以只有大于0才处理
if (frame_count != 0)
{
//pre_integrations[frame_count]、tmp_pre_integration[frame_count]的初始化
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
//dt、加速度、角速度放入buf中
dt_buf[frame_count].push_back(dt);
linear_acceleration_buf[frame_count].push_back(linear_acceleration);
angular_velocity_buf[frame_count].push_back(angular_velocity);
// 又是一个中值积分，更新滑窗中状态量，本质是给非线性优化提供可信的初始值
int j = frame_count;
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g; //a0=Q(a^-ba)-g 上一帧速度
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j]; //w=1/2(w0+w1)-bg
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix(); //更新R
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g; //a1 当前帧速度
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1); //中值积分a=0.5*(a0+a1)
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc; //Pk+1=Pk+v*t+1/2*a*t^2
Vs[j] += dt * un_acc; //Vk+1=Vk+a*t
}
//当frame_count==0时表示华东窗口内还没有图像帧数据，不需要进行预积分，只进行加速度和角速度初始值的更新
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
:::success
数据结构:
1、Rs[frame_count]，Ps[frame_count]，Vs[frame_count]:是从IMU系转到world系的PVQ，数据是由IMU预积分得到的，目前在这里存放的是没有用bias修正过的值。
2、frame_count:这个值让我很疑惑，它只在processImage()里有过++操作，而且在estimator.hpp声明的时候，没有加上static关键字。它是在h文件中声明，在cpp文件里初始化的，后续需要再关注一下。
3、dt_buf，linear_acceleration_buf，angular_velocity_buf：帧数和IMU测量值的缓存，而且它们是对齐的。
3、pre_integrations[frame_count]，它是IntegrationBase的一个实例，在factor/integration_base.h中定义，它保存着frame_count帧中所有跟IMU预积分相关的量，包括F矩阵，Q矩阵，J矩阵等。
:::
新的理解
这里processIMU后面的中值积分部分其实就是当前时刻PVQ的中值离散形式，它表示了从第i个IMU时刻到i+1个IMU时刻的积分过程，代码中的j就是i+1时刻，IMU积分出来的第j时刻的物理量可以作为第j帧图像的视觉估计初值（本质就是提供了一个初值）
注意这里的Rs[j]、Ps[j]，Vs[j]并不是预积分，而是当前时刻的离散形式的中值积分
预积分是两帧之间的PVQ增量形式，与这里的当前时刻不同
8.3 重定位和回环检测
回到estimator_node.cpp的process()函数。接下来的代码的作用是，在relo_buf中取出最后一个重定位帧，拿出其中的信息并执行setReloFrame()
// set relocalization frame
sensor_msgs::PointCloudConstPtr relo_msg = NULL;
//2.2、在relo_buf中取出最后一个重定位帧，拿出其中的信息并执行setReloFrame()
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();
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);
}
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);//设置重定位帧
}
8.4 对image信息进行处理
(1)一开始，就定义了一个新的数据结构，解释一下：
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
:::success
数据结构: map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image
1、虽然它叫image，但是这个容器里面存放的信息是每一个特征点的！
2、索引值是feature_id；
3、value值是一个vector，如果系统是多目的，那么同一个特征点在不同摄像头下会有不同的观测信息，那么这个vector，就是存储着某个特征点在所有摄像头上的信息。对于VINS-mono来说，value它不是vector，仅仅是一个pair，其实是可以的。
4、接下来看这个vector里面的每一pair。int对应的是camera_id，告诉我们这些数据是当前特征点在哪个摄像头上获得的。
5、Matrix<double, 7, 1>是一个7维向量，依次存放着当前feature_id的特征点在camera_id的相机中的归一化坐标，像素坐标和像素运动速度，这些信息都是在feature_tracker_node.cpp中获得的。
:::
(2)这个数据结构定义完之后，接下来就是往这个容器中放数据。
//遍历img_msg里面的每一个特征点的归一化坐标
for (unsigned int i = 0; i < img_msg->points.size(); i++)
{
//把img的信息提取出来放在image容器里去，通过这里，可以理解img信息里面装的都是些什么
int v = img_msg->channels[0].values[i] + 0.5; //channels[0].values[i]==id_of_point
//hash
int feature_id = v / NUM_OF_CAM;
int camera_id = v % NUM_OF_CAM;
double x = img_msg->points[i].x;
double y = img_msg->points[i].y;
double z = img_msg->points[i].z;
double p_u = img_msg->channels[1].values[i];
double p_v = img_msg->channels[2].values[i];
double velocity_x = img_msg->channels[3].values[i];
double velocity_y = img_msg->channels[4].values[i];
Eigen::Matrix<double, 7, 1> 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);
}
(3) 处理图像processImage() (核心！)
estimator.processImage(image, img_msg->header);//处理图像帧：初始化，紧耦合的非线性优化
8.5 可视化
向RVIZ发布里程计信息、关键位姿、相机位姿、点云和TF关系，这些函数都定义在中utility/visualization.cpp里，都是ROS相关代码。
pubOdometry(estimator, header);
pubKeyPoses(estimator, header);
pubCameraPose(estimator, header);
pubPointCloud(estimator, header);
pubTF(estimator, header);
pubKeyframe(estimator);
if (relo_msg != NULL)
pubRelocalization(estimator);
8.6 IMU的PVQ信息更新
更新IMU参数[P,Q,V,ba,bg,a,g]，需要上锁，注意线程安全。这里对应的知识点是4.1.1最后一个公式。
