本文参考各网站大佬的各种总结讲解,在此致谢。针对程序我自己不明白不清楚的部分,做注释。
本文仅供学习,如果有错误的地方,请大家指点指点
小白VINS-Mono代码学习_1
从feature_tracker_node.cpp开始读整个程序,先看main函数,上来就是 readParameters(n);
void readParameters(ros::NodeHandle &n) 位于parameters.cpp中,下面主要先学习下这段程序
一、 VINS-Mono/feature_tracker/src/parameters.cpp
#include "parameters.h"
std::string IMAGE_TOPIC;
std::string IMU_TOPIC;
std::vector<std::string> CAM_NAMES;
std::string FISHEYE_MASK;
int MAX_CNT;
int MIN_DIST;
int WINDOW_SIZE;
int FREQ;
double F_THRESHOLD;
int SHOW_TRACK;
int STEREO_TRACK;
int EQUALIZE;
int ROW;
int COL;
int FOCAL_LENGTH;
int FISHEYE;
bool PUB_THIS_FRAME;
// ************************************ 从ROS节点中读取参数 ************************************* //
/*首先在launch文件中进行配置:见 /home/tr-vins-mono/catkin_ws/src/VINS-Mono/vins_estimator/launch/euroc_no_extrinsic_param.launch
<launch>
<arg name="config_path" default = "$(find feature_tracker)/../config/euroc/euroc_config_no_extrinsic.yaml" />
<arg name="vins_path" default = "$(find feature_tracker)/../config/../" />
<node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log">
<param name="config_file" type="string" value="$(arg config_path)" />
<param name="vins_folder" type="string" value="$(arg vins_path)" />
</node>
然后在相应的cpp里,按照模板定义一个读取参数的程序。*/
/* 1、函数模板的格式:
template <class 形参名,class 形参名,......> 返回类型 函数名(参数列表)
{
函数体
}
其中template和class是关键字,class可以用typename 关键字代替,在这里typename 和class没区别,<>括号中的参数叫模板形参,
模板形参和函数形参很相像,模板形参不能为空。
一但声明了模板函数就可以用模板函数的形参名声明类中的成员变量和成员函数,即可以在该函数中使用内置类型的地方都可以使用模板形参名。
模板形参需要调用该模板函数时提供的模板实参来初始化模板形参,一旦编译器确定了实际的模板实参类型就称他实例化了函数模板的一个实例
2、类模板的格式为:
template<class 形参名,class 形参名,…> class 类名
{ ... };
*/
template <typename T>
T readParam(ros::NodeHandle &n, std::string name)
{
T ans;
if (n.getParam(name, ans))
{
ROS_INFO_STREAM("Loaded " << name << ": " << ans);
}
else
{
ROS_ERROR_STREAM("Failed to load " << name);
n.shutdown();
}
return ans;
}
// 读配置参数,通过roslaunch文件的参数服务器获得
void readParameters(ros::NodeHandle &n)
{
std::string config_file;
// 首先获得配置文件的路径
config_file = readParam<std::string>(n, "config_file");
// 使用opencv的yaml文件接口来读取文件
cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
// 2.判断是否打开成功
if(!fsSettings.isOpened())
{
std::cerr << "ERROR: Wrong path to settings" << std::endl;
}
std::string VINS_FOLDER_PATH = readParam<std::string>(n, "vins_folder");
// image_topic: "/cam0/image_raw"
fsSettings["image_topic"] >> IMAGE_TOPIC;
// imu_topic: "/imu0"
fsSettings["imu_topic"] >> IMU_TOPIC;
// max feature number in feature tracking // 在特征追踪中的最大特征数
MAX_CNT = fsSettings["max_cnt"];
// min distance between two features // 两个特征之间的最小距离
MIN_DIST = fsSettings["min_dist"];
ROW = fsSettings["image_height"];
COL = fsSettings["image_width"];
// frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
// 发布跟踪结果的频率,良好的估计至少10Hz.如果设为0,则发布频率跟原始图像一致
FREQ = fsSettings["freq"];
// ransac threshold (pixel) // Ransac 阈值(像素)
F_THRESHOLD = fsSettings["F_threshold"];
// publish tracking image as topic // 发布跟踪的图像作为topic
SHOW_TRACK = fsSettings["show_track"];
// 是否做均衡化处理 // 均衡化可以减少图像太暗太亮带来的影响,以致能找到足够的特征
EQUALIZE = fsSettings["equalize"];
// if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
FISHEYE = fsSettings["fisheye"];
if (FISHEYE == 1)
FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg";
CAM_NAMES.push_back(config_file);
WINDOW_SIZE = 20;
STEREO_TRACK = false;
// 焦距
FOCAL_LENGTH = 460;
PUB_THIS_FRAME = false;
if (FREQ == 0)
FREQ = 100;
// .release() 记得不能少
fsSettings.release();
}