目录
目录
上一节笔记的链接：
一、特征点
首先更具上一次笔记的内容，我们已经可以启动运行程序了。我再打开一个终端输入可以看见目前的节点信息。我们来看一下前端节点代表的什么含义。
rqt_graph从下图可以看见节点以可视化的方式展现： 
| /cam0/image_raw | 原始图片话题 |
| /feature_tracker | 前端节点 |
| /feature_tracker/feature_img | 给RVIZ可视化 |
| /feature_tracker/restart | 重启消息并且发给后端 |
| /feature_tracker/feature | 将特征点的消息发给后端 |
那/feature_tracker/feature这个话题发送的消息有那些呢？
1、像素坐标->特征点提取算法
2、去畸变后归一化坐标->特征点去畸变算法
3、特征点id->光流最终算法（三角化对应三维坐标序号）
4、特征点速度
二、feature_tracker_node
经过前面的简单的介绍，我们开始对了解代码，首先找到feature_tracker_node.cpp文件，我们从main函数开始
ros::init(argc, argv, "feature_tracker");
ros::NodeHandle n("~");//~表示一个节点的命名空间
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
readParameters(n);//读配置文件readParameters为读取参数函数，我们进去入函数查看该函数在parameters.cpp中。本函数的主要功能是读取yaml中的config_file的文件路径之后讲需要的参数提取出来。euroc_config.yaml中就是程序和相机的参数。
<arg name="config_path" default = "$(find feature_tracker)/../config/euroc/euroc_config.yaml" />这里简单介绍一下FileStorage类，该类为opencv中用来读取yaml文件的类。通过该类可以实现读取和写入yaml文件的参数。一下为opencv官网的介绍：
cv::FileStorage::FileStorage ( const String & filename, int flags, const String & encoding = String())
其中filename为文件路径，flags有一下模式定义：
READ value, open the file for reading
WRITE value, open the file for writing
APPEND value, open the file for appending
MEMORY flag, read data from source or write data to the internal buffer (which is returned by FileStorage::release)
FORMAT_MASK mask for format flags
FORMAT_AUTO flag, auto format
FORMAT_XML flag, XML format
FORMAT_YAML flag, YAML format
FORMAT_JSON flag, JSON format
BASE64 flag, write rawdata in Base64 by default. (consider using WRITE_BASE64)
WRITE_BASE64 flag, enable both WRITE and BASE64
从程序可以看出我们读取yaml文件只读。本类在之后程序中也会使用，所以在这里简单介绍一下使用该类读取：
//读取信息
string strSettingsFile = "./setting.yaml";
cv::FileStorage fread(strSettingsFile.c_str(),cv::FileStorage::READ);
float fxread,fyread;
fread["fx"]>>fxread;
fread["fy"]>>fyread;
//写入
//1.创建
cv::FileStorage fwrite("./setting.yaml",cv::FileStorage::WRITE);
//2.写入数据
float fx = 100.0;
float fy = 101.0;接下来看程序代码：
void readParameters(ros::NodeHandle &n)
{
std::string config_file;
config_file = readParam<std::string>(n, "config_file");//读取config_file的路径
cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);//使用opencv的接口打开yaml文件
if(!fsSettings.isOpened())
{
std::cerr << "ERROR: Wrong path to settings" << std::endl;
}
std::string VINS_FOLDER_PATH = readParam<std::string>(n, "vins_folder");
fsSettings["image_topic"] >> IMAGE_TOPIC;//图片的话题
fsSettings["imu_topic"] >> IMU_TOPIC;//imu的话题
MAX_CNT = fsSettings["max_cnt"];//单帧图片最大特征点的数目
MIN_DIST = fsSettings["min_dist"];//两个特征点直接最短的像素距离
ROW = fsSettings["image_height"];//图像的高度
COL = fsSettings["image_width"];//图像的宽度
FREQ = fsSettings["freq"];
F_THRESHOLD = fsSettings["F_threshold"];//求解对极约束的ransac求解的inlier们限值
SHOW_TRACK = fsSettings["show_track"];//是否可视化
EQUALIZE = fsSettings["equalize"];//是否均衡化
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;
fsSettings.release();
}三、读取相机参数
接下来回到主函数，可以看见下面代码为循环读取相机的参数，实际中NUM_OF_CAM已经写死了所以只读一个相机的参数。
for (int i = 0; i < NUM_OF_CAM; i++)//读取相机默认单目相机
trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);接下来我们进入readIntrinsicParameter函数：
/// @brief 读取相机的参数
/// @param calib_file 配置文件的路径
void FeatureTracker::readIntrinsicParameter(const string &calib_file)
{
ROS_INFO("reading paramerter of camera %s", calib_file.c_str());//输出相机的参数
m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);
}generateCameraFromYamlFile函数是本函数的核心，读取相机文件的路径。再进去改函数查看，进入函数就首先判断是否为针孔模型，我们主要讨论的也是针孔模型。如果是真孔模型则生成真孔模型的对象，之后进入下面的switch语句。
/// @brief 读取相机的参数
/// @param filename 读取yaml文件的路径
/// @return
CameraPtr
CameraFactory::generateCameraFromYamlFile(const std::string& filename)
{
cv::FileStorage fs(filename, cv::FileStorage::READ);
if (!fs.isOpened())
{
return CameraPtr();
}
Camera::ModelType modelType = Camera::MEI;
if (!fs["model_type"].isNone())
{
std::string sModelType;
fs["model_type"] >> sModelType;//获取相机的类型
//以下为相机类型
if (boost::iequals(sModelType, "kannala_brandt"))
{
modelType = Camera::KANNALA_BRANDT;
}
else if (boost::iequals(sModelType, "mei"))
{
modelType = Camera::MEI;
}
else if (boost::iequals(sModelType, "scaramuzza"))
{
modelType = Camera::SCARAMUZZA;
}
else if (boost::iequals(sModelType, "pinhole"))//最常用的针孔相机
{
modelType = Camera::PINHOLE;
}
else
{
std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl;
return CameraPtr();
}
}
switch (modelType)//更具不同的相机模型生成不同的对象
{
case Camera::KANNALA_BRANDT:
{
EquidistantCameraPtr camera(new EquidistantCamera);
EquidistantCamera::Parameters params = camera->getParameters();
params.readFromYamlFile(filename);
camera->setParameters(params);
return camera;
}
//针孔模型
case Camera::PINHOLE:
{
PinholeCameraPtr camera(new PinholeCamera);//生成相机模型
PinholeCamera::Parameters params = camera->getParameters();
params.readFromYamlFile(filename);//读取相机的参数
camera->setParameters(params);//讲读取的参数传入相机对象中
return camera;
}
case Camera::SCARAMUZZA:
{
OCAMCameraPtr camera(new OCAMCamera);
OCAMCamera::Parameters params = camera->getParameters();
params.readFromYamlFile(filename);
camera->setParameters(params);
return camera;
}
case Camera::MEI:
default:
{
CataCameraPtr camera(new CataCamera);
CataCamera::Parameters params = camera->getParameters();
params.readFromYamlFile(filename);
camera->setParameters(params);
return camera;
}
}
return CameraPtr();
}首先生成一个针孔相机模型，readFromYamlFile首先读取相机文件的参数，
/// @brief 读取相机的内参等信息
/// @param filename yaml文件的路径
/// @return
bool
PinholeCamera::Parameters::readFromYamlFile(const std::string& filename)
{
cv::FileStorage fs(filename, cv::FileStorage::READ);
if (!fs.isOpened())
{
return false;
}
if (!fs["model_type"].isNone())//判断是否为针孔模型
{
std::string sModelType;
fs["model_type"] >> sModelType;
if (sModelType.compare("PINHOLE") != 0)
{
return false;
}
}
m_modelType = PINHOLE;
fs["camera_name"] >> m_cameraName;//读取相机名称
m_imageWidth = static_cast<int>(fs["image_width"]);//获取相机长宽
m_imageHeight = static_cast<int>(fs["image_height"]);
cv::FileNode n = fs["distortion_parameters"];//获取相机畸变参数
m_k1 = static_cast<double>(n["k1"]);
m_k2 = static_cast<double>(n["k2"]);
m_p1 = static_cast<double>(n["p1"]);
m_p2 = static_cast<double>(n["p2"]);
n = fs["projection_parameters"];//读取相机的内参
m_fx = static_cast<double>(n["fx"]);
m_fy = static_cast<double>(n["fy"]);
m_cx = static_cast<double>(n["cx"]);
m_cy = static_cast<double>(n["cy"]);
return true;
}之后将读取的params对象设置setParameters到相机模型对象中：
/// @brief 讲读取的相参数传给相机对象
/// @param parameters 真孔相机类
void
PinholeCamera::setParameters(const PinholeCamera::Parameters& parameters)
{
mParameters = parameters;
if ((mParameters.k1() == 0.0) &&
(mParameters.k2() == 0.0) &&
(mParameters.p1() == 0.0) &&
(mParameters.p2() == 0.0))//如果不带畸变
{
m_noDistortion = true;//无畸变标志位为true
}
else
{
m_noDistortion = false;
}
//为后续去畸变做预处理
m_inv_K11 = 1.0 / mParameters.fx();
m_inv_K13 = -mParameters.cx() / mParameters.fx();
m_inv_K22 = 1.0 / mParameters.fy();
m_inv_K23 = -mParameters.cy() / mParameters.fy();
}四、main函数
这样我们就完成了对相机参数的读取工作之后再回到Main函数，判断是否是鱼眼相机。我们主要讨论针孔相机所以这里跳过。之后订阅相机的话题，并且注册消息的发布着。之前用rqt_graph中我们也清晰的看见确实订阅和发布了这几个话题。想了解我话题通讯的同学可以看我写的另一个博客：
//先roscore注册订阅这个topic，收到一次message就执行一个回调函数
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
//注册消息的发送者
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
pub_restart = n.advertise<std_msgs::Bool>("restart",1000);
/*
if (SHOW_TRACK)
cv::namedWindow("vis", cv::WINDOW_NORMAL);
*/
ros::spin();
subscribe(IMAGE_TOPIC, 100, img_callback);为ros中的函数， IMAGE_TOPIC为相机的话题，100是定义的缓存，img_callback是每次接受到话题的回调函数。这个函数是本.cpp的重点。
到此为止我们就看完了main函数，接下来我们来看回调函数，这是本章的重点。
img_callback首先会接受到话题传送过来的图像则调用一次会回调函数。首先一开始，先判断是否是第一帧图像，如果是则复制一下时间戳。并且将标志位置为false，并且挑出函数等待下一帧图像的发送。
当输入为第二帧图像需要判断是否超过1s，主要是为了不影响光流匹配（因为光流追踪图片差距 不能太大）。如果超过时间则重新从第一张图片开始，并且发布复位指令。
if(first_image_flag)//判断是否是第一帧图像
{
first_image_flag = false;
first_image_time = img_msg->header.stamp.toSec();//复制相应的时间错
last_image_time = img_msg->header.stamp.toSec();
return;
}
// detect unstable camera stream
//第二帧的判断
//失败的case1:上以帧和当前帧的时间不超过1秒，如果超过则会影响光流匹配
//case2:时间撮错乱当前时间小于上一时间
if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)
{
ROS_WARN("image discontinue! reset the feature tracker!");
first_image_flag = true; //重新从第一张开始
last_image_time = 0;
pub_count = 1;
std_msgs::Bool restart_flag;
restart_flag.data = true;
pub_restart.publish(restart_flag);//发布给后端复位指令
return;
}如果满足上面两个条件，则记录一下当前图片的时间戳。并且需要判断一下频率。pub_count为接受图片的数目，pub_count / (img_msg->header.stamp.toSec() - first_image_time))就是
图片数目/（当前时间-上一个时间）=发送图片频率
作者希望发送的频率大概在10hz左右为了保证实时性。在if里面还有一个判断，如果当前频率已经很接近预设频率了。相机有可能突然帧率升高。为了不让后端接收过多的帧需要重新设置pub_count，first_image_time重置为当前时间。否则PUB_THIS_FRAME=false不进入光流追踪。
//记录当前时间
last_image_time = img_msg->header.stamp.toSec();
// frequency control
//控制送给后端的频率，大概为10hz 主要是为了保证实时性
//pub_count表示发出多少帧img_msg->header.stamp.toSec() - first_image_time表示时间间隔
if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{
PUB_THIS_FRAME = true;//光流需要连续两帧的变化小，连续两帧还需要做
// reset the frequency control
//如果这段时间的频率和预设频率十分接近，为了不让后端接收过多帧需要重置pub_count，first_image_time重置为当前时间
if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
{
first_image_time = img_msg->header.stamp.toSec();
pub_count = 0;
}
}
else
PUB_THIS_FRAME = false;我们通过获取当前帧时间戳来判断是否可以进行光流追踪，接下来对输入的图片进行处理，ROS中的图片需要通过cv_bridge进过转化才能给opencv使用。我们需要判断一下是否是灰度图片，这里作者重新申请了一个sensor_msgs::Image来复制sensor_msgs::ImageConstPtr的信息，其实toCvCopy第一个参数可以直接接收
//通过cb_brigde转化到opencv中
cv_bridge::CvImageConstPtr ptr;
//如果图片是一通道图片
if (img_msg->encoding == "8UC1")
{
sensor_msgs::Image img;
//读取图片信息
img.header = img_msg->header;
img.height = img_msg->height;
img.width = img_msg->width;
img.is_bigendian = img_msg->is_bigendian;
img.step = img_msg->step;
img.data = img_msg->data;
img.encoding = "mono8";
ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
}
else
ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);cvbrdige的讲解可翻看笔者的之前博客
CvBridge在ROS图像和OpenCV图像之间进行转化_每日亿学的博客-CSDN博客
接下来就是重点！！
五、光流法
首先接收cvbridge传递过来的图片。之后进入循环，可以看见for循环的NUM_OF_CAM为1表示只有一个镜头。下面判断如果不是双目图片则读取图像
cv::Mat show_img = ptr->image;//ptr->image转化到opencv的mat类
TicToc t_r;//计算时间类
for (int i = 0; i < NUM_OF_CAM; i++)
{
ROS_DEBUG("processing camera %d", i);
if (i != 1 || !STEREO_TRACK)//如果不是是双目相机
//readImage(读取图片，和时间搓)
trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());进入readimage函数，本函数就是前端主要处理的内容。处理的基本步骤是：
1、图像均衡化
2、光流追踪
3、提取新的特征点
4、所有特征点去畸变
首先介绍一下feature_tracker类的成员变量的含义：
cv::Mat cur_img, forw_img;//cur_img之前图像，forw_img当前图像
vector<cv::Point2f> , cur_pts, forw_pts;//cur_pts之前的关键点 forw_pts当前关键点
在这里你可能发现和变量命名完全不是一个意思啊？我们看一下代码
EQUALIZE是从yaml文件读取是否需要图像均衡化主要是为了减少光照变化的影响。如果选择均衡化则执行。如果不选择均衡化则使用原图，这里均衡化的代码是直接调用opencv中的函数。本函数是自适应的生成均衡化图像,第一个参数为对比阈值，第二个参数直方图均衡化的网格大小。输入图像将被分成大小相等的矩形块。之后使用CLAHE类中的apple()成员函数使用对比度受限自适应直方图均衡化来均衡灰度图像的直方图。
Ptr<CLAHE> cv::createCLAHE ( double clipLimit = 40.0,Size tileGridSize = Size(8, 8))
if (EQUALIZE)//图像均衡化
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
TicToc t_c;
clahe->apply(_img, img);
ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
}
else//使用原图像
img = _img;
if (forw_img.empty())//如果当前图像为空
{
prev_img = cur_img = forw_img = img;//则当前之前和现在图像都复制为目前图像，浅复制
}
else
{
forw_img = img;//否则更新
}这里 forw_img为什么为当前帧，从上面代码可以看出，如果判断当前帧为空则之前和当前都复制传入的图像。如果不为空cur_img则一定为之前图像。不知道为什么作者如此命名有点容易误解。
接下来，为当前特征点清除，并且判断上一帧是否有特征点。这里光流追踪使用的为opencv的api函数。函数解释如下：
void cv::calcOpticalFlowPyrLK ( InputArray prevImg, InputArray nextImg, InputArray prevPts, InputOutputArray nextPts, OutputArray status, OutputArray err, Size winSize = Size(21, 21),int maxLevel = 3,TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01),int flags = 0,double minEigThreshold = 1e-4)
各个参数的含义：
| prevImg | 上一帧图像 |
| nextImg | 当前帧图像 |
| prevPts | 上一帧特征点 |
| nextPts | 当前帧特征点 |
| status | 如果上一帧和当前帧特征点找到匹配关系则为1反之 |
| err | 错误输出的向量，本代码未使用到 |
| winSize | 每个金字塔搜索窗口的大小 |
| maxLevel | 金字塔的层数 |
| criteria | 指定算法搜索条件 |
| flags | 这个标志位可以不要考虑 |
| minEigThreshold | 最小阈值 |
之后我们开始循环当前的特征点，首先判断是否匹配成功，如果成功但是不在图像边界内则将状态为置为0；inBorder成员函数很简单笔者就不介绍了。之后就更具更新的状态剔除其他变量的值。
//forw_pts为当前帧特征点的信息，需要提前清空
forw_pts.clear();
if (cur_pts.size() > 0)//上一帧有特征点，就可以进行光流追踪
{
TicToc t_o;
vector<uchar> status;
vector<float> err;
//Step 1 通过opencv光流最终的状态剔除outlier
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
for (int i = 0; i < int(forw_pts.size()); i++)//当前特征点数目进行循环
if (status[i] && !inBorder(forw_pts[i]))//是否匹配成功& 是否在图像范围内
status[i] = 0;//不在图像范围内则修改状态
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(ids, status);
reduceVector(cur_un_pts, status);//去畸变的坐标
reduceVector(track_cnt, status);//追踪次数
ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
}reduceVector是用来修剪变量的函数：本函数有两个重载，都是更具状态值来修剪变量。这个函数设计的很巧妙是一个双指针函数。以下为图加文字的解释，本函数无需要开辟新的空间，空间复杂度为0，而且只使用了一个循环o(1)复杂度，是最优解！
首先i=0 状态为1 则v[0]=v[0] j++;j=1;i++;i=1
i=1 状态为0 则跳过；i++;i=2 j=1
i=2 状态为0则跳过；i++;i=3 j=1
i=3状态为1则；v[3]=v[1];j++;j=2;i++;i=4...依次循环
/// @brief 通过新的状态来更改v中的内容,空间复杂度为0时间复杂度为O(1)
/// @param v 输入修改的向量
/// @param status 更新的状态向量
void reduceVector(vector<cv::Point2f> &v, vector<uchar> status)
{
int j = 0;
for (int i = 0; i < int(v.size()); i++)
if (status[i])
v[j++] = v[i];
v.resize(j);
}
void reduceVector(vector<int> &v, vector<uchar> status)
{
int j = 0;
for (int i = 0; i < int(v.size()); i++)
if (status[i])
v[j++] = v[i];
v.resize(j);
}六、剔除特征点
如何来判断检测到的特征点是否为可用的特征点？作者使用2种方法来剔除坏的特征点。
1、光流剔除
2、通过对极约束，排除特征点
接下来讲解的介绍用过对极约束剔除，rejectWithF()的作用就是剔除特征点。
if (PUB_THIS_FRAME)
{
//step2 通过对极约束，排除特征点
rejectWithF();进入函数首先，看过视觉slam十四讲的知道8点法，对极约束至少需要8个点。满足8个点后，创建2个去畸变的特征点对象，un_cur_pts和un_forw_pts。之后使用opencv的函数findFundamentalMat来计算本质矩阵。我们来看这个函数的参数。
Mat cv::findFundamentalMat ( InputArray points1, InputArray points2, int method, double ransacReprojThreshold, double confidence, int maxIters, OutputArray mask = noArray())
points1: 为上一帧特征点
points2：为下一帧特征点
method：计算基础矩阵的方法作者使用用于 RANSAC 算法
ransacReprojThreshold：仅用于 RANSAC 和 LMedS 方法的参数。它指定了估计矩阵正确的理想置信度（概率）水平。
mask：我们需要剔除的点
之后再通过reduceVector()函数剔除错误的特征点。
void FeatureTracker::rejectWithF()
{
//对极约束最少需要8个点
if (forw_pts.size() >= 8)
{
ROS_DEBUG("FM ransac begins");
TicToc t_f;
vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
Eigen::Vector3d tmp_p;
//去畸变输入一个图像坐标得到相机坐标（归一化）系下的坐标
m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);
tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());//去畸变的像素坐标,上一帧
m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);
tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());//当前帧
}
vector<uchar> status;
//使用opencv接口计算本质矩阵，本质上也是对outlier的剔除
cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
int size_a = cur_pts.size();
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(cur_un_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);
ROS_DEBUG("FM ransac costs: %fms", t_f.toc());
}
}七、特征点均匀化
玩出特征点的剔除之后，为了防止特征点扎堆。作者使用了一个非常简单的方法来进行均匀化处理。首先进入函数：
//step3 特征点均匀化
setMask();首先进入函数判断是否为鱼眼相机，我们这里不考虑鱼眼相机。
1、作者先创建一个原图相同大小的图片。并且整张图都为255大小。
2、创建一个vector<pair<int, pair<cv::Point2f, int>>>对象，第一个为跟踪次数，第二个pair为特征点坐标和id
3、通过跟踪次数来排序，如果跟踪次数越多则排越前。（跟踪次数越多说明特征点跟稳定）
4、然后判断特征点在创建的mask图中是否为255，是则将附件的一个MIN_DIST半径区域画一个圆。圆内值都为0而且为实心圆，这样下次如果循环到该特征点附件的特征点，进入后mask的值为0就会跳出。这样画圆就防止该圆内还会出现特征点。
void FeatureTracker::setMask()
{
//tag 不考虑是鱼眼相机
if(FISHEYE)
mask = fisheye_mask.clone();
else
mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));
// prefer to keep features that are tracked for long time
// 第一个int为跟踪次数，第二个pair中第一个为特征点坐标，第二个为id
vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;
//当前特征点数目进行循环
for (unsigned int i = 0; i < forw_pts.size(); i++)
cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i])));
//进行排序如果追踪次数越多则排在前面
sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair<int, pair<cv::Point2f, int>> &a, const pair<int, pair<cv::Point2f, int>> &b)
{
return a.first > b.first;
});
forw_pts.clear();
ids.clear();
track_cnt.clear();
for (auto &it : cnt_pts_id)
{
if (mask.at<uchar>(it.second.first) == 255)
{
//把调选剩下的特征点重新放入容器
forw_pts.push_back(it.second.first);
ids.push_back(it.second.second);
track_cnt.push_back(it.first);
//在以当前特征点为圆形 以MIN_DIST为半径画圆，并且填充为0 下一次这个圈内的特征点都无法访问
cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
}
}
}后面的代码就很简单了，后面使用设置的最大特征点-当前特征点数目，判断特征点的数目是否足够。如果不足够的化就使用goodFeaturesToTrack在补充特征点。该函数是计算Harris角点和shi-tomasi角点。函数很简单去官网查看OpenCV: Feature Detection
之后新加入的点也要加入到对象中，使用addPoints()。
int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
if (n_max_cnt > 0)
{
if(mask.empty())
cout << "mask is empty " << endl;
if (mask.type() != CV_8UC1)
cout << "mask type wrong " << endl;
if (mask.size() != forw_img.size())
cout << "wrong size " << endl;
//新提特征点保证特征点数目足够 调用opencv函数，只有发布才可以提取特征点，同时避免提取的点进mask
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
}以下为完整的代码：
/// @brief
/// @param _img 输入图像
/// @param _cur_time 图像的时间戳
/*
1、图像均衡化处理
2、光流追踪
3、提取新的特征点（如果发布）
4、所有特征点去畸变，计算速度
*/
void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
cv::Mat img;
TicToc t_r;
cur_time = _cur_time;
if (EQUALIZE)//图像均衡化
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
TicToc t_c;
clahe->apply(_img, img);
ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
}
else//使用原图像
img = _img;
if (forw_img.empty())//如果当前图像为空
{
prev_img = cur_img = forw_img = img;//则当前之前和现在图像都复制为目前图像，浅复制
}
else
{
forw_img = img;//否则更新
}
//forw_pts为当前帧特征点的信息，需要提前清空
forw_pts.clear();
if (cur_pts.size() > 0)//上一帧有特征点，就可以进行光流追踪
{
TicToc t_o;
vector<uchar> status;
vector<float> err;
//Step 1 通过opencv光流最终的状态剔除outlier
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
for (int i = 0; i < int(forw_pts.size()); i++)//当前特征点数目进行循环
if (status[i] && !inBorder(forw_pts[i]))//是否匹配成功& 是否在图像范围内
status[i] = 0;//不在图像范围内则修改状态
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(ids, status);
reduceVector(cur_un_pts, status);//去畸变的坐标
reduceVector(track_cnt, status);//追踪次数
ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
}
for (auto &n : track_cnt)
n++;
//如果要发布出去
if (PUB_THIS_FRAME)
{
rejectWithF();
ROS_DEBUG("set mask begins");
TicToc t_m;
setMask();
ROS_DEBUG("set mask costs %fms", t_m.toc());
ROS_DEBUG("detect feature begins");
TicToc t_t;
int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
if (n_max_cnt > 0)
{
if(mask.empty())
cout << "mask is empty " << endl;
if (mask.type() != CV_8UC1)
cout << "mask type wrong " << endl;
if (mask.size() != forw_img.size())
cout << "wrong size " << endl;
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
}
else
n_pts.clear();
ROS_DEBUG("detect feature costs: %fms", t_t.toc());
ROS_DEBUG("add feature begins");
TicToc t_a;
addPoints();
ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
}
prev_img = cur_img;
prev_pts = cur_pts;
prev_un_pts = cur_un_pts;
cur_img = forw_img;
cur_pts = forw_pts;
undistortedPoints();
prev_time = cur_time;
}