1. feature_tracker.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
|
#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 "parameters.h"
#include "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();
void readImage(const cv::Mat &_img,double _cur_time);
void setMask();
void addPoints();
bool updateID(unsigned int i);
void readIntrinsicParameter(const string &calib_file);
void showUndistortion(const string &name);
void rejectWithF();
void undistortedPoints();
cv::Mat mask;
cv::Mat fisheye_mask;
cv::Mat prev_img, cur_img, forw_img;
vector<cv::Point2f> n_pts;
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;
vector<int> track_cnt;
map<int, cv::Point2f> cur_un_pts_map;
map<int, cv::Point2f> prev_un_pts_map;
camodocal::CameraPtr m_camera;
double cur_time;
double prev_time;
static int n_id;
};
| cs |
Line 10 to 25
Line 10은 해당 파일이 한번만 컴파일되도록 한다.
Line 11은 입출력과 관련되어있다.
Line 12도 입출력과 관련되어있다.
Line 13는 queue와 관련되어 있는데, queue는 데이터를 효율적으로 관리할 수 있도록 도와주는 모듈이다.
Line 14은 디버깅을 위해 런타임 콜스택 정보를 얻을 수 있게 해준다. 자세한 내용은 아래 링크를 참고한다.
(링크 참조 : https://www.podovat.com/?p=412 )
Line 15는 프로그램이 실행 중에 다양한 시그널들을 어떻게 처리하는지를 정의한다. 자세한 내용은 다음 링크를 참고한다.
(링크 참조 : https://ko.wikipedia.org/wiki/C_%EC%8B%9C%EA%B7%B8%EB%84%90_%EC%B2%98%EB%A6%AC )
Line 16와 Line 17은 영상처리 및 선형대수 라이브러리를 포함시킨다.
Line 18부터 Line 20는 카메라 캘리브레이션을 위한 내용이다.
Line 21은 파라미터들을 설정할 때 사용된다.
Line 22은 초단위의 시간을 잴 때 사용된다.
Line 23부터 Line 25는 namespace 설정이다.
Line 26부터 28은 함수에 대한 내용인데, cpp 파일에서 좀 더 자세히 확인하자.
Line 29부터 Line 56은 FeatureTracker 클래스를 이용한 함수 및 변수들이 존재한다. cpp 파일에서 좀 더 자세히 확인하자.
2. feature_tracker.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
|
#include "feature_tracker.h"
int FeatureTracker::n_id = 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;
}
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()
{
}
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.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)
{
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);
}
}
}
void FeatureTracker::addPoints()
{
for (auto &p : n_pts)
{
forw_pts.push_back(p);
ids.push_back(-1);
track_cnt.push_back(1);
}
}
void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
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;
if (forw_img.empty())
{
prev_img = cur_img = forw_img = img;
}
else
{
forw_img = img;
}
forw_pts.clear();
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());
}
for (auto &n : track_cnt)
n++;
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;
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;
}
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());
}
}
bool FeatureTracker::updateID(unsigned int i)
{
if (i < ids.size())
{
if (ids[i] == -1)
ids[i] = n_id++;
return true;
}
else
return false;
}
void FeatureTracker::readIntrinsicParameter(const string &calib_file)
{
ROS_INFO("reading paramerter of camera %s", calib_file.c_str());
m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);
}
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->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));
}
}
cv::imshow(name, undistortedImg);
cv::waitKey(0);
}
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;
}
| cs |
Line 18 to 24
border는 경계를 의미한다. 즉, inborder는 내부경계를 의미한다.매개변수로는 cv::Point2f &pt 를 받게 되는데, 영상내의 어떠한 포인트를 의미한다.
cvRound는 0.6 이상의 수에 대해 반올림을 하는 함수다.
즉, pt.x라는 실수를 img_x라는 정수로, pt.y라는 실수를 img_y라는 정수로 바꾸는 것이다.
최종적으로는 매개변수로 전달되는 포인터가 영상의 경계에 머물지 않고 한 칸의 픽셀 이상 안쪽에 존재한다면 true를 반환하게 되는 것이다.
Line 25 to 32
reduceVector는 벡터는 vector<cv::Point2f> &v와 vector<uchar> status를 매개변수로 전달받는다. 해당 함수가 쓰이는 부분을 봐야하겠지만, 내 예상엔 다음과 같은 내용일 것 같다.v라는 매개변수에는 영상의 픽셀 값들이 들어갈 것이고, status라는 매개변수에는 해당 픽셀 값들이 key-frame인지 아닌지에 대한 내용이 들어갈 것이다. 그래서 최종적으로는 v 안에는 key-frame과 관련된 픽셀값들 만이 존재하게 될 것이다.
v.resize(j); 또한 key-frame이 존재하는 픽셀들의 수만큼 vector의 사이즈를 줄여주는 것이다.
Line 33 to 40
이 부분도 위의 내용과 비슷하지만 매개변수로 vector<int> &v를 받는 부분만 다르다.Line 41 to 43
클래스의 생성자지만, 아무것도 들어있진 않다.Line 44 to 72
먼저 Fisheye 설정을 확인한 뒤에 mask를 fisheye용으로 할지, 일반 영상용 mask를 할지 정하게 된다.이 후에 다음과 같은 변수를 만들게 되는데,
// 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.push_back(make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i])));
주석에 쓰여있다시피, 프레임마다 검출되는 features들 중 오랫동안 검출되는 features들만을 저장하게 된다.
자료형을 보면 pair가 있는데, pair는 두 개의 변수를 저장할 수 있는 struct이다.
그 이후의 내용을 보면 해당 변수에는 (track_cnt, forw_pts, ids) 세 개의 값이 들어가는 것을 알 수 있다.
forw_pts는 forw_img에서 검출된 여러 key-frame 중에 하나의 좌표값을 저장한다.
참고로 forw_img는 다음의 세 이미지 중에 하나로써, 연속된 세 개의 이미지 중 마지막 이미지를 의미하는 듯 하다.
cv::Mat prev_img, cur_img, forw_img;
track_cnt는 해당 key-frame이 검출된 횟수를 카운트한다.
id는 해당 key-frame의 인덱스이다..
다음으로 아래의 내용이 등장하는데,
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;
});
sort는 begin, end의 범위에 있는 인자들을 3번째 인자에 정의한 함수를 기준으로 정렬을 할 수 있다.
즉, 여기서는 cnt_pts_id에 저장되어있는 벡터들을 정렬시키는데, 그 기준은 first 끼리의 비교니까, track_cnt의 갯수로 정렬을 시키게 되는 것이다.
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);
}
}
위 내용은 cnt_pts_id 라는 벡터에서 하나의 벡터씩 꺼내와 it에 넣으면서 for문을 돌리는 것이다.
이 후에, mask.at<uchar>(it.second.first) 라는 부분이 있는데,
mask는 cv::Mat 형태의 데이터이고, mask.at<uchar> 의 자료형인 것으로 보아서, 영상 데이터에 벡터 형태로 데이터가 대입되는 것 같다. 그러므로 it.second.first인 forw_pts는 영상 이미지에서의 하나의 key-frame의 좌표값인 것이다.
mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));
초기에 마스크 설정을 통해서 Mat 형태의 영상 데이터인 mask에는 각 픽셀값이 255로 차는 것을 확인했으니, 해당 if문은 무조건 동작하게 되어있고,
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);
해당 부분에서 메모리 해제되었던 forw_pts, ids, track_cnt에 track_cnt 순으로 정렬되었던 내용을 순차적으로 담으면서 해당 key-frame에는 원을 그려주어서 특징점을 표시하는 것을 확인할 수 있다.
Line 73 to 81
앞부분에서 forw_pts, ids, track_cnt의 데이터를 cnt_pts_ids에 담았었는데,여기의 함수 addPointst를 사용해서 n_pts로부터 forw_pts, ids, track_cnt 의 값을 채워주게 된다.
여기서 중요한 부분은 n_pts는 특징점 검출로 발견된게 아니라, 코너점 검출로 발견된 points란 것이다.
Line 82 to 159
void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)readImage 함수인 만큼, 여기에서 영상데이터를 받아 여러 작업이 수행 될 것이다.
매개변수로는 const cv::Mat &_img와 double _cur_time을 받게 되는데,
_img가 입력되는 이미지, _cur_time은 확실친 않지만 readImage가 동작할 때의 시간을 의미하는 것 같다.
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;
Equalize는 평활화를 의미한다. 평활화 옵션이 켜져있다면 평활화를 한 이미지를 사용하고, 그렇지 않다면 원래의 이미지를 사용한다.
CLAHE 하고 쓰여져 있는 것은 Contrast Limited Adaptive Histogram Equalization 의 약자이다. 보통 평활화를 하면 영상 데이터의 대비가 더 커지고 밝고 선명해진다.
자세한 내용은 아래 링크를 참조하자.
( 링크 참조 : https://m.blog.naver.com/samsjang/220543360864 )
if (forw_img.empty())
{
prev_img = cur_img = forw_img = img;
}
else
{
forw_img = img;
}
만약 forw_img가 없는 상태라면 현재 받아온 이미지를 prev_img, cur_img, forw_img로 만든다.
forw_img가 있다면, 현재 받아온 이미지가 forw_img가 된다.
forw_pts.clear();
forw_pts (key-frame의 좌표가 저장됨)을 메모리 해제해준다.
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());
}
만약 cur_img의 cur_pts에 key-frame이 존재한다면 해당 조건문을 실행시켜준다.
TicToc t_o;
vector<uchar> status;
vector<float> err;
해당 부분은 선언이다. status와 err가 선언되는것을 볼 수 있다.
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
오 이거 신기하다, 카메라와 피사체의 상대 운동에 의하여 발생하는 피사체의 운동에 대한 패턴을 검출한다. 라고한다.
PyrLK(Pyramid Lucas Kanade) 방법은 입력 이미지와 피라미드 이미지를 사용하여 코너를 기준으로 광학 흐름을 검출한다.
매개변수 내용은 다음과 같다.
calcOpticalFlowPyrLK(이전 프레임, 현재 프레임, 이전 프레임 코너 검출점, 계산된 현재 프레임 코너 검출점, 계산된 현재 프레임의 각 픽셀 상태(코너점인지 아닌지)가 기록된 프레임으로써 feature가 존재할 시 1이고 존재하지 않을시 0이 된다, 계산된 현재 프레임의 각 픽셀 에러(플래그에 따라 어떤 에러를 의미할지 결정)가 기록된 프레임, 각 피라미드 레벨에 따른 search window의 사이즈, max level(0으로 할 시, 피라미드는 사용되지 않고 단일 레벨에서 동작하고, 1로 할 시, 2 level로 동작한다. 3으로 할 시, 당연히 3 level로 동작한다.))
자세한 내용은 아래 링크를 참조하자.
(링크 참조 : https://docs.opencv.org/2.4/modules/video/doc/motion_analysis_and_object_tracking.html )
for (int i = 0; i < int(forw_pts.size()); i++)
if (status[i] && !inBorder(forw_pts[i]))
status[i] = 0;
해당 부분에서는 현재 이미지에서 검출된 특징점인 forw_pts들 중 status가 1(특징점이면 1임)이 아니거나 inBoder(경계로부터 1픽셀 안쪽에 존재하는지 확인하는 함수)인지 확인하고, 특징점이 아니고 경계 바깥에 존재한다면 status를 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);
reduceVector 함수에서는 prev_pts, cur_pts, forw_pts, ids, cur__un_pts, track_cnt 에 존재하는 status가 0인 벡터들을 없애버림.
ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
오, 이런 식으로 시간을 잼. TicToc class 는 생성자에서 시간체크가 시작되고, toc() 함수에서 종료시간을 계산함.
for (auto &n : track_cnt)
n++;
검출된 횟수를 늘리는 것임.
if (PUB_THIS_FRAME)
이 프레임을 Publish 한다는 플래그가 true라면 해당 프레임을 publish 할 것임.
어떤 프레임 인지는 조건문 안을 자세히 봐야 알 것 같음.
rejectWithF();
이 부분은 잠시 후에 살펴보기로 함. 아마 뭔가 지운다는 것 같음.
ROS_DEBUG("set mask begins");
TicToc t_m;
setMask();
ROS_DEBUG("set mask costs %fms", t_m.toc());
아 set mask가 여기서 쓰이는구나.
set mask의 역할 중 하나가 forw_pts를 track_cnt 순으로 정렬시키는 것이니 여기서 정렬을 시켜주고 오겠구나.
걸리는 시간도 계산하고.
ROS_DEBUG("detect feature begins");
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());
먼저 한 이미지당 최대로 검출될 특징점의 갯수에서 현재 이미지에서 검출된 특징점의 개수를 뺀 뒤, 해당 값이 0보다 크면, 즉 현재 이미지에서 검출된 특징점의 개수가 특징점 최대 검출 개수를 넘지 않는다면, 조건문을 발동시킨다.
혹시 넘는다면, n_pts를 초기화시킨다.
조건문 안을 살펴보면 앞부분은 mask의 세팅을 점검하는 내용이고,
이 부분을 보자.
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
이 부분은 코너를 검출하는 내용이다. 흠... 피라미드를 이용한 특징점 검출 후에 코너를 검출하게 되는걸까?
일단 매개변수를 살펴보자.
forw_img : 입력 이미지이다.
n_pts : 검출된 코너를 담을 벡터이다.
MAX_CNT : 반환될 코너의 최대 개수이다. MAX_CNT - forw_pts.size() 라고 되어있는데, 즉 특징점을 먼저 찾고도 MAX_CNT가 되지 않는다면 이 부분에서 코너점을 검출하는 거라고 보면 될 것 같다.
0.01 : 이 부분은 이론을 살펴봐야 겠지만, 간단히 이야기하면 코너라고 판단하기 위한 기준이 되는 최소의 값이라고 한다.
MIN_DIS : 반환되는 코너 사이의 최소 유클라디안 거리이다. 이 값이 작다면 한 코너에 무수한 포인트들이 생길 우려도 있기 때문이다.
mask : 코너를 찾을 관심영역이며, input과 크기가 동일해야 한다. 해당 마스크는 setMask에서 만들어진다.
자세한 설명에 대해서는 아래 링크를 참조하자.
(링크 참조 : https://m.blog.naver.com/PostView.nhn?blogId=pckbj123&logNo=100203116086&proxyReferer=http%3A%2F%2F59.14.144.11%2Ftm%2F%3Fa%3DCR%26b%3DWIN%26c%3D300023730305%26d%3D32%26e%3D3203%26f%3DbS5ibG9nLm5hdmVyLmNvbS9wY2tiajEyMy8xMDAyMDMxMTYwODY%3D%26g%3D1584063427533%26h%3D1584063426391%26y%3D0%26z%3D0%26x%3D1%26w%3D2019-10-25%26in%3D3203_1037_00008751%26id%3D20200313 )
ROS_DEBUG("add feature begins");
TicToc t_a;
addPoints();
ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
아 이제 알겠다. addPoints 함수는 특징점으로 검출된 points가 아니라, 코너점 검출로 발견한 points들을 forw_pts에 추가해주는 것이다.
prev_img = cur_img;
prev_pts = cur_pts;
prev_un_pts = cur_un_pts;
cur_img = forw_img;
cur_pts = forw_pts;
아아 해당 부분에서 forw에서 cur로 cur에서 prev로 한 칸씩 미뤄준다.
undistortedPoints();
prev_time = cur_time;
시간도 미뤄준다.
undistortedPoints()는 밑에서 다루자.
Line 160 to 191
void FeatureTracker::rejectWithF()대체 뭘 reject 하는걸까, 특징점 검출된 후의 데이터를 이용하는 것 같던데, 한번 살펴보자.
if (forw_pts.size() >= 8)
일단 특징점이 적어도 8개는 되어야 조건문이 동작한다.
ROS_DEBUG("FM ransac begins");
아 ransac 알고리즘이었구나. 검색해보니까 FM ransac은 feature matching ransac인 것 같다.
기본적으로 ransac 알고리즘은 컨센서스가 최대치인 모델을 뽑기 위한 절차적 방법을 말한다. outlier를 배제시키는데 많은 도움이 된다.
자세한 내용은 아래 링크를 참고하자.
(링크 참조 : https://darkpgmr.tistory.com/61 )
vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());
cur_pts의 크기만큼 un_cur_pts 벡터를 만들고, forw_pts의 크기만큼 un_forw_pts 벡터를 만든다.
for (unsigned int i = 0; i < cur_pts.size(); i++)
cur_pts의 크기만큼 반복문이 실행된다.
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());
핀홀카메라 모델에서 Image와 Scene에서의 좌표값은 위 그림과 같이 표현될 수 있다.
즉, 여기서는 우리가 원래 알고있는 Image Point의 좌표값을 liftProjective를 통해 Scene Point의 좌표값으로 만들어주고 이를 다시 역변환 하여 image point로 만들어주는 것이다.
Image의 특징점의 좌표를 Scene 좌표에 매핑하는 것은 왜곡을 처리하는 과정을 포함한다고 한다.
그래서 최종적으로는 이 과정을 통해 왜곡이 포함된 cur_pts, forw_pts로부터 왜곡이 제거된 un_cur_pts와 un_forw_pts를 얻어냈다고 보면 되겠다.
아마 un 자체가 undistorted를 의미하는 것 같다.
(링크 참조 : http://www.cse.psu.edu/~rtc12/CSE486/lecture12.pdf )
vector<uchar> status;
cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
RANSAC 알고리즘을 수행하는 함수이다. 최종적으로 status에 outlier가 제거된 내용만 남게된다.
자세한 내용은 다음 링크를 참조한다.
( 링크 참조 : https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#Mat%20findFundamentalMat(InputArray%20points1,%20InputArray%20points2,%20int%20method,%20double%20param1,%20double%20param2,%20OutputArray%20mask) )
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(cur_un_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
이 후에, reduceVector를 통해 outlier가 제거된 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());
걸린 시간을 표시하게 되고, ransac을 통하여 특징점이 얼마나 제거되었는지 표시한다.
Line 192 to 202
ids의 값이 null(-1)일 때, 새로운 id를 부여해주기위한 함수인듯하다.Line 203 to 207
내부 파라미터 값을 읽어오기 위한 함수.Line 208 to 242
void FeatureTracker::showUndistortion(const string &name)왜곡보정한 내용을 보여준다고 한다. 무슨 내용일까?
cv::Mat undistortedImg(ROW + 600, COL + 600, CV_8UC1, cv::Scalar(0));
원래 영상보다 가로 세로로 600씩 크며, 픽셀들이 스칼라 0값을 갖는 Mat 데이터를 생성한다.
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->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());
}
앞 부분에서, liftProjective를 이용해서 왜곡을 보정한다고 이야기했다. 여기서도 왜곡을 보정하는 역할을 한다.
그렇게 a는 왜곡이 있는 포인트가 되고, b는 왜곡이 보정된 포인트가 된다.
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;
해당부분은 Image Point로 변환하는 과정이다.
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));
}
흠, 이부분에서 undistortedImg를 만들어주는 것 같은데, 솔직히 의미도 잘 모르겠고,
내가 보기에 showUnditorted 함수 자체가 완벽하지 않다. 아무래도 참고용으로 존재하는 함수인거 같고, 실제론 쓰이지 않는 듯 하다.
Line 243 to 291
void FeatureTracker::undistortedPoints()함수명으로 미루어보았을 때, 왜곡보정된 포인트에 대한 내용 같다.
cur_un_pts.clear();
cur_un_pts_map.clear();
current undistorted Points 라는 의미인 듯 하다.
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);
}
a에서 b로 liftProjective를 사용하여 왜곡을 보정하고, 이후에는 다시 사영과정을 진행해 cur_un_pts와 cur_un_pts_map을 구해준다. cur_un_pts_map은 ids가 존재한다.
if (!prev_un_pts_map.empty())
해당 조건문은 prev_un_pts_map에 데이터가 존재한다면 작동하게 되어있다.
double dt = cur_time - prev_time;
pts_velocity.clear();
현재 시간에서 과거 시간을 빼서 dt을 구해준다.
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));
}
}
cur_un_pts의 크기만큼 반복문이 돌아가며,
ids가 -1이거나 prev_un_pts_map이 끝까지 반복된 경우, 해당부분의 velocity는 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;
만약 prev_un_pts_map이 비어있는 경우 속도를 0으로 만들고
cur_un_pts_map을 prev_un_pts_map으로 만들어준다.
댓글 없음:
댓글 쓰기