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
|
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Bool.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include "feature_tracker.h"
#define SHOW_UNDISTORTION 0
vector<uchar> r_status;
vector<float> r_err;
queue<sensor_msgs::ImageConstPtr> img_buf;
ros::Publisher pub_img,pub_match;
ros::Publisher pub_restart;
FeatureTracker trackerData[NUM_OF_CAM];
double first_image_time;
int pub_count = 1;
bool first_image_flag = true;
double last_image_time = 0;
bool init_pub = 0;
void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
if(first_image_flag)
{
first_image_flag = false;
first_image_time = img_msg->header.stamp.toSec();
last_image_time = img_msg->header.stamp.toSec();
return;
}
// detect unstable camera stream
if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)
{
ROS_WARN("image discontinue! reset the feature tracker!");
first_image_flag = true;
last_image_time = 0;
pub_count = 1;
std_msgs::Bool restart_flag;
restart_flag.data = true;
pub_restart.publish(restart_flag);
return;
}
last_image_time = img_msg->header.stamp.toSec();
// frequency control
if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{
PUB_THIS_FRAME = true;
// reset the frequency control
if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
{
first_image_time = img_msg->header.stamp.toSec();
pub_count = 0;
}
}
else
PUB_THIS_FRAME = false;
cv_bridge::CvImageConstPtr ptr;
if (img_msg->encoding == "8UC1")
{
sensor_msgs::Image img;
img.header = img_msg->header;
img.height = img_msg->height;
img.width = img_msg->width;
img.is_bigendian = img_msg->is_bigendian;
img.step = img_msg->step;
img.data = img_msg->data;
img.encoding = "mono8";
ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
}
else
ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
cv::Mat show_img = ptr->image;
TicToc t_r;
for (int i = 0; i < NUM_OF_CAM; i++)
{
ROS_DEBUG("processing camera %d", i);
if (i != 1 || !STEREO_TRACK)
trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());
else
{
if (EQUALIZE)
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();
clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);
}
else
trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));
}
#if SHOW_UNDISTORTION
trackerData[i].showUndistortion("undistrotion_" + std::to_string(i));
#endif
}
for (unsigned int i = 0;; i++)
{
bool completed = false;
for (int j = 0; j < NUM_OF_CAM; j++)
if (j != 1 || !STEREO_TRACK)
completed |= trackerData[j].updateID(i);
if (!completed)
break;
}
if (PUB_THIS_FRAME)
{
pub_count++;
sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
sensor_msgs::ChannelFloat32 id_of_point;
sensor_msgs::ChannelFloat32 u_of_point;
sensor_msgs::ChannelFloat32 v_of_point;
sensor_msgs::ChannelFloat32 velocity_x_of_point;
sensor_msgs::ChannelFloat32 velocity_y_of_point;
feature_points->header = img_msg->header;
feature_points->header.frame_id = "world";
vector<set<int>> hash_ids(NUM_OF_CAM);
for (int i = 0; i < NUM_OF_CAM; i++)
{
auto &un_pts = trackerData[i].cur_un_pts;
auto &cur_pts = trackerData[i].cur_pts;
auto &ids = trackerData[i].ids;
auto &pts_velocity = trackerData[i].pts_velocity;
for (unsigned int j = 0; j < ids.size(); j++)
{
if (trackerData[i].track_cnt[j] > 1)
{
int p_id = ids[j];
hash_ids[i].insert(p_id);
geometry_msgs::Point32 p;
p.x = un_pts[j].x;
p.y = un_pts[j].y;
p.z = 1;
feature_points->points.push_back(p);
id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
u_of_point.values.push_back(cur_pts[j].x);
v_of_point.values.push_back(cur_pts[j].y);
velocity_x_of_point.values.push_back(pts_velocity[j].x);
velocity_y_of_point.values.push_back(pts_velocity[j].y);
}
}
}
feature_points->channels.push_back(id_of_point);
feature_points->channels.push_back(u_of_point);
feature_points->channels.push_back(v_of_point);
feature_points->channels.push_back(velocity_x_of_point);
feature_points->channels.push_back(velocity_y_of_point);
ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec());
// skip the first image; since no optical speed on frist image
if (!init_pub)
{
init_pub = 1;
}
else
pub_img.publish(feature_points);
if (SHOW_TRACK)
{
ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8);
//cv::Mat stereo_img(ROW * NUM_OF_CAM, COL, CV_8UC3);
cv::Mat stereo_img = ptr->image;
for (int i = 0; i < NUM_OF_CAM; i++)
{
cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW);
cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB);
for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++)
{
double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
//draw speed line
/*
Vector2d tmp_cur_un_pts (trackerData[i].cur_un_pts[j].x, trackerData[i].cur_un_pts[j].y);
Vector2d tmp_pts_velocity (trackerData[i].pts_velocity[j].x, trackerData[i].pts_velocity[j].y);
Vector3d tmp_prev_un_pts;
tmp_prev_un_pts.head(2) = tmp_cur_un_pts - 0.10 * tmp_pts_velocity;
tmp_prev_un_pts.z() = 1;
Vector2d tmp_prev_uv;
trackerData[i].m_camera->spaceToPlane(tmp_prev_un_pts, tmp_prev_uv);
cv::line(tmp_img, trackerData[i].cur_pts[j], cv::Point2f(tmp_prev_uv.x(), tmp_prev_uv.y()), cv::Scalar(255 , 0, 0), 1 , 8, 0);
*/
//char name[10];
//sprintf(name, "%d", trackerData[i].ids[j]);
//cv::putText(tmp_img, name, trackerData[i].cur_pts[j], cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0));
}
}
//cv::imshow("vis", stereo_img);
//cv::waitKey(5);
pub_match.publish(ptr->toImageMsg());
}
}
ROS_INFO("whole feature tracker processing costs: %f", t_r.toc());
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "feature_tracker");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
readParameters(n);
for (int i = 0; i < NUM_OF_CAM; i++)
trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);
if(FISHEYE)
{
for (int i = 0; i < NUM_OF_CAM; i++)
{
trackerData[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0);
if(!trackerData[i].fisheye_mask.data)
{
ROS_INFO("load mask fail");
ROS_BREAK();
}
else
ROS_INFO("load mask success");
}
}
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
pub_restart = n.advertise<std_msgs::Bool>("restart",1000);
/*
if (SHOW_TRACK)
cv::namedWindow("vis", cv::WINDOW_NORMAL);
*/
ros::spin();
return 0;
}
// new points velocity is 0, pub or not?
// track cnt > 1 pub?
| cs |
Line 200 to 232
ros::init(argc, argv, "feature_tracker");ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
readParameters(n);
feature_tracker를 클래스로하는 노드를 만들고 파라미터값들을 불러온다.
for (int i = 0; i < NUM_OF_CAM; i++)
trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);
카메라의 갯수만큼 내부파라미터를 불러온다.
if(FISHEYE)
{
for (int i = 0; i < NUM_OF_CAM; i++)
{
trackerData[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0);
if(!trackerData[i].fisheye_mask.data)
{
ROS_INFO("load mask fail");
ROS_BREAK();
}
else
ROS_INFO("load mask success");
}
}
Fisheye 관련 설정이다. 지금은 쓰지 않으니 킵한다.
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
image를 0.1초 단위로 수신하는 서브스크라이버를 만든다.
image를 획득하면 img_callback 함수가 동작한다.
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
pub_restart = n.advertise<std_msgs::Bool>("restart",1000);
총 세 개의 퍼블리셔를 정의한다.
pub_img 는 feature cloud data
pub_match 는 feature가 표시된 image
pub_restart 는 reset 신호가 된다.
Line 34 to 199
void img_callback(const sensor_msgs::ImageConstPtr &img_msg)img를 수신하는 callback 함수이다.
if(first_image_flag)
{
first_image_flag = false;
first_image_time = img_msg->header.stamp.toSec();
last_image_time = img_msg->header.stamp.toSec();
return;
}
해당 이미지가 첫 이미지라면 시간을 저장한다.
// detect unstable camera stream
if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)
{
ROS_WARN("image discontinue! reset the feature tracker!");
first_image_flag = true;
last_image_time = 0;
pub_count = 1;
std_msgs::Bool restart_flag;
restart_flag.data = true;
pub_restart.publish(restart_flag);
return;
}
만약 이미지가 정상적인 패턴으로 수신되지 않는다면 설정을 초기화하고 리셋한다.
last_image_time = img_msg->header.stamp.toSec();
first image가 아닌 경우에는 last_image_time이 기록되게 된다.
// frequency control
if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{
PUB_THIS_FRAME = true;
// reset the frequency control
if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
{
first_image_time = img_msg->header.stamp.toSec();
pub_count = 0;
}
}
else
PUB_THIS_FRAME = false;
frequency를 체크한다. pub_count는 노드가 동작하고 마지막에 publishing 작업까지 진행하면 1이 증가하고, 이를 현재까지의 시간으로 나누게 되면 frequency를 얻을 수 있다.
이 때, FREQ의 값은 parameters.cpp에서 받아오게 된다.
계산된 frequency가 FREQ보다 작다면 frame을 publish하게 되고, 크다면 frame을 publish 하지 않도록 하여 publish frequency를 적절히 조절해준다.
또한 frequency가 FREQ보다 작은 경우에 frequency와 FREQ의 차이가 FREQ의 1퍼센트정도밖에 안된다면 별 차이가 없는 것이니 시간과 pub_count를 리셋시켜서 다시 frequency를 측정한다.
cv_bridge::CvImageConstPtr ptr;
if (img_msg->encoding == "8UC1")
{
sensor_msgs::Image img;
img.header = img_msg->header;
img.height = img_msg->height;
img.width = img_msg->width;
img.is_bigendian = img_msg->is_bigendian;
img.step = img_msg->step;
img.data = img_msg->data;
img.encoding = "mono8";
ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
}
else
ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
영상데이터를 확인하여 MONO8 엔코딩으로 바꾸어서 ptr에 대입해준다.
cv::Mat show_img = ptr->image;
TicToc t_r;
for (int i = 0; i < NUM_OF_CAM; i++)
{
ROS_DEBUG("processing camera %d", i);
if (i != 1 || !STEREO_TRACK)
trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());
else
{
if (EQUALIZE)
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();
clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);
}
else
trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));
}
#if SHOW_UNDISTORTION
trackerData[i].showUndistortion("undistrotion_" + std::to_string(i));
#endif
}
Mat 형태의 show_img에 MONO8 엔코딩된 ptr->image 를 담아주고,
카메라의 개수만큼 반복문을 돌려준다.
반복문 안에서 if문의 경우엔 단안 카메라의 이미지를 읽어오는 과정이고, else문의 경우엔 스테레오 카메라의 작업을 위한 내용이다.
마지막 부분에 showUndistortion을 통해 왜곡보정된 이미지를 보여주게 되는데, 이 때, 왜곡보정은 liftProjective를 통해 이루어진다.
for (unsigned int i = 0;; i++)
{
bool completed = false;
for (int j = 0; j < NUM_OF_CAM; j++)
if (j != 1 || !STEREO_TRACK)
completed |= trackerData[j].updateID(i);
if (!completed)
break;
}
각 카메라의 ID를 업데이트 해주는 부분
if (PUB_THIS_FRAME)
{
pub_count++;
sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
sensor_msgs::ChannelFloat32 id_of_point;
sensor_msgs::ChannelFloat32 u_of_point;
sensor_msgs::ChannelFloat32 v_of_point;
sensor_msgs::ChannelFloat32 velocity_x_of_point;
sensor_msgs::ChannelFloat32 velocity_y_of_point;
선언부분
feature_points->header = img_msg->header;
feature_points->header.frame_id = "world";
feature_points의 프레임은 world.
vector<set<int>> hash_ids(NUM_OF_CAM);
for (int i = 0; i < NUM_OF_CAM; i++)
{
auto &un_pts = trackerData[i].cur_un_pts;
auto &cur_pts = trackerData[i].cur_pts;
auto &ids = trackerData[i].ids;
auto &pts_velocity = trackerData[i].pts_velocity;
for (unsigned int j = 0; j < ids.size(); j++)
{
if (trackerData[i].track_cnt[j] > 1)
{
int p_id = ids[j];
hash_ids[i].insert(p_id);
geometry_msgs::Point32 p;
p.x = un_pts[j].x;
p.y = un_pts[j].y;
p.z = 1;
feature_points->points.push_back(p);
id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
u_of_point.values.push_back(cur_pts[j].x);
v_of_point.values.push_back(cur_pts[j].y);
velocity_x_of_point.values.push_back(pts_velocity[j].x);
velocity_y_of_point.values.push_back(pts_velocity[j].y);
}
}
}
feature_points->channels.push_back(id_of_point);
feature_points->channels.push_back(u_of_point);
feature_points->channels.push_back(v_of_point);
feature_points->channels.push_back(velocity_x_of_point);
feature_points->channels.push_back(velocity_y_of_point);
ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec());
vector의 hash_ids는 정수형의 짧고 고유한 id를 생성하는 함수이다.
이후에 un_pts, cur_pts, ids, pts_velocity에 원래 영상이 지니고있는 값들을 넣어준다.
그리고 반복문을 통해 hash_ids의 값들도 원래 id 값으로 채워주고, geometry_msgs::Point32 형태의 p도 만들어 준다. (ROS 메세지 형태)
이후에 PointCloud 형태의 feature_points에 각 p들을 넣어주게 되며, 이 때, 다른 값들도 함께 넣어주게 된다.
최종적으로 feature_points 에 모든 데이터를 취합해주게 된다.
// skip the first image; since no optical speed on frist image
if (!init_pub)
{
init_pub = 1;
}
else
pub_img.publish(feature_points);
그리고 속도가 존재하는 두 번째 이미지부터 publish 한다.
if (SHOW_TRACK)
{
ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8);
//cv::Mat stereo_img(ROW * NUM_OF_CAM, COL, CV_8UC3);
cv::Mat stereo_img = ptr->image;
for (int i = 0; i < NUM_OF_CAM; i++)
{
cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW);
cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB);
for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++)
{
double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
//draw speed line
/*
Vector2d tmp_cur_un_pts (trackerData[i].cur_un_pts[j].x, trackerData[i].cur_un_pts[j].y);
Vector2d tmp_pts_velocity (trackerData[i].pts_velocity[j].x, trackerData[i].pts_velocity[j].y);
Vector3d tmp_prev_un_pts;
tmp_prev_un_pts.head(2) = tmp_cur_un_pts - 0.10 * tmp_pts_velocity;
tmp_prev_un_pts.z() = 1;
Vector2d tmp_prev_uv;
trackerData[i].m_camera->spaceToPlane(tmp_prev_un_pts, tmp_prev_uv);
cv::line(tmp_img, trackerData[i].cur_pts[j], cv::Point2f(tmp_prev_uv.x(), tmp_prev_uv.y()), cv::Scalar(255 , 0, 0), 1 , 8, 0);
*/
//char name[10];
//sprintf(name, "%d", trackerData[i].ids[j]);
//cv::putText(tmp_img, name, trackerData[i].cur_pts[j], cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0));
}
}
//cv::imshow("vis", stereo_img);
//cv::waitKey(5);
pub_match.publish(ptr->toImageMsg());
}
}
ROS_INFO("whole feature tracker processing costs: %f", t_r.toc());
해당 부분은 feature fracking한 부분을 출력하는 내용이다.
double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
이 부분을 보면, track한 횟수가 많은 만큼 더 큰 도형을 그리게 되는것을 알 수 있다.
댓글 없음:
댓글 쓰기