目录
feature_tracker.h
#pragma once
#include <cstdio>
#include <iostream>
#include <queue>
#include <execinfo.h>
#include <csignal>
#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Dense>
#include "camodocal/camera_models/CameraFactory.h"
#include "camodocal/camera_models/CataCamera.h"
#include "camodocal/camera_models/PinholeCamera.h"
#include "../estimator/parameters.h"
#include "../utility/tic_toc.h"
using namespace std;
using namespace camodocal;
using namespace Eigen;
// 判断跟踪的特征点是否在图像边界内部
bool inBorder(const cv::Point2f &pt);
// 去除无法跟踪的特征点
void reduceVector(vector<cv::Point2f> &v, vector<uchar> status);
void reduceVector(vector<int> &v, vector<uchar> status);
class FeatureTracker
{
public:
FeatureTracker();
//使用稀疏光流法进行追踪函数,包含了单目双目的处理情况
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1 = cv::Mat());
void setMask();
//读取相机的内参
void readIntrinsicParameter(const vector<string> &calib_file);
void showUndistortion(const string &name);
;//使用FM矩阵进行点的筛选,但是具体好像没有使用,使用了反追踪光流去筛选点
void rejectWithF();
//对获得点集去畸变
void undistortedPoints();
vector<cv::Point2f> undistortedPts(vector<cv::Point2f> &pts, camodocal::CameraPtr cam);
//计算特征点的速度,具体可以参考SLAM14讲的稀疏光流法
vector<cv::Point2f> ptsVelocity(vector<int> &ids, vector<cv::Point2f> &pts,
map<int, cv::Point2f> &cur_id_pts, map<int, cv::Point2f> &prev_id_pts);
void showTwoImage(const cv::Mat &img1, const cv::Mat &img2,
vector<cv::Point2f> pts1, vector<cv::Point2f> pts2);
//绘制特征点,有一部分是点的数量决定颜色
void drawTrack(const cv::Mat &imLeft, const cv::Mat &imRight,
vector<int> &curLeftIds,
vector<cv::Point2f> &curLeftPts,
vector<cv::Point2f> &curRightPts,
map<int, cv::Point2f> &prevLeftPtsMap);
//设置预测点
void setPrediction(map<int, Eigen::Vector3d> &predictPts);
//两点的欧式距离
double distance(cv::Point2f &pt1, cv::Point2f &pt2);
//去除外点
void removeOutliers(set<int> &removePtsIds);
// 返回的画好特征的图像
cv::Mat getTrackImage();
bool inBorder(const cv::Point2f &pt);
int row, col;
cv::Mat imTrack;
// 图像掩码
cv::Mat mask;
// 鱼眼相机mash,用于去除边缘的噪点
cv::Mat fisheye_mask;
// prev_img是上一次发布的帧的图像数据
// cur_img是光流跟踪的前一帧的图像数据
cv::Mat prev_img, cur_img;
// 每一帧中新提取的特征点
vector<cv::Point2f> n_pts;
// 预测的特征点
vector<cv::Point2f> predict_pts;
// 预测特征点的debug
vector<cv::Point2f> predict_pts_debug;
// 前帧点,当前帧点,和当前右图点
vector<cv::Point2f> prev_pts, cur_pts, cur_right_pts;
// 去畸变的前帧点,当前帧点,和当前右图点
vector<cv::Point2f> prev_un_pts, cur_un_pts, cur_un_right_pts;
// 特征点的速度,看光流法克制,右图
vector<cv::Point2f> pts_velocity, right_pts_velocity;
// 特征点的id
vector<int> ids, ids_right;
// 追踪到点的数量
vector<int> track_cnt;
// /这几个哈希表在去畸变的时候用到了,但是没有找到填充数据的地方,是空映射有点疑惑
map<int, cv::Point2f> cur_un_pts_map, prev_un_pts_map;
map<int, cv::Point2f> cur_un_right_pts_map, prev_un_right_pts_map;
map<int, cv::Point2f> prevLeftPtsMap;
// 相机参数和工具
vector<camodocal::CameraPtr> m_camera;
// 当前帧时间
double cur_time;
// 前一帧时间
double prev_time;
// 是否双目的标志位
bool stereo_cam;
// 帧的id
int n_id;
// 是否有预测点信息的标志位
bool hasPrediction;
};
feature_tracker.cpp
#include "feature_tracker.h"
// 计算关键点放在范围之内
bool FeatureTracker::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;
}
// 计算两点之间的欧氏距离感觉可以作为内联函数使用,更快一下
double distance(cv::Point2f pt1, cv::Point2f pt2)
{
//printf("pt1: %f %f pt2: %f %f\n", pt1.x, pt1.y, pt2.x, pt2.y);
double dx = pt1.x - pt2.x;
double dy = pt1.y - pt2.y;
return sqrt(dx * dx + dy * dy);
}
// 去除不合适的关键点
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);
}
// 构造函数
FeatureTracker::FeatureTracker()
{
stereo_cam = 0;
n_id = 0;
hasPrediction = false;
}
// 对跟踪点进行排序并去除密集点
// 对跟踪到的特征点，按照被追踪到的次数排序并依次选点，使用mask进行类似非极大抑制，半径为30，去掉密集点，
// 使特征点分布均匀。对跟踪到的特征点从大到小排序并去除密集的点。
void FeatureTracker::setMask()
{
// 建立对应的掩码
mask = cv::Mat(row, col, CV_8UC1, cv::Scalar(255));
// 保存长时间跟踪到的特征点
// 构造(cnt，pts，id)序列，（追踪次数，当前特征点坐标，id）
vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;
// 对光流跟踪到的特征点forw_pts，按照被跟踪到的次数cnt从大到小排序（lambda表达式）
for (unsigned int i = 0; i < cur_pts.size(); i++)
// 把追踪得到的点track_cnt放入cnt_pts_id
cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(cur_pts[i], ids[i])));
// ort 对给定区间的所有元素进行排序，按照点的跟踪次数，从多到少进行排序
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;
});
//清空cnt，pts，id并重新存入
cur_pts.clear();
ids.clear();
track_cnt.clear();
// 对于所有追踪到的点
for (auto &it : cnt_pts_id)
{
// 如果在特征点出的灰度值等于255
if (mask.at<uchar>(it.second.first) == 255)
{
// 这个特征点对应的mask值为255，表明点是黑的，还没占有
cur_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);
}
}
}
// 两个特征点之间的距离
double FeatureTracker::distance(cv::Point2f &pt1, cv::Point2f &pt2)
{
//printf("pt1: %f %f pt2: %f %f\n", pt1.x, pt1.y, pt2.x, pt2.y);
double dx = pt1.x - pt2.x;
double dy = pt1.y - pt2.y;
return sqrt(dx * dx + dy * dy);
}
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;
/*
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
clahe->apply(cur_img, cur_img);
if(!rightImg.empty())
clahe->apply(rightImg, rightImg);
}
*/
// 清空左目的特征点
cur_pts.clear();
// 上一个图像对应的特征点
if (prev_pts.size() > 0)
{
// 用于进行对应的计算时间
TicToc t_o;
// 用于存放特征点的状态
vector<uchar> status;
// 存放对应的误差
vector<float> err;
// 判断是否有预测点使用光流法匹配
if(hasPrediction)
{
cur_pts = predict_pts;
// 该函数实现了金字塔中Lucas-Kanade光流的稀疏迭代版本
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;
}
void FeatureTracker::rejectWithF()
{
if (cur_pts.size() >= 8)
{
ROS_DEBUG("FM ransac begins");
TicToc t_f;
vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_prev_pts(prev_pts.size());
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
Eigen::Vector3d tmp_p;
m_camera[0]->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[0]->liftProjective(Eigen::Vector2d(prev_pts[i].x, prev_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_prev_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
}
vector<uchar> status;
cv::findFundamentalMat(un_cur_pts, un_prev_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
int size_a = cur_pts.size();
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(cur_un_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, cur_pts.size(), 1.0 * cur_pts.size() / size_a);
ROS_DEBUG("FM ransac costs: %fms", t_f.toc());
}
}
// 读取表定好的相机内参数
void FeatureTracker::readIntrinsicParameter(const vector<string> &calib_file)
{
for (size_t i = 0; i < calib_file.size(); i++)
{
ROS_INFO("reading paramerter of camera %s", calib_file[i].c_str());
camodocal::CameraPtr camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file[i]);
m_camera.push_back(camera);
}
if (calib_file.size() == 2)
stereo_cam = 1;
}
// 显示未畸变矫正的成果
void FeatureTracker::showUndistortion(const string &name)
{
cv::Mat undistortedImg(row + 600, col + 600, CV_8UC1, cv::Scalar(0));
vector<Eigen::Vector2d> distortedp, undistortedp;
for (int i = 0; i < col; i++)
for (int j = 0; j < row; j++)
{
Eigen::Vector2d a(i, j);
Eigen::Vector3d b;
m_camera[0]->liftProjective(a, b);
distortedp.push_back(a);
undistortedp.push_back(Eigen::Vector2d(b.x() / b.z(), b.y() / b.z()));
//printf("%f,%f->%f,%f,%f\n)\n", a.x(), a.y(), b.x(), b.y(), b.z());
}
for (int i = 0; i < int(undistortedp.size()); i++)
{
cv::Mat pp(3, 1, CV_32FC1);
pp.at<float>(0, 0) = undistortedp[i].x() * FOCAL_LENGTH + col / 2;
pp.at<float>(1, 0) = undistortedp[i].y() * FOCAL_LENGTH + row / 2;
pp.at<float>(2, 0) = 1.0;
//cout << trackerData[0].K << endl;
//printf("%lf %lf\n", p.at<float>(1, 0), p.at<float>(0, 0));
//printf("%lf %lf\n", pp.at<float>(1, 0), pp.at<float>(0, 0));
if (pp.at<float>(1, 0) + 300 >= 0 && pp.at<float>(1, 0) + 300 < row + 600 && pp.at<float>(0, 0) + 300 >= 0 && pp.at<float>(0, 0) + 300 < col + 600)
{
undistortedImg.at<uchar>(pp.at<float>(1, 0) + 300, pp.at<float>(0, 0) + 300) = cur_img.at<uchar>(distortedp[i].y(), distortedp[i].x());
}
else
{
//ROS_ERROR("(%f %f) -> (%f %f)", distortedp[i].y, distortedp[i].x, pp.at<float>(1, 0), pp.at<float>(0, 0));
}
}
// turn the following code on if you need
// cv::imshow(name, undistortedImg);
// cv::waitKey(0);
}
// 矫正点的畸变
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;
}
// 计算点的特征点移动的速度,为光流法预测的准备
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;
}
// 绘制追踪到的特征点
void FeatureTracker::drawTrack(const cv::Mat &imLeft, const cv::Mat &imRight,
vector<int> &curLeftIds,
vector<cv::Point2f> &curLeftPts,
vector<cv::Point2f> &curRightPts,
map<int, cv::Point2f> &prevLeftPtsMap)
{
//int rows = imLeft.rows;
int cols = imLeft.cols;
if (!imRight.empty() && stereo_cam)
cv::hconcat(imLeft, imRight, imTrack);
else
imTrack = imLeft.clone();
cv::cvtColor(imTrack, imTrack, CV_GRAY2RGB);
for (size_t j = 0; j < curLeftPts.size(); j++)
{
double len = std::min(1.0, 1.0 * track_cnt[j] / 20);
cv::circle(imTrack, curLeftPts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
}
if (!imRight.empty() && stereo_cam)
{
for (size_t i = 0; i < curRightPts.size(); i++)
{
cv::Point2f rightPt = curRightPts[i];
rightPt.x += cols;
cv::circle(imTrack, rightPt, 2, cv::Scalar(0, 255, 0), 2);
//cv::Point2f leftPt = curLeftPtsTrackRight[i];
//cv::line(imTrack, leftPt, rightPt, cv::Scalar(0, 255, 0), 1, 8, 0);
}
}
map<int, cv::Point2f>::iterator mapIt;
for (size_t i = 0; i < curLeftIds.size(); i++)
{
int id = curLeftIds[i];
mapIt = prevLeftPtsMap.find(id);
if(mapIt != prevLeftPtsMap.end())
{
cv::arrowedLine(imTrack, curLeftPts[i], mapIt->second, cv::Scalar(0, 255, 0), 1, 8, 0, 0.2);
}
}
}
//设置预测点,用前一帧的投影
void FeatureTracker::setPrediction(map<int, Eigen::Vector3d> &predictPts)
{
hasPrediction = true;
predict_pts.clear();
predict_pts_debug.clear();
map<int, Eigen::Vector3d>::iterator itPredict;
for (size_t i = 0; i < ids.size(); i++)
{
//printf("prevLeftId size %d prevLeftPts size %d\n",(int)prevLeftIds.size(), (int)prevLeftPts.size());
int id = ids[i];
itPredict = predictPts.find(id);
if (itPredict != predictPts.end())
{
Eigen::Vector2d tmp_uv;
m_camera[0]->spaceToPlane(itPredict->second, tmp_uv);
predict_pts.push_back(cv::Point2f(tmp_uv.x(), tmp_uv.y()));
predict_pts_debug.push_back(cv::Point2f(tmp_uv.x(), tmp_uv.y()));
}
else
predict_pts.push_back(prev_pts[i]);
}
}
// 根据的特征点的ID移除特征点
void FeatureTracker::removeOutliers(set<int> &removePtsIds)
{
std::set<int>::iterator itSet;
vector<uchar> status;
for (size_t i = 0; i < ids.size(); i++)
{
itSet = removePtsIds.find(ids[i]);
if(itSet != removePtsIds.end())
status.push_back(0);
else
status.push_back(1);
}
reduceVector(prev_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
}
cv::Mat FeatureTracker::getTrackImage()
{
return imTrack;
}光流法
关键点的提取和描述子的计算特别耗时，在实践中，SIFI无法进行实时的计算，而ORB的计算需要接近20毫秒才能完成，保留特征点，但只计算关键点，不计算描述子。使用光流法进行特征点之间的运动。
#include<iostream>
#include <fstream>
#include <list>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>
using namespace std;
int main(int argc, char **argv) {
if (argc != 2) {
cout << "usage: optical_flow data_set_path" << endl;
return 1;
}
// 数据的文件夹路径
string data_set_path = argv[1];
// 相机的位姿
string associate_file = data_set_path + "/associate.txt";
ifstream fin(associate_file);
string rgb_file, depth_file, time_rgb, time_depth;
// 特征点
list<cv::Point2f> key_points;
// 暂存的图像
cv::Mat color, depth, last_color;
for (int index = 0; index < 573; ++index) {
fin >> time_rgb >> rgb_file >> time_depth >> depth_file;
// 对应RGB文件
color = cv::imread(data_set_path + "/" + rgb_file);
depth = cv::imread(data_set_path + "/" + depth_file, -1);
if (index == 0) {
// 对第一帧提取FAST特征点
vector<cv::KeyPoint> kps;
cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
detector->detect(color, kps);
for (auto kp:kps) {
key_points.push_back(kp.pt);
}
last_color = color;
continue;
}
if (color.data == nullptr || depth.data == nullptr)
continue;
// 对其他帧用LK跟踪特征点
vector<cv::Point2f> next_key_points;
vector<cv::Point2f> prev_key_points;
for (const auto &kp:key_points) {
prev_key_points.push_back(kp);
}
vector<unsigned char> status;
vector<float> error;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
cv::calcOpticalFlowPyrLK(last_color, color, prev_key_points, next_key_points, status, error);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
auto time_used = chrono::duration_cast<chrono::duration<double >>(t2 - t1);
cout << index << "--LK Flow costs time: " << time_used.count() << " seconds." << endl;
// 删掉跟丢的点
int i = 0;
for (auto iter = key_points.begin(); iter != key_points.end(); i++) {
if (status[i] == 0) {
iter = key_points.erase(iter);
continue;
}
*iter = next_key_points[i];
iter++;
}
cout << "tracked key points: " << key_points.size() << endl;
if (key_points.empty()) {
cout << "all key points are lost." << endl;
break;
}
// 画出key points
cv::Mat img_show = color.clone();
for (const auto &kp:key_points) {
cv::circle(img_show, kp, 3, cv::Scalar(0, 240, 255), 1);
}
cv::imshow("corners", img_show);
cv::waitKey(0);
last_color = color;
}
return 0;
}cv::goodFeaturesToTrack
opencv中的goodFeaturesToTrack函数可以计算Harris角点和shi-tomasi角点，但默认情况下计算的是shi-tomasi角点，函数原型如下：
void cv::goodFeaturesToTrack( InputArray _image, OutputArray _corners,
int maxCorners, double qualityLevel, double minDistance,
InputArray _mask, int blockSize,
bool useHarrisDetector, double harrisK )- _image：8位或32位浮点型输入图像，单通道
- _corners：保存检测出的角点
- maxCorners：角点数目最大值，如果实际检测的角点超过此值，则只返回前maxCorners个强角点
- qualityLevel：角点的品质因子
- minDistance：对于初选出的角点而言，如果在其周围minDistance范围内存在其他更强角点，则将此角点删除
- _mask：指定感兴趣区，如不需在整幅图上寻找角点，则用此参数指定ROI
- blockSize：计算协方差矩阵时的窗口大小
- useHarrisDetector：指示是否使用Harris角点检测，如不指定，则计算shi-tomasi角点
- harrisK：Harris角点检测需要的k值