1 /*
2 * cv_feature_match.cpp - optical flow feature match
3 *
4 * Copyright (c) 2016-2017 Intel Corporation
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 *
18 * Author: Wind Yuan <feng.yuan@intel.com>
19 * Author: Yinhang Liu <yinhangx.liu@intel.com>
20 */
21
22 #include "cv_feature_match.h"
23 #include "xcam_obj_debug.h"
24 #include "image_file_handle.h"
25 #include "cl_utils.h"
26
27 #define XCAM_CV_FM_DEBUG 0
28 #define XCAM_CV_OF_DRAW_SCALE 2
29
30 namespace XCam {
31 #if XCAM_CV_FM_DEBUG
32 static void debug_write_image (
33 const SmartPtr<VideoBuffer> &buf, const Rect &rect, char *img_name, char *frame_str, char *fm_idx_str);
34 #endif
35
CVFeatureMatch()36 CVFeatureMatch::CVFeatureMatch ()
37 : CVBaseClass ()
38 , FeatureMatch ()
39 {
40 XCAM_ASSERT (_cv_context.ptr ());
41 }
42
43 bool
get_crop_image(const SmartPtr<VideoBuffer> & buffer,const Rect & crop_rect,cv::UMat & img)44 CVFeatureMatch::get_crop_image (
45 const SmartPtr<VideoBuffer> &buffer, const Rect &crop_rect, cv::UMat &img)
46 {
47 SmartPtr<CLBuffer> cl_buffer = convert_to_clbuffer (_cv_context->get_cl_context (), buffer);
48 VideoBufferInfo info = buffer->get_video_info ();
49 cl_mem cl_mem_id = cl_buffer->get_mem_id ();
50
51 cv::UMat umat;
52 cv::ocl::convertFromBuffer (cl_mem_id, info.strides[0], info.height, info.width, CV_8U, umat);
53 if (umat.empty ()) {
54 XCAM_LOG_ERROR ("FeatureMatch(idx:%d): convert bo buffer to UMat failed", _fm_idx);
55 return false;
56 }
57
58 img = umat (cv::Rect(crop_rect.pos_x, crop_rect.pos_y, crop_rect.width, crop_rect.height));
59
60 return true;
61 }
62
63 void
add_detected_data(cv::InputArray image,cv::Ptr<cv::Feature2D> detector,std::vector<cv::Point2f> & corners)64 CVFeatureMatch::add_detected_data (
65 cv::InputArray image, cv::Ptr<cv::Feature2D> detector, std::vector<cv::Point2f> &corners)
66 {
67 std::vector<cv::KeyPoint> keypoints;
68 detector->detect (image, keypoints);
69 corners.reserve (corners.size () + keypoints.size ());
70 for (size_t i = 0; i < keypoints.size (); ++i) {
71 cv::KeyPoint &kp = keypoints[i];
72 corners.push_back (kp.pt);
73 }
74 }
75
76 void
get_valid_offsets(std::vector<cv::Point2f> & corner0,std::vector<cv::Point2f> & corner1,std::vector<uchar> & status,std::vector<float> & error,std::vector<float> & offsets,float & sum,int & count,cv::InputOutputArray debug_img,cv::Size & img0_size)77 CVFeatureMatch::get_valid_offsets (
78 std::vector<cv::Point2f> &corner0, std::vector<cv::Point2f> &corner1,
79 std::vector<uchar> &status, std::vector<float> &error,
80 std::vector<float> &offsets, float &sum, int &count,
81 cv::InputOutputArray debug_img, cv::Size &img0_size)
82 {
83 count = 0;
84 sum = 0.0f;
85 for (uint32_t i = 0; i < status.size (); ++i) {
86 if (!status[i])
87 continue;
88
89 #if XCAM_CV_FM_DEBUG
90 cv::Point start = cv::Point(corner0[i]) * XCAM_CV_OF_DRAW_SCALE;
91 cv::circle (debug_img, start, 4, cv::Scalar(255), XCAM_CV_OF_DRAW_SCALE);
92 #endif
93 if (error[i] > _config.max_track_error)
94 continue;
95 if (fabs(corner0[i].y - corner1[i].y) >= _config.max_valid_offset_y)
96 continue;
97 if (corner1[i].x < 0.0f || corner1[i].x > img0_size.width)
98 continue;
99
100 float offset = corner1[i].x - corner0[i].x;
101 sum += offset;
102 ++count;
103 offsets.push_back (offset);
104
105 #if XCAM_CV_FM_DEBUG
106 cv::Point end = (cv::Point(corner1[i]) + cv::Point (img0_size.width, 0)) * XCAM_CV_OF_DRAW_SCALE;
107 cv::line (debug_img, start, end, cv::Scalar(255), XCAM_CV_OF_DRAW_SCALE);
108 #else
109 XCAM_UNUSED (debug_img);
110 XCAM_UNUSED (img0_size);
111 #endif
112 }
113 }
114
115 void
calc_of_match(cv::InputArray image0,cv::InputArray image1,std::vector<cv::Point2f> & corner0,std::vector<cv::Point2f> & corner1,std::vector<uchar> & status,std::vector<float> & error,int & last_count,float & last_mean_offset,float & out_x_offset)116 CVFeatureMatch::calc_of_match (
117 cv::InputArray image0, cv::InputArray image1,
118 std::vector<cv::Point2f> &corner0, std::vector<cv::Point2f> &corner1,
119 std::vector<uchar> &status, std::vector<float> &error,
120 int &last_count, float &last_mean_offset, float &out_x_offset)
121 {
122 cv::_InputOutputArray debug_img;
123 cv::Size img0_size = image0.size ();
124 XCAM_ASSERT (img0_size.height == image1.rows ());
125 XCAM_UNUSED (image1);
126
127 #if XCAM_CV_FM_DEBUG
128 cv::Mat mat;
129 cv::UMat umat;
130 cv::Size img1_size = image1.size ();
131 cv::Size size (img0_size.width + img1_size.width, img0_size.height);
132
133 if (image0.isUMat ()) {
134 umat.create (size, image0.type ());
135 debug_img = cv::_InputOutputArray (umat);
136
137 image0.copyTo (umat (cv::Rect(0, 0, img0_size.width, img0_size.height)));
138 image1.copyTo (umat (cv::Rect(img0_size.width, 0, img1_size.width, img1_size.height)));
139 umat.copyTo (debug_img);
140 } else {
141 mat.create (size, image0.type ());
142 debug_img = cv::_InputOutputArray (mat);
143
144 image0.copyTo (mat (cv::Rect(0, 0, img0_size.width, img0_size.height)));
145 image1.copyTo (mat (cv::Rect(img0_size.width, 0, img1_size.width, img1_size.height)));
146 mat.copyTo (debug_img);
147 }
148
149 cv::Size scale_size = size * XCAM_CV_OF_DRAW_SCALE;
150 cv::resize (debug_img, debug_img, scale_size, 0, 0);
151 #endif
152
153 std::vector<float> offsets;
154 float offset_sum = 0.0f;
155 int count = 0;
156 float mean_offset = 0.0f;
157 offsets.reserve (corner0.size ());
158 get_valid_offsets (corner0, corner1, status, error,
159 offsets, offset_sum, count, debug_img, img0_size);
160 #if XCAM_CV_FM_DEBUG
161 XCAM_LOG_INFO ("FeatureMatch(idx:%d): valid offsets:%d", _fm_idx, offsets.size ());
162 char file_name[256];
163 std::snprintf (file_name, 256, "fm_optical_flow_%d_%d.jpg", _frame_num, _fm_idx);
164 cv::imwrite (file_name, debug_img);
165 #endif
166
167 bool ret = get_mean_offset (offsets, offset_sum, count, mean_offset);
168 if (ret) {
169 if (fabs (mean_offset - last_mean_offset) < _config.delta_mean_offset) {
170 out_x_offset = out_x_offset * _config.offset_factor + mean_offset * (1.0f - _config.offset_factor);
171
172 if (fabs (out_x_offset) > _config.max_adjusted_offset)
173 out_x_offset = (out_x_offset > 0.0f) ? _config.max_adjusted_offset : (-_config.max_adjusted_offset);
174 }
175 }
176
177 last_count = count;
178 last_mean_offset = mean_offset;
179 }
180
181 void
detect_and_match(cv::InputArray img_left,cv::InputArray img_right,Rect & crop_left,Rect & crop_right,int & valid_count,float & mean_offset,float & x_offset,int dst_width)182 CVFeatureMatch::detect_and_match (
183 cv::InputArray img_left, cv::InputArray img_right, Rect &crop_left, Rect &crop_right,
184 int &valid_count, float &mean_offset, float &x_offset, int dst_width)
185 {
186 std::vector<float> err;
187 std::vector<uchar> status;
188 std::vector<cv::Point2f> corner_left, corner_right;
189 cv::Ptr<cv::Feature2D> fast_detector;
190 cv::Size win_size = cv::Size (5, 5);
191
192 if (img_left.isUMat ())
193 win_size = cv::Size (16, 16);
194
195 fast_detector = cv::FastFeatureDetector::create (20, true);
196 add_detected_data (img_left, fast_detector, corner_left);
197
198 if (corner_left.empty ()) {
199 return;
200 }
201
202 cv::calcOpticalFlowPyrLK (
203 img_left, img_right, corner_left, corner_right, status, err, win_size, 3,
204 cv::TermCriteria (cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 10, 0.01f));
205 cv::ocl::finish();
206
207 calc_of_match (img_left, img_right, corner_left, corner_right,
208 status, err, valid_count, mean_offset, x_offset);
209
210 adjust_stitch_area (dst_width, x_offset, crop_left, crop_right);
211
212 #if XCAM_CV_FM_DEBUG
213 XCAM_LOG_INFO (
214 "FeatureMatch(idx:%d): stiching area: left_area(pos_x:%d, width:%d), right_area(pos_x:%d, width:%d)",
215 _fm_idx, crop_left.pos_x, crop_left.width, crop_right.pos_x, crop_right.width);
216 #endif
217 }
218
219 void
optical_flow_feature_match(const SmartPtr<VideoBuffer> & left_buf,const SmartPtr<VideoBuffer> & right_buf,Rect & left_crop_rect,Rect & right_crop_rect,int dst_width)220 CVFeatureMatch::optical_flow_feature_match (
221 const SmartPtr<VideoBuffer> &left_buf, const SmartPtr<VideoBuffer> &right_buf,
222 Rect &left_crop_rect, Rect &right_crop_rect, int dst_width)
223 {
224 cv::UMat left_umat, right_umat;
225 cv::Mat left_mat, right_mat;
226 cv::_InputArray left_img, right_img;
227
228 if (!get_crop_image (left_buf, left_crop_rect, left_umat)
229 || !get_crop_image (right_buf, right_crop_rect, right_umat))
230 return;
231
232 if (_use_ocl) {
233 left_img = cv::_InputArray (left_umat);
234 right_img = cv::_InputArray (right_umat);
235 } else {
236 left_mat = left_umat.getMat (cv::ACCESS_READ);
237 right_mat = right_umat.getMat (cv::ACCESS_READ);
238
239 left_img = cv::_InputArray (left_mat);
240 right_img = cv::_InputArray (right_mat);
241 }
242
243 detect_and_match (left_img, right_img, left_crop_rect, right_crop_rect,
244 _valid_count, _mean_offset, _x_offset, dst_width);
245
246 #if XCAM_CV_FM_DEBUG
247 XCAM_ASSERT (_fm_idx >= 0);
248
249 char frame_str[64] = {'\0'};
250 std::snprintf (frame_str, 64, "frame:%d", _frame_num);
251 char fm_idx_str[64] = {'\0'};
252 std::snprintf (fm_idx_str, 64, "fm_idx:%d", _fm_idx);
253
254 char img_name[256] = {'\0'};
255 std::snprintf (img_name, 256, "fm_in_stitch_area_%d_%d_0.jpg", _frame_num, _fm_idx);
256 debug_write_image (left_buf, left_crop_rect, img_name, frame_str, fm_idx_str);
257
258 std::snprintf (img_name, 256, "fm_in_stitch_area_%d_%d_1.jpg", _frame_num, _fm_idx);
259 debug_write_image (right_buf, right_crop_rect, img_name, frame_str, fm_idx_str);
260
261 XCAM_LOG_INFO ("FeatureMatch(idx:%d): frame number:%d done", _fm_idx, _frame_num);
262 _frame_num++;
263 #endif
264 }
265
266 #if XCAM_CV_FM_DEBUG
267 static void
debug_write_image(const SmartPtr<VideoBuffer> & buf,const Rect & rect,char * img_name,char * frame_str,char * fm_idx_str)268 debug_write_image (
269 const SmartPtr<VideoBuffer> &buf, const Rect &rect, char *img_name, char *frame_str, char *fm_idx_str)
270 {
271 cv::Scalar color = cv::Scalar(0, 0, 255);
272 VideoBufferInfo info = buf->get_video_info ();
273
274 cv::Mat mat;
275 CVBaseClass cv_obj;
276 cv_obj.convert_to_mat (buf, mat);
277
278 cv::putText (mat, frame_str, cv::Point(rect.pos_x, 30), cv::FONT_HERSHEY_COMPLEX, 0.8f, color, 2, 8, false);
279 cv::putText (mat, fm_idx_str, cv::Point(rect.pos_x, 70), cv::FONT_HERSHEY_COMPLEX, 0.8f, color, 2, 8, false);
280
281 cv::line (mat, cv::Point(rect.pos_x, rect.pos_y), cv::Point(rect.pos_x + rect.width, rect.pos_y), color, 1);
282 cv::line (mat, cv::Point(rect.pos_x, rect.pos_y + rect.height),
283 cv::Point(rect.pos_x + rect.width, rect.pos_y + rect.height), color, 1);
284
285 cv::line (mat, cv::Point(rect.pos_x, 0), cv::Point(rect.pos_x, info.height), color, 2);
286 cv::line (mat, cv::Point(rect.pos_x + rect.width, 0), cv::Point(rect.pos_x + rect.width, info.height), color, 2);
287
288 cv::imwrite (img_name, mat);
289 }
290 #endif
291
292 }
293