#include "feature_tracker.hpp"
#include <iostream>
#include <unordered_map>
#include <opencv2/video/tracking.hpp>
#include "multi_view_geometry.hpp"//用于多视角几何计算
/*
函数FeatureTracker使用Lucas-Kanade光流算法进行特征跟踪，并返回跟踪结果。
它的输入参数是两个图像金字塔vprevpyr和vcurpyr，它们都是OpenCV中的cv::Mat类型的向量。nwinsize是每个特征点周围用于跟踪的像素窗口大小。
nbpyrlvl是金字塔的层数。ferr是允许的光流估计误差阈值，fmax_fbklt_dist是反向跟踪的最大距离阈值。
vkps是一个cv::Point2f类型的向量，其中存储了需要跟踪的特征点的初始位置。vpriorkps是特征点的先前位置，vkpstatus是特征点的跟踪状态，
如果跟踪成功，则为true，否则为false。
该函数的输出是vkps、vpriorkps和vkpstatus，这些向量将包含成功跟踪的特征点的位置、先前位置和跟踪状态。
*/
void FeatureTracker::fbKltTracking(const std::vector<cv::Mat> &vprevpyr, const std::vector<cv::Mat> &vcurpyr,
int nwinsize, int nbpyrlvl, float ferr, float fmax_fbklt_dist, std::vector<cv::Point2f> &vkps,
std::vector<cv::Point2f> &vpriorkps, std::vector<bool> &vkpstatus) const
{
// std::cout << "\n \t >>> Forward-Backward kltTracking with Pyr of Images and Motion Prior! \n";
//检查先前和当前金字塔图像的大小是否相等，如果不相等，则返回。
assert(vprevpyr.size() == vcurpyr.size());
//如果vkps为空，则函数也返回。
if( vkps.empty() ) {
// std::cout << "\n \t >>> No kps were provided to kltTracking()!\n";
return;
}
//根据提供的参数设置KLT算法使用的窗口大小、金字塔的级数，并准备了一些用于存储光流跟踪结果的向量。
cv::Size klt_win_size(nwinsize, nwinsize);
if( (int)vprevpyr.size() < 2*(nbpyrlvl+1) ) {
nbpyrlvl = vprevpyr.size() / 2 - 1;
}
// Objects for OpenCV KLT
size_t nbkps = vkps.size();
vkpstatus.reserve(nbkps); //vkpstatus向量存储每个感兴趣点的状态（跟踪成功或失败）
std::vector<uchar> vstatus;
std::vector<float> verr;
std::vector<int> vkpsidx;
vstatus.reserve(nbkps); //vstatus向量存储每个感兴趣点的状态（跟踪成功或失败）
verr.reserve(nbkps); //verr向量存储每个感兴趣点的跟踪误差
vkpsidx.reserve(nbkps); //vkpsidx向量存储vkps中每个感兴趣点的索引。
// Tracking Forward
/*
函数使用OpenCV的calcOpticalFlowPyrLK函数执行光流跟踪。该函数接收先前和当前的金字塔图像（vprevpyr和vcurpyr）、感兴趣点向量（vkps）、
先前帧感兴趣点向量（vpriorkps）、感兴趣点状态向量（vstatus）、感兴趣点跟踪误差向量（verr）、KLT算法使用的窗口大小（klt_win_size）、
金字塔级数（nbpyrlvl）、KLT算法的收敛标准（klt_convg_crit_）和一些标志位（cv::OPTFLOW_USE_INITIAL_FLOW + cv::OPTFLOW_LK_GET_MIN_EIGENVALS）。
其中，cv::OPTFLOW_USE_INITIAL_FLOW标志告诉算法使用vpriorkps中的先前帧感兴趣点作为初始估计，
cv::OPTFLOW_LK_GET_MIN_EIGENVALS标志告诉算法计算每个感兴趣点的最小特征值，以便过滤出不稳定的跟踪点。
*/
cv::calcOpticalFlowPyrLK(vprevpyr, vcurpyr, vkps, vpriorkps,
vstatus, verr, klt_win_size, nbpyrlvl, klt_convg_crit_,
(cv::OPTFLOW_USE_INITIAL_FLOW + cv::OPTFLOW_LK_GET_MIN_EIGENVALS)
);
std::vector<cv::Point2f> vnewkps; //返回一个新的感兴趣点向量（vnewkps）,存储跟踪后的感兴趣点
std::vector<cv::Point2f> vbackkps; //一个回溯的感兴趣点向量（vbackkps）,存储它们在先前帧中的位置,这两个向量的长度与vkps的长度相同，因为它们对应着每个输入的感兴趣点
vnewkps.reserve(nbkps);
vbackkps.reserve(nbkps);
size_t nbgood = 0;
// Init outliers vector & update tracked kps初始化非关键点 (outlier) 列表和已跟踪的关键点列表
//用于估计关键点位的算法的实现
for( size_t i = 0 ; i < nbkps ; i++ ) //循环遍历所有的关键点
{
if( !vstatus.at(i) ) {
vkpstatus.push_back(false);//对于每个关键点，检查其状态是否为未标记,如果未标记则将其添加到非关键点列表中。
continue;
}
if( verr.at(i) > ferr ) {
vkpstatus.push_back(false);//如果该关键点的误差大于设定的最大误差 (ferr),则将其添加到非关键点列表中
continue;
}
if( !inBorder(vpriorkps.at(i), vcurpyr.at(0)) ) {
vkpstatus.push_back(false);//如果该关键点与上一个关键点的位置关系不在预定义的边界内 ,则将其添加到非关键点列表中
continue;
}
vnewkps.push_back(vpriorkps.at(i));//将当前关键点添加到新关键点列表中，并将其上一个关键点添加到旧关键点列表中
vbackkps.push_back(vkps.at(i));//将当前关键点的状态标记为已标记 (tagged),并将其索引添加到关键点状态列表中
vkpstatus.push_back(true);//将关键点列表中的所有关键点更新为当前关键点的位置信息
vkpsidx.push_back(i);
nbgood++;//nbgood 表示当前循环遍历到的关键点中状态为已标记 (tagged) 的关键点的数量，用于更新已跟踪的关键点列表
}
if( vnewkps.empty() ) {//检查一个名为vnewkps的向量是否为空。
return;
}
vstatus.clear();//如果vnewkps不为空，则清空另外两个名为vstatus和verr的向量
verr.clear();
// std::cout << "\n \t >>> Forward kltTracking : #" << nbgood << " out of #" << nbkps << " \n";
// Tracking Backward
/*
OpenCV库中的calcOpticalFlowPyrLK函数对两幅图像之间的光流进行计算，并且将结果存储在vnewkps, vbackkps, vstatus,
和 verr四个向量中。其中，vcurpyr和vprevpyr是待计算光流的两幅图像，klt_win_size是KLT算法中的窗口大小，klt_convg_crit_是KLT算法中的迭代收敛标准。
*/
cv::calcOpticalFlowPyrLK(vcurpyr, vprevpyr, vnewkps, vbackkps,
vstatus, verr, klt_win_size, 0, klt_convg_crit_,
(cv::OPTFLOW_USE_INITIAL_FLOW + cv::OPTFLOW_LK_GET_MIN_EIGENVALS)
);
nbgood = 0;
for( int i = 0, iend=vnewkps.size() ; i < iend ; i++ )//遍历vnewkps向量中的所有特征点
{
int idx = vkpsidx.at(i);
//检查其对应的光流跟踪结果是否有效,如果vstatus向量中对应的元素值为false，则说明该特征点的光流跟踪失败，将其在vkpstatus向量中的对应元素值也设置为false。
if( !vstatus.at(i) ) {
vkpstatus.at(idx) = false;
continue;
}
//如果该特征点与其在之前图像中的位置的距离超过了fmax_fbklt_dist，则同样认为其光流跟踪失败，将其在vkpstatus向量中的对应元素值也设置为false。
if( cv::norm(vkps.at(idx) - vbackkps.at(i)) > fmax_fbklt_dist ) {
vkpstatus.at(idx) = false;
continue;
}
nbgood++;//将所有光流跟踪成功的特征点的数量记录在nbgood变量中
}
// std::cout << "\n \t >>> Backward kltTracking : #" << nbgood << " out of #" << vkpsidx.size() << " \n";
}
/*
这是一个特征跟踪器（FeatureTracker）类中的函数，用于计算两幅图像（iml和imr）中某个点（pt）处，
一条水平线上（bgoleft为false）或者一条与水平线倾斜的线上（bgoleft为true），在一个指定大小（nwinsize）的窗口内最小的绝对差异（L1误差），
并返回该误差和所在位置的横坐标（xprior）。
*/
void FeatureTracker::getLineMinSAD(const cv::Mat &iml, const cv::Mat &imr,
const cv::Point2f &pt, const int nwinsize, float &xprior,
float &l1err, bool bgoleft) const
{
xprior = -1;
//检查指定的窗口大小是否为奇数，如果不是，会输出错误信息并返回。
if( nwinsize % 2 == 0 ) {
std::cerr << "\ngetLineMinSAD requires an odd window size\n";
return;
}
//获取目标点（pt）在imr图像中所在的矩形窗口，矩形大小由指定的窗口大小nwinsize来确定。
const float x = pt.x;
const float y = pt.y;
int halfwin = nwinsize / 2;
//根据目标点所在位置（pt）和指定的窗口大小（nwinsize），计算矩形窗口在imr图像中的位置和大小。
//如果矩形窗口的某个边界超出了imr图像的边界，函数会重新计算窗口大小以保证其在图像内部，并返回。
if( x - halfwin < 0 )
halfwin += (x-halfwin);
if( x + halfwin >= imr.cols )
halfwin += (x+halfwin - imr.cols - 1);
if( y - halfwin < 0 )
halfwin += (y-halfwin);
if( y + halfwin >= imr.rows )
halfwin += (y+halfwin - imr.rows - 1);
//如果窗口大小小于等于0，则直接返回
if( halfwin <= 0 ) {
return;
}
/*
计算两幅图像之间的均方误差（Mean Squared Error，MSE）和绝对差值误差（L1误差）。以下代码的主要目的是找到两幅图像中相似区域的匹配位置。
*/
cv::Size winsize(2 * halfwin + 1, 2 * halfwin + 1);//定义一个cv::Size类型的变量winsize，用于表示搜索窗口的大小。其中halfwin是一个整数变量，表示搜索窗口的半径。
int nbwinpx = (winsize.width * winsize.height);//计算搜索窗口中像素的数量，用于后面的归一化处理。
float minsad = 255.;//初始化最小的L1误差为255
// int minxpx = -1;
cv::Mat patch, target; //定义两个cv::Mat类型的变量patch和target，分别表示搜索窗口和目标图像
cv::getRectSubPix(iml, winsize, pt, patch);//从左图像iml中提取以pt为中心，大小为winsize的矩形区域，并将其存储在patch中
//根据标志bgoleft的值，选择从右图像的左侧还是右侧开始搜索。这个标志表示当前搜索的方向
if( bgoleft ) {
for( float c = x ; c >= halfwin ; c-=1. )
{
cv::getRectSubPix(imr, winsize, cv::Point2f(c, y), target);//从右图像imr中提取以(c, y)为中心，大小为winsize的矩形区域，并将其存储在target中
l1err = cv::norm(patch, target, cv::NORM_L1);//计算patch和target之间的L1误差
l1err /= nbwinpx;//对L1误差进行归一化，使其除以搜索窗口中像素的数量
//如果L1误差小于最小值，则更新最小值和匹配位置的x坐标xprior
if( l1err < minsad ) {
minsad = l1err;
xprior = c;
}
}
} else {
for( float c = x ; c < imr.cols - halfwin ; c+=1. )
{
cv::getRectSubPix(imr, winsize, cv::Point2f(c, y), target);
l1err = cv::norm(patch, target, cv::NORM_L1);
l1err /= nbwinpx;
if( l1err < minsad ) {
minsad = l1err;
xprior = c;
}
}
}
l1err = minsad;//最后，将最小L1误差赋值给l1err变量
}
/**
* \brief Perform a forward-backward calcOpticalFlowPyrLK() tracking with OpenCV.
*
* \param[in] pt Opencv 2D point.
* \return True if pt is within image borders, False otherwise
*/
//判断一个二维坐标点是否在图像边缘内部
bool FeatureTracker::inBorder(const cv::Point2f &pt, const cv::Mat &im) const
{
const float BORDER_SIZE = 1.;//定义了一个常量BORDER_SIZE，用于表示图像边缘的大小
/*
检查点pt的x坐标是否大于等于BORDER_SIZE且小于图像im的宽度减去BORDER_SIZE，同时检查点pt的y坐标是否大于等于BORDER_SIZE且小于图像im的高度减去BORDER_SIZE。
如果同时满足这两个条件，函数将返回true，表示该点在图像边界内；否则，函数将返回false，表示该点在图像边界外。
*/
return BORDER_SIZE <= pt.x && pt.x < im.cols - BORDER_SIZE && BORDER_SIZE <= pt.y && pt.y < im.rows - BORDER_SIZE;
}
1482
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
