目录
1、先通过getMeasurements()函数获取imu和相机帧的信息。
5、向RVIZ发布里程计信息、关键位姿、相机位姿、点云和TF关系
estimator_node.cpp
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;
}
参数
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() |
VIO主线程 |
vector<pair<vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> getMeasurements() |
对imu和图像数据进行对齐并组合 |
Estimator::processIMU()本节末尾 | IMU预积分 |
Estimator::setReloFrame() | 设置重定位帧 |
Estimator::processImage()下一节再讲 | 处理图像帧 |
imu_callback() | imu回调函数,将imu_msg保存到imu_buf, IMU状态递推并发布PVQ,header |
feature_callback() | feature回调函数,将feature_msg放入feature_buf |
restart_callback() | restart回调函数,清空feature_buf和imu_buf,估计器重置,时间重置 |
relocalization_callback() | 重定位回调函数,将points_msg放入relo_buf |
update() | imu_buf中PVQ递推更新 |
predict() | 离散中值积分 从IMU测量值imu_msg和上一个PVQ递推得到下一个 tmp_Q,tmp_P,tmp_V |
一、process() 函数处理观测值线程
等待并获取measurements:(IMUs, img_msg)s,计算dt
estimator.processIMU() 进行IMU预积分
estimator.setReloFrame() 设置重定位帧
estimator.processImage() 处理图像帧:初始化,紧耦合的非线性优化