1. process主线程
（1）process大致流程
- 在使用线程锁的情况下，使用getMeasurements进行视觉惯性对齐
- 获取对齐后的imu数据。其中，对于最后一个imu数据，需要根据时间戳通过插值将最后一个imu数据对齐到image上。然后再执行processIMU，主要是更新预积分量和滑窗中的状态量。目的是为了给滑窗中的非线性优化提供可信的初始值。
- 进行回环检测
- 获取对齐后的image中像素信息，然后执行视觉重投影约束的优化processImage
- 发送topic，用最新VIO结果最新imu对应的位姿update
（2）process代码实现
// vins_estimator/src/estimator_node.cpp
void process()
{
while (true)
{
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>,
sensor_msgs::PointCloudConstPtr>> measurements;
std::unique_lock<std::mutex> lk(m_buf);
// 通过getMeasurements获取对齐后的图像和IMU数据
// con.wait: 等待lk锁的通知, 无论是IMU还是feature，在获得新数据时都要执行
// con.notify_one(); 等待通知 若return 返回true，则可以进行后续操作
con.wait(lk, [&]
{
return (measurements = getMeasurements()).size() != 0; // 将视觉与IMU进行对齐
});
lk.unlock(); // 数据buffer的锁解锁，回调可以继续塞数据了
m_estimator.lock(); // 进行后端求解，不能和复位重启冲突
// 给予范围的for循环，这里是遍历每组对齐后的image imu组合,一组imu-image组合包含一个image和多个imu数据
for (auto &measurement : measurements)
{
auto img_msg = measurement.second; // 获取对齐的img
double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
// 遍历这一组对齐imu-image组合中的所有imu (图像帧之前的imu数据 + 后一个imu数据)
// 目的是进行IMU的优化???
for (auto &imu_msg : measurement.first)
{
double t = imu_msg->header.stamp.toSec(); // imu时间戳
double img_t = img_msg->header.stamp.toSec() + estimator.td; // 图像时间戳
if (t <= img_t)
{
// 因为最后一个imu时间戳在图像帧时间戳的后面，所以此处表示的是最后一个imu之前的所有imu数据
if (current_time < 0)
current_time = t;
double dt = t - current_time; // 相邻imu数据之间的时间差
ROS_ASSERT(dt >= 0);
current_time = t;
// 取出当前imu的加速度(加速度计数据)
dx = imu_msg->linear_acceleration.x;
dy = imu_msg->linear_acceleration.y;
dz = imu_msg->linear_acceleration.z;
// 取出当前imu的角速度(陀螺仪数据)
rx = imu_msg->angular_velocity.x;
ry = imu_msg->angular_velocity.y;
rz = imu_msg->angular_velocity.z;
// 时间差和imu数据送到后端的processIMU中(进行优化？)
estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
}
// 针对最后一个imu数据，需要做一个简单的线性插值
else
{
// current_time: 上一个imu时间戳 img_t: 图像帧的时间戳 t: 当前imu时间戳
// 排列顺序：current_time -- img_t -- t
// 下面求出彼此之间的时间间隔，便于插值
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);
// 获取线性插值后的imu加速度
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;
// 获取线性插值后的imu角速度
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;
// 将dt_1时间差和插值后的imu数据送进去
estimator.processIMU(dt_1,
Vector3d(dx, dy, dz),
Vector3d(rx, ry, rz));
}
}
// 回环相关部分
sensor_msgs::PointCloudConstPtr relo_msg = NULL;
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; // 回环帧的归一化坐标和地图点idx
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);
}
TicToc t_s;
// 特征点id->特征点信息
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
// 对于前面对齐后得到的img，遍历上面的像素
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 = 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];
ROS_ASSERT(z == 1); // 检查是不是归一化后的
// 组成7维的eigen
Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
// 存入map结构
image[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}
estimator.processImage(image, img_msg->header);
}
}
}
2. 视觉惯性对齐getMeasurements
（1）大致思路
主要是分情况将传入的imu与图像进行过滤，然后返回对齐后的imu和image，具体情况如下：
- 若imu或image只要有一个为空，则无法对齐，直接返回
- 若imu数据与image没有重叠部分，则无法对齐，直接返回
- 若image在imu数据之前开始，则应该丢掉前面的部分image帧
- 若imu数据在image之前开始，则获取在image之前的imu数据，以及在image之后的一个imu数据
（2）代码实现
// 获得匹配好的图像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)
{
// 若imu或视觉feature只要有一个为空，则无法对齐，直接返回
// 这里的imu_buf和feature_buf是在前面imu_callback和feature_callback中获取的
if (imu_buf.empty() || feature_buf.empty())
return measurements;
// 第一个时间在左 最后一个时间在右
// imu *******
// image *****
// 希望最后一个imu数据的时间大于第一个视觉feature的时间+相应的延时时间estimator.td(括号中的)
// 这样可保证imu与视觉feature之间存在重叠部分，从而可以对齐
// 若取反，最后一个imu数据的时间 小于 第一个视觉feature的时间 + 相应的延时时间estimator.td
// 则表明imu数据与视觉feature没有重叠部分，自然就无法对齐
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++; // 等待imu数据，使其与视觉feature存在重叠部分
return measurements;
}
/*
imu ****
image ******
希望第一个imu数据时间 小于 第一个视觉feature的时间 也就是要求imu先开始，然后再有视觉feature
若第一个imu数据时间 大于 第一个视觉feature的时间，则表明视觉feature在imu之前开始，此时应该丢掉前面的部分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");
// 由于在第一帧image之前没有imu数据，处理此时的image没有任何意义，故丢弃掉此时的第一帧image，再进行判断
feature_buf.pop();
continue;
}
// 此时就保证了图像前一定有imu数据
// imu ****************
// img * *
sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front(); // 取出第一个图像帧
feature_buf.pop(); // 取出之后，在feature_buf队列中删除掉该第一帧
// 一般第一帧不会严格对齐，但是后面就都会对齐，当然第一帧也不会用到
std::vector<sensor_msgs::ImuConstPtr> IMUs;
// 从imu_buf中删除掉小于第一个图像帧 + 延时的imu数据，并将这些imu数据放到IMUs中
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_buf中小于第一个图像帧 + 延时的imu数据都被删除掉了
// 而IMUs中不仅存入了这些删掉的imu数据，而且将第一个图像帧的后一个imu数据也存入进来
// 这样IMUs中最后两个imu数据就是第一个图像帧左右两边相邻的imu数据
// 后面取图像帧左右两边相邻imu数据，对其做插值，从而与图像帧进行对齐
// 最终将经过滤后的imu与image放到measurements中，格式大致为:
// imu * * * * * * *
// image *
IMUs.emplace_back(imu_buf.front());
if (IMUs.empty())
ROS_WARN("no imu between two image");
// 取出此时对齐的imu和图像帧数据放到下面的measurements中, 传入外面的process()函数中进行对齐
measurements.emplace_back(IMUs, img_msg);
}
return measurements;
}
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
