Skip to content

Commit

Permalink
添加 image 特征提取的注释
Browse files Browse the repository at this point in the history
  • Loading branch information
@lvhualong
lvhualong committed Mar 6, 2019
1 parent 46c9501 commit 5428cd6
Show file tree
Hide file tree
Showing 4 changed files with 36 additions and 15 deletions.
2 changes: 2 additions & 0 deletions camera_models/src/camera_models/EquidistantCamera.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -411,6 +411,7 @@ EquidistantCamera::estimateIntrinsics(const cv::Size& boardSize,
*
* \param p image coordinates
* \param P coordinates of the point on the sphere
* 2D u-v像素坐标 映射到球面上 3D point
*/
void
EquidistantCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const
Expand All @@ -423,6 +424,7 @@ EquidistantCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) cons
*
* \param p image coordinates
* \param P coordinates of the projective ray
* 3Dpoint
*/
void
EquidistantCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const
Expand Down
2 changes: 1 addition & 1 deletion config/mynteye-d/mynt_stereo_imu_config.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ body_T_cam1: !!opencv-matrix
multiple_thread: 1

#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
max_cnt: 150 # max feature number in feature tracking 光流+特征提取， 光流跟不够的用新提取的corners角点来补
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
Expand Down
33 changes: 25 additions & 8 deletions vins_estimator/src/featureTracker/feature_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@

#include "feature_tracker.h"

// return: true 内点， false 边缘点
bool FeatureTracker::inBorder(const cv::Point2f &pt)
{
const int BORDER_SIZE = 1;
Expand Down Expand Up @@ -52,6 +53,7 @@ FeatureTracker::FeatureTracker()
hasPrediction = false;
}

//通过mask去除边缘畸变比较大的点
void FeatureTracker::setMask()
{
mask = cv::Mat(row, col, CV_8UC1, cv::Scalar(255));
Expand Down Expand Up @@ -83,6 +85,7 @@ void FeatureTracker::setMask()
}
}

// 把光流没追够的，用corner角点补进去 n_pts + cur_pts == >track_cnt
void FeatureTracker::addPoints()
{
for (auto &p : n_pts)
Expand All @@ -93,6 +96,7 @@ void FeatureTracker::addPoints()
}
}

//两个像素点的欧式距离
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);
Expand All @@ -101,9 +105,15 @@ double FeatureTracker::distance(cv::Point2f &pt1, cv::Point2f &pt2)
return sqrt(dx * dx + dy * dy);
}

// ***********************************
// 特征点追踪：
// calcOpticalFlowPyrLK-->反向追踪去除外点-->去除图像边缘的特征点-->通过setMask去除边缘畸变比较大的点-->
// goodFeaturesToTrack 光流跟不够的用新提取的corners角点来补,使其达到每帧最大的特征点数量
// input : 某一时刻的左图，设置为cur_img
// ***********************************
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;
TicToc t_r; //计时
cur_time = _cur_time;
cur_img = _img;
row = cur_img.rows;
Expand All @@ -124,24 +134,28 @@ map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> FeatureTracker::trackIm
TicToc t_o;
vector<uchar> status;
vector<float> err;
if(hasPrediction)
if(hasPrediction) // has prediction 少跟踪一些
{
cur_pts = predict_pts;

//computes sparse optical flow using multi-scale Lucas-Kanade algorithm
//根据prev_img的prev_pts的特征点，在cur_img图像中预测cur_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++)
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
else // hasPrediction = false 多跟踪一些
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);
// reverse check

// reverse check 光流逆向追踪， 可能是为了去除outlire, 得到更稳定追踪的点
if(FLOW_BACK)
{
vector<uchar> reverse_status;
Expand All @@ -151,7 +165,7 @@ map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> FeatureTracker::trackIm
//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)
if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5) //正向和反向都追踪到了，而且距离没有异常
{
status[i] = 1;
}
Expand All @@ -161,14 +175,14 @@ map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> FeatureTracker::trackIm
}

for (int i = 0; i < int(cur_pts.size()); i++)
if (status[i] && !inBorder(cur_pts[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());
printf("track cnt %d\n", (int)ids.size());
}

for (auto &n : track_cnt)
Expand All @@ -177,13 +191,15 @@ map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> FeatureTracker::trackIm
if (1)
{
//rejectWithF();
// 通过setMask去除边缘畸变较大的点
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;
//光流跟不够的用新提取的corners角点来补,使其达到每帧最大的特征点数量
int n_max_cnt = MAX_CNT - static_cast<int>(cur_pts.size());
if (n_max_cnt > 0)
{
Expand All @@ -197,6 +213,7 @@ map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> FeatureTracker::trackIm
n_pts.clear();
ROS_DEBUG("detect feature costs: %fms", t_t.toc());

// 把光流没追够的，用corner角点补进去 n_pts + cur_pts == >track_cnt
ROS_DEBUG("add feature begins");
TicToc t_a;
addPoints();
Expand Down
14 changes: 8 additions & 6 deletions vins_estimator/src/featureTracker/feature_tracker.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ using namespace std;
using namespace camodocal;
using namespace Eigen;

bool inBorder(const cv::Point2f &pt);
bool inBorder(const cv::Point2f &pt); //判断关键点是否在边界上
void reduceVector(vector<cv::Point2f> &v, vector<uchar> status);
void reduceVector(vector<int> &v, vector<uchar> status);

Expand All @@ -38,7 +38,7 @@ 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 setMask();
void addPoints();
void readIntrinsicParameter(const vector<string> &calib_file);
void showUndistortion(const string &name);
Expand All @@ -64,22 +64,24 @@ class FeatureTracker
cv::Mat imTrack;
cv::Mat mask;
cv::Mat fisheye_mask;
cv::Mat prev_img, cur_img;
vector<cv::Point2f> n_pts;
cv::Mat prev_img, cur_img; //当前需要追踪的图和上一张图
vector<cv::Point2f> n_pts; //光流追踪不够的，需要Corner角点补的特征点
vector<cv::Point2f> predict_pts;
vector<cv::Point2f> predict_pts_debug;
vector<cv::Point2f> prev_pts, cur_pts, cur_right_pts;
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;

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;
vector<camodocal::CameraPtr> m_camera; //所选的相机模型
double cur_time;
double prev_time;
bool stereo_cam;
int n_id;
bool hasPrediction;
// cur_pts n_id ids track_cnt 描述当前图追踪的特征点
};

0 comments on commit 5428cd6

Please sign in to comment.