对应解析13页，四、前端视觉处理
主要包括特征点检测和特征点跟踪两部分，似乎是基于openCV实现的。openCV2.4官方文档，（暂时未找到openCV3.4有类似的文档）
这一节在解析中的内容较少（可能崔神认为比较简单），然而于我个人而言，它依然是个新东西。所以，仍然得仔细推敲。
（发现VINS-Mono和VINS-Fusion之中的文件组织结构也略有差别，之后进行一下详细对比，先主要阅读VINS-Fusion的代码）
featureTracker文件夹下有个feature_tracker.h文件，其中定义了一个FeatureTracker类。
发现通篇仅有一个成员函数FeatureTracker::trackImage()中调用了cv::goodFeaturesToTracke()函数来检测特征点，因此，从该函数开始分析：
输入参数：_cur_time，_img，_img1
返回参数：map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> （乍看起来可复杂，细细分析之）首先是一个map，键值对中的值是一个vector，其中每个元素是一个pair。具体什么含义？
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1 = cv::Mat());
（洋洋洒洒写了200行左右，看来并不简单）
TicToc是一个类，感觉和计时有关，使用std::chrono实现（The elements in this header deal with time.）暂时不展开。
predict_pts，cur_pts：类数据成员，vector<cv::Point2f> predict_pts，cur_pts;
hadPrediction：数据成员，bool hasPrediction;，初始值为什么？(254行赋值为hasPrediction = false;)
如果值为true，则调用openCV的函数进行光流跟踪：
发现该cv::calcOpticalFlowPyrLK()函数所需参数好多。因为是核心，此处展开来讲（calcOpticalFlowPyrLK官方介绍）：
函数功能：Calculates an optical flow for a sparse feature set using the iterative Lucas-Kanade method with pyramids.
使用带金字塔的迭代Lucas Kanade方法 计算稀疏特征集的光流。
C++ API:
void 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 – first 8-bit input image or pyramid constructed by buildOpticalFlowPyramid().
nextImg – second input image or pyramid of the same size and the same type as prevImg.
prevPts – vector of 2D points for which the flow needs to be found; point coordinates must be single-precision floating-point numbers.
需要找到流的二维点（存放在vector里），点坐标必须是单精度浮点型数字。
nextPts – output vector of 2D points containing the calculated new positions of input features in the second image; when OPTFLOW_USE_INITIAL_FLOW flag is passed, the vector must have the same size as in the input.
在第二个图像中，计算得到的输入特征所在的新位置的二维点（存放在vector里）；当标志位为OPTFLOW_USE_INITIAL_FLOW时，该vector必须与输入中的vector大小相同。
status – output status vector (of unsigned chars); each element of the vector is set to 1 if the flow for the corresponding features has been found, otherwise, it is set to 0.
err – output vector of errors; each element of the vector is set to an error for the corresponding feature, type of the error measure can be set in flags parameter; if the flow wasn’t found then the error is not defined (use the status parameter to find such cases).
winSize – size of the search window at each pyramid level.
maxLevel – 0-based maximal pyramid level number; if set to 0, pyramids are not used (single level), if set to 1, two levels are used, and so on; if pyramids are passed to input then algorithm will use as many levels as pyramids have but no more than maxLevel.
criteria – parameter, specifying the termination criteria of the iterative search algorithm (after the specified maximum number of iterations criteria.maxCount or when the search window moves by less than criteria.epsilon.
算法终止条件，1.最大迭代次数次数maxCount；2.精度criteria.epsilon
flags –
OPTFLOW_USE_INITIAL_FLOW uses initial estimations, stored in nextPts; if the flag is not set, then prevPts is copied to nextPts and is considered the initial estimate.
OPTFLOW_LK_GET_MIN_EIGENVALS use minimum eigen values as an error measure (see minEigThreshold description); if the flag is not set（默认度量）, then L1 distance between patches around the original and a moved point, divided by number of pixels in a window, is used as a error measure.
作为误差度量
minEigThreshold – the algorithm calculates the minimum eigen value （最小特征值）of a 2x2 normal matrix of optical flow equations (this matrix is called a spatial gradient matrix in [Bouguet00]), divided by number of pixels in a window; if this value is less than minEigThreshold, then a corresponding feature is filtered out and its flow is not processed, so it allows to remove bad points and get a performance boost（剔除坏点，提高性能）.
有了这些理解，看一下代码中调用的时候，传入参数表示的含义：
prev_img, cur_img：上一帧图片，当前帧图片；
prev_pts, cur_pts：特征点在上一帧中的位置（等待找到对应的流），特整点在当前帧中的位置（用以保存该算法计算结果）；
status：vector<uchar> status;
err：vector<float> err;
cv::Size(21, 21)：在每层金字塔中搜索窗的大小（
21
×
21
21\times 21
21×21）；
1： 对应两层；
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01)：终止条件——迭代次数 和 精度；
cv::OPTFLOW_USE_INITIAL_FLOW：保持和原来一样多特征点数。
然后，统计跟踪成功的点数（即status为1的个数）
【如果】跟踪成功的点数少于10个，对应一些处理。（再执行一次，这次修改传入参数中金字塔的层数为4层）
============ 分割线 =====================
假设跟踪成功的点的数目足够多了，进行下一步处理操作：
FLOW_BACK：暂时未找到该变量定义的地方。。
但是，其对应函数块的含义是清晰的，即，当FLOW_BACK为true时，再调用cv::calcOpticalFlowPyrLK()进行一次反向的光流跟踪（传入参数时将两幅图像、特征点做对应调换）
符合以下要求的才认为是跟踪成功的点：
status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5
=============== 分割线 ==================
将跟踪失败的点删除（即当status[i]值为0，删除各vector中下标i对应的元素）：
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
ids, track_cnt：FeatureTracker类的数据成员vector<int> ids;和vector<int> track_cnt;
============ 分割线 ====================
vectortrack_cnt中所有的计数值，统一 +1。
调用一个setMask()成员函数，其主要完成了按照track_cnt对cur_pts, ids, track_cnt的排序，并且创建了一个mask，以点cur_pts为圆心，填充了一个给定大小的黑色圆，其余位置为白色。
详细的介绍放在了 VINS-Fusion代码阅读（六）里。
调用另外一个openCV函数cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);，可以猜测其想实现的功能为，在图片的其他位置，再检测出给定数目MAX_CNT - cur_pts.size()的特征点，放在n_pts中。
同样，详细的介绍放在了 VINS-Fusion代码阅读（六）里。
这样，将新检测到的特征点n_pts添加到cur_pts中去，并赋予相应的ids和track_cnt。
++++++++++++++ 新分割线 +++++++++++++++++
下一步，调用undistortedPts成员函数，该函数比较简单，主要想实现的功能是
将像素坐标系下的坐标，转换为归一化相机坐标系下的坐标？即cur_un_pts为归一化相机坐标系下的坐标。为什么使用的是m_camera[0]？
cur_un_pts = undistortedPts(cur_pts, m_camera[0]);
其中，vector<camodocal::CameraPtr> m_camera;是一个vector，其中元素为CameraPtr类型，下一步需要了解一下该类型中成员函数liftProjective()的功能与实现。
vector<cv::Point2f> FeatureTracker::undistortedPts(vector<cv::Point2f> &pts, camodocal::CameraPtr cam)
{
vector<cv::Point2f> un_pts;
for (unsigned int i = 0; i < pts.size(); i++)
{
Eigen::Vector2d a(pts[i].x, pts[i].y);
Eigen::Vector3d b;
cam->liftProjective(a, b);
un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
}
return un_pts;
}
接下来，计算pts_velocity，其为当前帧相对于前一帧 特征点沿x,y方向的像素移动速度。
pts_velocity = ptsVelocity(ids, cur_un_pts, cur_un_pts_map, prev_un_pts_map);
cur_un_pts_map中存放ids[i]和cur_un_pts[i]构成的键值对。
在prev_un_pts_map非空的情况下，对每一个cur_un_pts_map中的键值对都去寻找是否在prev_un_pts_map中存在，若存在像素移动速度不难计算；若不存在则为0；
如果prev_un_pts_map是空的情况下，置零。
vector<cv::Point2f> FeatureTracker::ptsVelocity(vector<int> &ids, vector<cv::Point2f> &pts,
map<int, cv::Point2f> &cur_id_pts, map<int, cv::Point2f> &prev_id_pts)
{
vector<cv::Point2f> pts_velocity;
cur_id_pts.clear();
for (unsigned int i = 0; i < ids.size(); i++)
{
cur_id_pts.insert(make_pair(ids[i], pts[i]));
}
// caculate points velocity
if (!prev_id_pts.empty())
{
double dt = cur_time - prev_time;
for (unsigned int i = 0; i < pts.size(); i++)
{
std::map<int, cv::Point2f>::iterator it;
it = prev_id_pts.find(ids[i]);
if (it != prev_id_pts.end())
{
double v_x = (pts[i].x - it->second.x) / dt;
double v_y = (pts[i].y - it->second.y) / dt;
pts_velocity.push_back(cv::Point2f(v_x, v_y));
}
else
pts_velocity.push_back(cv::Point2f(0, 0));
}
}
else
{
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
pts_velocity.push_back(cv::Point2f(0, 0));
}
}
return pts_velocity;
}
解析（14页）关于这一部分——前段视觉处理就结束了；代码中，由于支持双目相机因此下面对于双目相机的另一幅图像_img1进行处理。
++++++++++++ 分割线 +++++++++++++++++++++
if(!_img1.empty() && stereo_cam){ }
不太理解的是，为什么双目的时候，光流跟踪是在左右两幅图像之间进行？
cv::calcOpticalFlowPyrLK(cur_img, rightImg, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);
不同之处：使用的是m_camera[1]
cur_un_right_pts = undistortedPts(cur_right_pts, m_camera[1]);
像素速度的计算是依据 右边的先后两个点云：
right_pts_velocity = ptsVelocity(ids_right, cur_un_right_pts, cur_un_right_pts_map, prev_un_right_pts_map);
+++++++++++++ 分割线 +++++++++++++++++++
最最后，是一些数据的保存工作。重要的数据成员罗列如下：
prev_img = cur_img;
prev_pts = cur_pts;
prev_un_pts = cur_un_pts;
prev_un_pts_map = cur_un_pts_map;
prev_time = cur_time;
prevLeftPtsMap[ids[i]] = cur_pts[i]; ID和cur_pts像素位置的点云图（用在了绘图展示中）
最后，返回值map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame
其中Eigen::Matrix<double, 7, 1> xyz_uv_velocity;包含跟踪点归一化相机坐标系下的坐标3，像素平面上的坐标2，像素移动速度2。
featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity);
此处很神奇，我们可以看出没有使用make_pair，更没有使用vector.push_back()，简洁的一句.emplace_back()就搞定了！了解其使用！
同样地，对右边一个相机有相同的操作，featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity);不同的是camera_id，左边是0，右边是1。
============== 下面是源代码 可以不看了呀！==============
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> FeatureTracker::trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1)
{
TicToc t_r;
cur_time = _cur_time;
cur_img = _img;
row = cur_img.rows;
col = cur_img.cols;
cv::Mat rightImg = _img1; // 注意此处是一个浅拷贝
cur_pts.clear(); // vector的clear成员函数
if (prev_pts.size() > 0)
{
TicToc t_o;
vector<uchar> status;
vector<float> err;
if(hasPrediction)
{
cur_pts = predict_pts;
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 1,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
int succ_num = 0;
for (size_t i = 0; i < status.size(); i++)
{
if (status[i])
succ_num++;
}
if (succ_num < 10)
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);
}
else
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);
// reverse check
if(FLOW_BACK)
{
vector<uchar> reverse_status;
vector<cv::Point2f> reverse_pts = prev_pts;
cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 1,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
//cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 3);
for(size_t i = 0; i < status.size(); i++)
{
if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5)
{
status[i] = 1;
}
else
status[i] = 0;
}
}
for (int i = 0; i < int(cur_pts.size()); i++)
if (status[i] && !inBorder(cur_pts[i]))
status[i] = 0;
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
//printf("track cnt %d\n", (int)ids.size());
}
for (auto &n : track_cnt)
n++;
if (1)
{
//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>(cur_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;
cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);
}
else
n_pts.clear();
ROS_DEBUG("detect feature costs: %f ms", t_t.toc());
for (auto &p : n_pts)
{
cur_pts.push_back(p);
ids.push_back(n_id++);
track_cnt.push_back(1);
}
//printf("feature cnt after add %d\n", (int)ids.size());
}
cur_un_pts = undistortedPts(cur_pts, m_camera[0]);
pts_velocity = ptsVelocity(ids, cur_un_pts, cur_un_pts_map, prev_un_pts_map);
if(!_img1.empty() && stereo_cam)
{
ids_right.clear();
cur_right_pts.clear();
cur_un_right_pts.clear();
right_pts_velocity.clear();
cur_un_right_pts_map.clear();
if(!cur_pts.empty())
{
//printf("stereo image; track feature on right image\n");
vector<cv::Point2f> reverseLeftPts;
vector<uchar> status, statusRightLeft;
vector<float> err;
// cur left ---- cur right
cv::calcOpticalFlowPyrLK(cur_img, rightImg, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);
// reverse check cur right ---- cur left
if(FLOW_BACK)
{
cv::calcOpticalFlowPyrLK(rightImg, cur_img, cur_right_pts, reverseLeftPts, statusRightLeft, err, cv::Size(21, 21), 3);
for(size_t i = 0; i < status.size(); i++)
{
if(status[i] && statusRightLeft[i] && inBorder(cur_right_pts[i]) && distance(cur_pts[i], reverseLeftPts[i]) <= 0.5)
status[i] = 1;
else
status[i] = 0;
}
}
ids_right = ids;
reduceVector(cur_right_pts, status);
reduceVector(ids_right, status);
// only keep left-right pts
/*
reduceVector(cur_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
reduceVector(cur_un_pts, status);
reduceVector(pts_velocity, status);
*/
cur_un_right_pts = undistortedPts(cur_right_pts, m_camera[1]);
right_pts_velocity = ptsVelocity(ids_right, cur_un_right_pts, cur_un_right_pts_map, prev_un_right_pts_map);
}
prev_un_right_pts_map = cur_un_right_pts_map;
}
if(SHOW_TRACK)
drawTrack(cur_img, rightImg, ids, cur_pts, cur_right_pts, prevLeftPtsMap);
prev_img = cur_img;
prev_pts = cur_pts;
prev_un_pts = cur_un_pts;
prev_un_pts_map = cur_un_pts_map;
prev_time = cur_time;
hasPrediction = false;
prevLeftPtsMap.clear();
for(size_t i = 0; i < cur_pts.size(); i++)
prevLeftPtsMap[ids[i]] = cur_pts[i];
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
for (size_t i = 0; i < ids.size(); i++)
{
int feature_id = ids[i];
double x, y ,z;
x = cur_un_pts[i].x;
y = cur_un_pts[i].y;
z = 1;
double p_u, p_v;
p_u = cur_pts[i].x;
p_v = cur_pts[i].y;
int camera_id = 0;
double velocity_x, velocity_y;
velocity_x = pts_velocity[i].x;
velocity_y = pts_velocity[i].y;
Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}
if (!_img1.empty() && stereo_cam)
{
for (size_t i = 0; i < ids_right.size(); i++)
{
int feature_id = ids_right[i];
double x, y ,z;
x = cur_un_right_pts[i].x;
y = cur_un_right_pts[i].y;
z = 1;
double p_u, p_v;
p_u = cur_right_pts[i].x;
p_v = cur_right_pts[i].y;
int camera_id = 1;
double velocity_x, velocity_y;
velocity_x = right_pts_velocity[i].x;
velocity_y = right_pts_velocity[i].y;
Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}
}
//printf("feature track whole time %f\n", t_r.toc());
return featureFrame;
}
PS. 如何使用QT打开已有的ROS工作空间？
目前使用的方法仅仅可以打开并查看，其中一系列的“未找到定义”的错误。当然也无法编译。
相关的文章主要查看了：
创客空间——ROS与C++入门教程-搭建开发环境(QT+ros_qtc_plugin)，通过最后新建工作空间的方法。
QT打开ROS工作空间时遇到的问题和解决方法，按照该方法执行了，但依然未能成功。
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
