写在前面的话
终于把feature_tracker这一个node给整理好了，那些都是之前就已经看过的内容，所以整理起来比较快，接下来就慢慢边学边整理吧，这次先来看estimator_node.cpp里main()的内容。
补：在程序中会涉及到很多ROS里定义的数据类型和作者定义的复杂难理解的数据类型，比如sensor_msgs::ImageConstPtr sensor_msgs::PointCloudPtr feature_points 等等，在学习的时候我找到一个帮助理解的参考，如果你也因为这些数据类型而困扰，就点进去看一看吧。
参考连接： https://blog.csdn.net/qq_41839222/article/details/86030962
一、main()函数注释
int main(int argc, char **argv)
{
//[1]初始化，设置句柄，设置logger级别
ros::init(argc, argv, "vins_estimator");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
//【2】从config_file中读取参数，imu话题，Ba,Bg,Na,Ng，最大迭代次数等
readParameters(n);
//【3】优化器设置参数，包括相机IMU外参tic和ric
estimator.setParameter();
#ifdef EIGEN_DONT_PARALLELIZE
ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
ROS_WARN("waiting for image and imu...");
//[4]注册visualization.cpp中创建的发布器registerPub(n)
registerPub(n);
//[5]订阅imu话题，订阅/feature_tracker/feature，/feature_tracker/restart，/pose_graph/match_points等话题,执行回调函数
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);
//[6]创建主线程函数process()，在process()中处理VIO后端，包括IMU预积分、松耦合初始化和local BA
std::thread measurement_process{process};//创建线程measurement_process对象，一创建线程对象就启动线程 运行process
ros::spin();
return 0;
}
其中，步骤[2]的readParameters(n)的函数在paramaters.cpp中，如下：
void readParameters(ros::NodeHandle &n)
{
//[1]读取参数文件euroc_config.yaml
std::string config_file;
config_file = readParam<std::string>(n, "config_file");
cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
//[2]文件是否正确打开判断
if(!fsSettings.isOpened())
{
std::cerr << "ERROR: Wrong path to settings" << std::endl;
}
//[3]读取相应参数
fsSettings["imu_topic"] >> IMU_TOPIC;//imu_topic: "/imu0"
SOLVER_TIME = fsSettings["max_solver_time"];//solver的最大迭代时间0.04ms
NUM_ITERATIONS = fsSettings["max_num_iterations"];//最大迭代次数8
MIN_PARALLAX = fsSettings["keyframe_parallax"];//最小视差10pixel
MIN_PARALLAX = MIN_PARALLAX / FOCAL_LENGTH;//最小视差=最小视差/焦距=10.0/460.0
//[4]设置输出路径
std::string OUTPUT_PATH;
fsSettings["output_path"] >> OUTPUT_PATH;
VINS_RESULT_PATH = OUTPUT_PATH + "/vins_result_no_loop.csv";
std::cout << "result path " << VINS_RESULT_PATH << std::endl;
FileSystemHelper::createDirectoryIfNotExists(OUTPUT_PATH.c_str());//如果文件不存在，就创建一个
std::ofstream fout(VINS_RESULT_PATH, std::ios::out);
fout.close();
//【5】继续读取参数
ACC_N = fsSettings["acc_n"];//加速度计的noise
ACC_W = fsSettings["acc_w"];//加速度计的bias
GYR_N = fsSettings["gyr_n"];//陀螺仪的noise
GYR_W = fsSettings["gyr_w"];//陀螺仪的bias
G.z() = fsSettings["g_norm"];//重力
ROW = fsSettings["image_height"];
COL = fsSettings["image_width"];
ROS_INFO("ROW: %f COL: %f ", ROW, COL);
//【6】读取imu和camera的外参，根据读取的参数执行相应的操作
ESTIMATE_EXTRINSIC = fsSettings["estimate_extrinsic"];
if (ESTIMATE_EXTRINSIC == 2)//表示完全让算法标定imu和相机之间的外参
{
ROS_WARN("have no prior about extrinsic param, calibrate extrinsic param");
RIC.push_back(Eigen::Matrix3d::Identity());//给RIC赋值为单位阵
TIC.push_back(Eigen::Vector3d::Zero());//给TIC赋值为另矩阵
EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv";
}
else
{
if ( ESTIMATE_EXTRINSIC == 1)//表示我们提供一个初值，算法仅对初值进行优化
{
ROS_WARN(" Optimize extrinsic param around initial guess!");
EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv";
}
if (ESTIMATE_EXTRINSIC == 0)//表示算法信任我们提供的外参，不对其做任何修改
ROS_WARN(" fix extrinsic param ");
cv::Mat cv_R, cv_T;
fsSettings["extrinsicRotation"] >> cv_R;//imu与camera的旋转外参
fsSettings["extrinsicTranslation"] >> cv_T;//imu与camera的平移外参
Eigen::Matrix3d eigen_R;
Eigen::Vector3d eigen_T;
cv::cv2eigen(cv_R, eigen_R);//opencv的数据结构转eigen
cv::cv2eigen(cv_T, eigen_T);
Eigen::Quaterniond Q(eigen_R);
eigen_R = Q.normalized();//四元数归一化咋就转成Matrix3d啦？
RIC.push_back(eigen_R);
TIC.push_back(eigen_T);
ROS_INFO_STREAM("Extrinsic_R : " << std::endl << RIC[0]);
ROS_INFO_STREAM("Extrinsic_T : " << std::endl << TIC[0].transpose());
}
//[7]给参数赋初始值
INIT_DEPTH = 5.0;
BIAS_ACC_THRESHOLD = 0.1;
BIAS_GYR_THRESHOLD = 0.1;
//[8]继续读取参数TD，图像和IMU数据在时间上的偏移量，这里配置文件中为0.0
TD = fsSettings["td"];//0 initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
ESTIMATE_TD = fsSettings["estimate_td"];//0
if (ESTIMATE_TD)
ROS_INFO_STREAM("Unsynchronized sensors, online estimate time offset, initial td: " << TD);
else
ROS_INFO_STREAM("Synchronized sensors, fix time offset: " << TD);
ROLLING_SHUTTER = fsSettings["rolling_shutter"];//0是全局快门，1是卷帘快门
if (ROLLING_SHUTTER)
{
TR = fsSettings["rolling_shutter_tr"];//这个时间默认设置的是0
ROS_INFO_STREAM("rolling shutter camera, read out time per line: " << TR);
}
else
{
TR = 0;
}
fsSettings.release();
}
步骤【4】中的registerPub(ros::NodeHandle &n)函数在utility–>visualization.cpp中，注释如下：
void registerPub(ros::NodeHandle &n)
{
//[1]发布相关topic
pub_latest_odometry = n.advertise<nav_msgs::Odometry>("imu_propagate", 1000);//1000指队列长度
pub_path = n.advertise<nav_msgs::Path>("path", 1000);
pub_relo_path = n.advertise<nav_msgs::Path>("relocalization_path", 1000);
pub_odometry = n.advertise<nav_msgs::Odometry>("odometry", 1000);
pub_point_cloud = n.advertise<sensor_msgs::PointCloud>("point_cloud", 1000);
pub_margin_cloud = n.advertise<sensor_msgs::PointCloud>("history_cloud", 1000);
pub_key_poses = n.advertise<visualization_msgs::Marker>("key_poses", 1000);
pub_camera_pose = n.advertise<nav_msgs::Odometry>("camera_pose", 1000);
pub_camera_pose_visual = n.advertise<visualization_msgs::MarkerArray>("camera_pose_visual", 1000);
pub_keyframe_pose = n.advertise<nav_msgs::Odometry>("keyframe_pose", 1000);
pub_keyframe_point = n.advertise<sensor_msgs::PointCloud>("keyframe_point", 1000);
pub_extrinsic = n.advertise<nav_msgs::Odometry>("extrinsic", 1000);
pub_relo_relative_pose= n.advertise<nav_msgs::Odometry>("relo_relative_pose", 1000);
//[2]可视化的相关设置
cameraposevisual.setScale(1);
cameraposevisual.setLineWidth(0.05);
keyframebasevisual.setScale(0.1);
keyframebasevisual.setLineWidth(0.01);
}
接下来就是对四个回调函数的处理了。。。
二、 imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)回调函数处理
注释如下：
{
//[1]当前imu采样时间小于上次imu采样时间，说明有bug,跳出此条件。
if (imu_msg->header.stamp.toSec() <= last_imu_t)//初值last_imu_t=0
{
ROS_WARN("imu message in disorder!");
return;
}
last_imu_t = imu_msg->header.stamp.toSec();
//[2]在修改多个线程共享的变量的时候进行上锁，防止多个线程同时访问该变量
m_buf.lock();
imu_buf.push(imu_msg);//将IMU数据保存到imu_buf中
m_buf.unlock();
con.notify_one();//唤醒作用于process线程中的获取观测值数据的函数
last_imu_t = imu_msg->header.stamp.toSec();
//这个括弧是啥写法？
{
//[3]预测tmp_P,tmp_V,tmp_Q
std::lock_guard<std::mutex> lg(m_state);//模板类std::lock_guard，在构造时就能提供已锁的互斥量，并在析构的时候进行解锁,在std::lock_guard对象构造时，传入的mutex对象(即它所管理的mutex对象)会被当前线程锁住
predict(imu_msg);
//[4]如果solver_flag为非线性优化，发布最新的里程计消息
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);
}
}
在imu_callback( )函数中又调用了predict( )函数来获取临时的P,V,Q值，来看一下predict( )函数的注释：
void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
double t = imu_msg->header.stamp.toSec();
//[1]判断是否是第一帧imu_msg
if (init_imu) //if语句里的return，使程序跳出if所在的函数，返回到母函数中继续执行。
{
latest_time = t;
init_imu = 0;
return;
}
//[2]计算当前imu_msg距离上一个imu_msg的时间间隔
double dt = t - latest_time;
latest_time = t;
//[3]取x,y,z三个方向上的线加速度
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};
//【4】获取x,y,z三个方向上的角速度
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};
//【5】tmp_P,tmp_V,tmp_Q更新,这一部分有公式推导 ，利用的是中值积分
Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;//a理想值=R(a测量值-Ba)-g w理想值=w测量值-BW 此式为k时刻的加速度的模型
Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;//中值法：取k时刻和k+1时刻的角速度再除2
tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);//旋转的更新
Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;//k+1时刻的加速度
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;
tmp_V = tmp_V + dt * un_acc;
//【6】迭代赋值
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
此外，还调用了pubLatestOdometry（）发布P、V、Q值，此函数在visualization.cpp文件中。
三、feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)注释
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
//[1]判断是否是第一个特征，如果是第一个特征直接跳过
if (!init_feature)//init_feature=0
{
//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();
con.notify_one();//唤醒process线程
}
四、restart_callback(const std_msgs::BoolConstPtr &restart_msg)注释
void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
{
if (restart_msg->data == true)
{
ROS_WARN("restart the estimator!");
//[1]当feature_buf和imu_buf非空时，循环删除队首元素
m_buf.lock();
while(!feature_buf.empty())
feature_buf.pop();
while(!imu_buf.empty())
imu_buf.pop();
m_buf.unlock();
//[2]清除估计器的状态和参数，时间重置
m_estimator.lock();
estimator.clearState();
estimator.setParameter();
m_estimator.unlock();
current_time = -1;
last_imu_t = 0;
}
return;
}
重定位的回调函数就不贴过来了。。。因为就两三句，没啥好贴的。。。
下面还剩下process( )函数，这个函数里面又调用了getMeasurements( )、processIMU( )、setReloFrame( )、processImage( ),还有一些发布数据的函数，是一个比较大的事儿，写在下一节吧。。。朋友们，再见！
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
