本文主要介绍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代码解读
**程序入口 int main(int argc, char argv)
int main(int argc, char **argv)
{
// 1.ROS初始化设置节点vins_estimator，设置句柄n
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();
#ifdef EIGEN_DONT_PARALLELIZE
ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
ROS_WARN("waiting for image and imu...");
//发布用于RVIZ显示的Topic，本模块具体发布的内容详见输入输出
registerPub(n);
// 3.订阅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);
// 4.创建VIO主线程
std::thread measurement_process{process};
ros::spin();
return 0;
}
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);
参数
Estimator estimator;// Estimator类的对象
std::condition_variable con;//条件变量
double current_time = -1;
// 三个buf
queue<sensor_msgs::ImuConstPtr> imu_buf;
queue<sensor_msgs::PointCloudConstPtr> feature_buf;
queue<sensor_msgs::PointCloudConstPtr> relo_buf;
int sum_of_wait = 0;// 等待IMU刷新时间
// 互斥量，锁
std::mutex m_buf;
std::mutex m_state;
std::mutex i_buf;
std::mutex m_estimator;
double latest_time;
//IM U项[P,Q,B,Ba,Bg,a,g]
Eigen::Vector3d tmp_P;
Eigen::Quaterniond tmp_Q;
Eigen::Vector3d tmp_V;
Eigen::Vector3d tmp_Ba;
Eigen::Vector3d tmp_Bg;
Eigen::Vector3d acc_0;
Eigen::Vector3d gyr_0;
// 初始化feature、imu、last_imu_t
bool init_feature = 0;
bool init_imu = 1;
double last_imu_t = 0;
一、void process() 函数处理观测值线程
通过while (true)不断循环，主要功能包括等待并获取measurements，计算dt，然后执行以下函数：
stimator.processIMU()进行IMU预积分
estimator.setReloFrame()设置重定位帧
estimator.processImage()处理图像帧：初始化，紧耦合的非线性优化
其中measurements的数据格式可以表示为：(IMUs, img_msg)s s表示容器（vector）
1.通过getMeasurements（）函数获取imu和相机帧的信息。
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
// 1. 在提取measurements时互斥锁m_buf会锁住，此时无法接收数据;等待measurements上面两个接收数据完成就会被唤醒
std::unique_lock<std::mutex> lk(m_buf);
// 调用wait函数，先解锁lk，然后判断lambda的返回值
con.wait(lk, [&]{return (measurements = getMeasurements()).size() != 0;});
lk.unlock();
std::condition_variable con;//条件变量
有关条件变量和互斥锁unique_lock的内容，可以参考此博客https://blog.csdn.net/try_again_later/article/details/104864189。
这里的意思是，提取measurements时互斥锁m_buf会锁住，直到接受完之后才会唤醒这个线程。
在提取观测值数据的时候用到的互斥锁锁住imu_buf和feature_buf，等提取完成后才释放。
整个过程：回调函数接收数据，接受完一组数据后唤醒提取数据的线程；提取数据线程提取完数据后，回调函数继续接收数据。
2. IMU预积分
对于measurement中的每一个imu_msg，计算dt并执行processIMU()。
processIMU()实现了IMU的预积分，通过中值积分得到当前PQV作为优化初值
m_estimator.lock();
for (auto &measurement : measurements)// 对measurements中的每一个measurement (IMUs,IMG)组合进行操作，遍历获取的Feature和IMU测量值
{
// 2.1 IMU 预积分
auto img_msg = measurement.second;//对应这段的img data
double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
for (auto &imu_msg : measurement.first)// 某一图像帧下遍历对齐的imu
{
double t = imu_msg->header.stamp.toSec();
double img_t = img_msg->header.stamp.toSec() + estimator.td;
//发送IMU数据进行预积分
if (t <= img_t)
{
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;
rx = imu_msg->angular_velocity.x;// 角速度
ry = imu_msg->angular_velocity.y;
rz = imu_msg->angular_velocity.z;
//imu预积分
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
{
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;
//对于measurement中的每一个imu_msg，计算dt并执行processIMU()。
//processIMU()实现了IMU的预积分，通过中值积分得到当前PQV作为优化初值
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函数。
3. 重定位setReloFrame（）
在relo_buf中取出最后一个重定位帧，拿出其中的信息并执行setReloFrame()
// 2.2 重定位
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;
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);
}
4. processImage（）处理图像
建立每个特征点的(camera_id,[x,y,z,u,v,vx,vy])s的map，索引为feature_id
实现了视觉与IMU的初始化以及非线性优化的紧耦合
// 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 = 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);
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);
}
//处理图像特征,这里实现了视觉与IMU的初始化以及非线性优化的紧耦合
estimator.processImage(image, img_msg->header);
5、向RVIZ发布里程计信息、关键位姿、相机位姿、点云和TF关系
//给RVIZ发送topic
pubOdometry(estimator, header);//"odometry" 里程计信息PQV
pubKeyPoses(estimator, header);//"key_poses" 关键点三维坐标
pubCameraPose(estimator, header);//"camera_pose" 相机位姿
pubPointCloud(estimator, header);//"history_cloud" 点云信息
pubTF(estimator, header);//"extrinsic" 相机到IMU的外参
pubKeyframe(estimator);//"keyframe_point"、"keyframe_pose" 关键帧位姿和点云
if (relo_msg != NULL)
pubRelocalization(estimator);//"relo_relative_pose" 重定位位姿
//ROS_ERROR("end: %f, at %f", img_msg->header.stamp.toSec(), ros::Time::now().toSec());
}
m_estimator.unlock();
6、 更新IMU参数[P,Q,V,ba,bg,a,g]
// 3. 更新IMU参数
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();
二、getMeasurements（）函数
1、图像注解
对imu和图像数据对齐组合，直到把缓存中图像数据或者IMU数据读取完，才能跳出函数，返回数据。
注意这里只保证了相邻的feature数据之间有完整的imu数据
主要分为以下几种情况：
1.IMU或者相机帧的buff为空，measurements返回空值。
2.IMU最新的时间戳imu_buf.back() 小于 最旧的图像帧feature_buf.front()+ td，说明IMU数据太少，等待IMU刷新
3.IMU最旧的时间戳imu_buf.front() 大于 图像帧最旧的时间戳feature_buf.front()+ td，最开始只有图像，过了很久来了IMU，但是第一帧图像太旧了，要丢弃第一帧图像。
4.理想：IMU最旧的在图像帧最旧的之前，说明IMU数据足够
图示如下（时间为从左向右流逝）：
1、IMU或者相机帧的buff为空，measurements返回空值。
if (imu_buf.empty() || feature_buf.empty())
t0时刻：两个buff都为空，measurements返回空值
t1时刻：feature_buff为空，measurements返回空值
t0时刻;IMU_buff为空，measurements返回空值
2、IMU最新的时间戳imu_buf.back() 小于 最旧的图像帧feature_buf.front()+ td，
imu_buff 只有1 2 ，在图像帧a的右侧没有imu数据了，说明IMU数据太少，等待IMU刷新
if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td))
t2时刻：feature_buff,imu_buff不为空，但是最新的imu帧（2）没有新过图像帧（a）的时间戳的时候，返回结果为空值
3.IMU最旧的时间戳imu_buf.front() 大于 图像帧最旧的时间戳feature_buf.front()+ td，
最开始只有图像a，过了很久来了IMU 1 2，但是第一帧图像太旧了，要丢弃第一帧图像。
if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td))
t1时刻：IMU_buff,feature_buff都不为空，但是不满足最旧的IMU帧旧过相机帧，因此要把太旧的这个相机帧丢弃，重新开始循环（程序特别有交代说明，这种情况只可能出现在程序的开头处）。
此时由于feature_buff为空，则返回空值的measurements.
4、理想的对齐：IMU最旧的在图像帧最旧的之前，说明IMU数据足够，这是最好情况。在imu_buf中小于该帧的都和该帧一起放进去。
while (imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td)
t3时刻：feature_buff，IMU_buff不为空，且最旧的imu也旧过了图像帧的数据，此时图像buff拿出一个a,IMUs开始装载imu_buf中的1,2，最后装了一个3（每一个图像帧每次都会多装一个imu帧）。之后进入第二次while循环的过程中，图像buff拿出一个b,IMUs开始装载imu_buff中的4,5，最后装了一个6.第三次循环，由于相机buff等于空，因此返回装有数据的数值measurements,此时的measurements装载有两组数据。
代码部分
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)
{ // 1.IMU或feature是空的直接返回空结果
//直到把缓存中的图像特征数据或者IMU数据取完，才能够跳出此函数
if (imu_buf.empty() || feature_buf.empty())
return measurements;
// 2.对齐标准：IMU最后一个数据的时间要大于第一个图像特征数据的时间，否则继续等待，返回空值
// IMU最新的时间戳 小于 最旧的图像帧，说明IMU数据太少，等待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;
}
// 3.对齐标准：不满足 最旧的IMU在最旧的相机帧之前，即相机帧太旧了，要丢弃。保证了图像帧之前一定要有IMU
// 开头，只有图像，后来突然来了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;
// 4.图像数据(img_msg)，对应多组在时间戳内的imu数据,然后塞入measurements
// 理想：IMU最旧的在图像帧最旧的之前，说明IMU数据足够
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;
}
三、3个回调函数
1、队列imu_buf、feature_buf、relo_buf是被多线程共享的，因而在回调函数将相应的msg放入buf或进行pop时，需要设置互斥锁m_buf，在操作前lock()，操作后unlock()。
2、在feature_callback和imu_callback中还设置了条件锁，在完成将msg放入buf的操作后唤醒作用于process线程中的获取观测值数据的函数。
con.notify_one();
在imu_callback中还通过lock_guard的方式构造互斥锁m_state，能够自动加锁、解锁
std::lock_guard<std::mutex> lg(m_state);
imu_callback（）回调函数
将imu_msg保存到imu_buf，IMU状态递推并发布【PVQ，header】
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
// 1. 判断时间间隔是否为正
if (imu_msg->header.stamp.toSec() <= last_imu_t)
{
//ROS_WARN("imu message ins disorder!");
return;
}
last_imu_t = imu_msg->header.stamp.toSec();// 获取最新的imu时间戳
// 2.将imu_msg保存到imu_buf，记得加锁解锁
m_buf.lock();
imu_buf.push(imu_msg);
m_buf.unlock();
// 3.唤醒作用于process线程中的获取观测值数据的函数
con.notify_one();
last_imu_t = imu_msg->header.stamp.toSec();
{
// 构造互斥锁m_state，析构时解锁
std::lock_guard<std::mutex> lg(m_state);
// 4. 递推得到IMU的PQV
predict(imu_msg);
std_msgs::Header header = imu_msg->header;
header.frame_id = "world";
// 5.发布最新的由IMU直接递推得到的PQV
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
}
}
feature_callback（）函数
feature回调函数，将feature_msg放入feature_buf
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
// 1、 判断是否第一帧，光流没有速度
if (!init_feature)
{
//skip the first detected feature, which doesn't contain optical flow speed
init_feature = 1;
return;
}
// 2.feature_msg加入到feature_buf中，记得加锁解锁
m_buf.lock();
feature_buf.push(feature_msg);
m_buf.unlock();
// 3、唤醒作用于process线程中的获取观测值数据的函数
con.notify_one();
}
restart_callback（）
收到restart时清空feature_buf和imu_buf，估计器重置，时间重置
void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
{
if (restart_msg->data == true)
{
ROS_WARN("restart the estimator!");
// 1.清空feature和imu的buf，记得对m_uf加锁解锁
m_buf.lock();
while(!feature_buf.empty())
feature_buf.pop();
while(!imu_buf.empty())
imu_buf.pop();
m_buf.unlock();
// 2.估计器重置，参数重置，记得对m_eastimator加锁解锁
m_estimator.lock();
estimator.clearState();
estimator.setParameter();
m_estimator.unlock();
// 3.时间重置
current_time = -1;
last_imu_t = 0;
}
return;
}
relocalization_callback（）
重定位回调函数，将points_msg放入relo_buf
void relocalization_callback(const sensor_msgs::PointCloudConstPtr &points_msg)
{
//printf("relocalization callback! \n");
m_buf.lock();
relo_buf.push(points_msg);
m_buf.unlock();
}
四、predict（）
离散中值积分：从IMU测量值imu_msg和上一个PVQ递推得到下一个tmp_Q，tmp_P，tmp_V
void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
// 1.先取当前imu时间戳
double t = imu_msg->header.stamp.toSec();
// 如果imu第一个时间戳
if (init_imu)// init_imu=1表示第一个IMU数据
{
latest_time = t;
init_imu = 0;
return;
}
double dt = t - latest_time;// 时间间隔
latest_time = t; //更新上一次imu时间戳
// 2.线性加速度和角速度
double dx = imu_msg->linear_acceleration.x;
double dy = imu_msg->linear_acceleration.y;
double dz = imu_msg->linear_acceleration.z;
Eigen::Vector3d linear_acceleration{dx, dy, dz};
double rx = imu_msg->angular_velocity.x;
double ry = imu_msg->angular_velocity.y;
double rz = imu_msg->angular_velocity.z;
Eigen::Vector3d angular_velocity{rx, ry, rz};
Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;//
Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;// 中值积分的角速度
tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);// Q=Q*[0.5w]
Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;
Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);// 中值积分的加速度
tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;// P=P+0.5*v+0.5*a*t^2
tmp_V = tmp_V + dt * un_acc; // V=V+a*t
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
五、update（）
从估计器中得到滑动窗口当前图像帧的imu更新项[P,Q,V,ba,bg,a,g]
因为imu的频率比图像频率要高很多，在getMeasurements(）将图像和imu时间对齐后，imu_buf中还会存在剩余的imu数据
对imu_buf中剩余的所有imu_msg进行PVQ递推
void update()
{
TicToc t_predict;
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;
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());
扫一扫
5338
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
