该函数读取图像Mat以及当前世界_cur_time,并对其进行处理,这一段判断是否对图像利用opencv进行处理,从而消除其明暗变化,并且将传入的时间记录为当前时间。
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;
接下来是图像的一个保存,如果是处理第一帧图像,prev_img = cur_img = forw_img = img, 三个图像都是第一帧图像,第二帧图像进入后perv和cur是上一帧图像,forw_img是最新进入的图像
if (forw_img.empty())
{
prev_img = cur_img = forw_img = img;
}
else
{
forw_img = img;
}
forw_pts.clear();
forw_pts记录新的特征点,因此对特征点集进行清空
forw_pts.clear();
cur_pts是记录的上一帧已经处理的图像的特征点,当第一帧图像进入时,cur_pts还为空,因此不会进入这一处理函数。如果是上一帧的img已经存在并且提取了特征点,就可以进行光流跟踪。cur_img是上一处理过的图像,cur_pts是对那张图像处理获得的二维点,forw_img是最新进来的图像,forw_pts是利用光流获得的跟踪点,status会记录是否跟踪成功。
if (cur_pts.size() > 0)
{
TicToc t_o;
vector<uchar> status;
vector<float> err;
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());
}
进行光流跟踪后,会判断新的点是否在图像内,如果距离边界小于1个像素,也会被排除
for (int i = 0; i < int(forw_pts.size()); i++)
if (status[i] && !inBorder(forw_pts[i]))
status[i] = 0;
bool inBorder(const cv::Point2f &pt)
{
const int BORDER_SIZE = 1;
int img_x = cvRound(pt.x);
int img_y = cvRound(pt.y);
return BORDER_SIZE <= img_x && img_x < COL - BORDER_SIZE && BORDER_SIZE <= img_y && img_y < ROW - BORDER_SIZE;
}
如果status为0,也就是未成功跟踪,或者出界的点,将会被从prev_pts,cur_pts,forw_pts,ids,cur_un_pts,track_cnt中进行移除
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(ids, status);
reduceVector(cur_un_pts, status);
reduceVector(track_cnt, status);
对所有未移除的点,track_nt记录跟踪成功的次数,需要对其进行+1
for (auto &n : track_cnt)
n++;
如果PUB_THIS_FRAME,
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;
利用F矩阵来排除异常点
void FeatureTracker::rejectWithF()
{
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;
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());
}
}
利用F矩阵来排除异常点,函数liftProjective是对图像的二维点去畸变,然后投影到归一化平面,其在camera.cc中是一个纯虚函数,在不同相机类,PinholeCamera.cc等中进行了具体实现,就是去畸变,然后投影。之后利用小孔成像,重新计算x和y,再利用opencv的函数寻找异常点
void FeatureTracker::rejectWithF()
{
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;
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());
}
}
接下来会寻找新的特征点,例如总特征点数为150,上一帧还剩下120个特征点,那么就需要重新寻找新的30个特征点,新的30个特征点需要与已找到的特征点尽量分开,因此会通过setMask函数来对已有的点进行记录
void FeatureTracker::setMask()
{
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
vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;
for (unsigned int i = 0; i < forw_pts.size(); i++) //对最新跟踪的点放入到cnt_pts_id中
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) //对cnt_pts_id利用跟踪的次数进行排序
{
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);
cv::circle(mask, it.second.first, MIN_DIST, 0, -1); //将特征点附近设置为0
}
}
}
检测新的角点
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;
其中addPoints会将新检测的点添加到forw_pts中,ids全部设置为-1,track_cnt则会设置为1,因此是第一次被追踪
void FeatureTracker::addPoints()
{
for (auto &p : n_pts)
{
forw_pts.push_back(p);
ids.push_back(-1);
track_cnt.push_back(1);
}
}
对当前的点计算计算速度与无畸变的投影
void FeatureTracker::undistortedPoints()
{
cur_un_pts.clear();
cur_un_pts_map.clear();
//cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);
Eigen::Vector3d b;
m_camera->liftProjective(a, b);
cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z())));
//printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y);
}
// caculate points velocity
if (!prev_un_pts_map.empty())
{
double dt = cur_time - prev_time;
pts_velocity.clear();
for (unsigned int i = 0; i < cur_un_pts.size(); i++)
{
if (ids[i] != -1)
{
std::map<int, cv::Point2f>::iterator it;
it = prev_un_pts_map.find(ids[i]);
if (it != prev_un_pts_map.end())
{
double v_x = (cur_un_pts[i].x - it->second.x) / dt;
double v_y = (cur_un_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
{
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));
}
}
prev_un_pts_map = cur_un_pts_map;
}
经过处理后获得了以下数据:
vector<cv::Point2f> prev_pts, cur_pts, forw_pts; //前,中,后三帧图像的特征点
vector<cv::Point2f> prev_un_pts, cur_un_pts; //去畸变后的点
vector<cv::Point2f> pts_velocity; //点的速度
vector<int> ids; //点的id
vector<int> track_cnt; //点跟踪的次数
map<int, cv::Point2f> cur_un_pts_map; //map
map<int, cv::Point2f> prev_un_pts_map; //map