1 /*
2 * cv_capi_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 * Author: Zong Wei <wei.zong@intel.com>
21 */
22
23 #include "cv_capi_feature_match.h"
24
25 #define XCAM_CV_CAPI_FM_DEBUG 0
26
27 #if XCAM_CV_CAPI_FM_DEBUG
28 #include "ocl/cv_base_class.h"
29 #endif
30
31 namespace XCam {
32 #if XCAM_CV_CAPI_FM_DEBUG
33 static void
34 debug_write_image (
35 const SmartPtr<VideoBuffer> &buf, const Rect &rect, char *img_name, char *frame_str, char *fm_idx_str);
36 #endif
37
CVCapiFeatureMatch()38 CVCapiFeatureMatch::CVCapiFeatureMatch ()
39 : FeatureMatch()
40 {
41 }
42
43 bool
get_crop_image(const SmartPtr<VideoBuffer> & buffer,const Rect & crop_rect,std::vector<char> & crop_image,CvMat & img)44 CVCapiFeatureMatch::get_crop_image (
45 const SmartPtr<VideoBuffer> &buffer, const Rect &crop_rect, std::vector<char> &crop_image, CvMat &img)
46 {
47 VideoBufferInfo info = buffer->get_video_info ();
48
49 uint8_t* image_buffer = buffer->map();
50 int offset = info.strides[NV12PlaneYIdx] * crop_rect.pos_y + crop_rect.pos_x;
51
52 crop_image.resize (crop_rect.width * crop_rect.height);
53 for (int i = 0; i < crop_rect.height; i++) {
54 for (int j = 0; j < crop_rect.width; j++) {
55 crop_image[i * crop_rect.width + j] =
56 image_buffer[offset + i * info.strides[NV12PlaneYIdx] + j];
57 }
58 }
59
60 img = cvMat (crop_rect.height, crop_rect.width, CV_8UC1, (void*)&crop_image[0]);
61
62 return true;
63 }
64
65 void
add_detected_data(CvArr * image,std::vector<CvPoint2D32f> & corners)66 CVCapiFeatureMatch::add_detected_data (
67 CvArr* image, std::vector<CvPoint2D32f> &corners)
68 {
69 std::vector<CvPoint2D32f> keypoints;
70
71 int found_num = 300;
72 double quality = 0.01;
73 double min_dist = 5;
74
75 corners.resize (found_num);
76 CvPoint2D32f* corner_points = &corners[0];
77
78 cvGoodFeaturesToTrack (image, NULL, NULL, corner_points, &found_num, quality, min_dist);
79 XCAM_ASSERT (found_num <= 300);
80
81 #if XCAM_CV_CAPI_FM_DEBUG
82 XCAM_LOG_INFO ("FeatureMatch(idx:%d): detected corners:%d, reserved size:%d", _fm_idx, found_num, (int)corners.size ());
83 #endif
84 if (found_num < (int)corners.size ())
85 corners.resize (found_num);
86 }
87
88 void
get_valid_offsets(std::vector<CvPoint2D32f> & corner0,std::vector<CvPoint2D32f> & corner1,std::vector<char> & status,std::vector<float> & error,std::vector<float> & offsets,float & sum,int & count,CvArr * image,CvSize & img0_size)89 CVCapiFeatureMatch::get_valid_offsets (
90 std::vector<CvPoint2D32f> &corner0, std::vector<CvPoint2D32f> &corner1,
91 std::vector<char> &status, std::vector<float> &error,
92 std::vector<float> &offsets, float &sum, int &count,
93 CvArr* image, CvSize &img0_size)
94 {
95 count = 0;
96 sum = 0.0f;
97
98 for (uint32_t i = 0; i < status.size (); ++i) {
99 if (!status[i])
100 continue;
101
102 #if XCAM_CV_CAPI_FM_DEBUG
103 cv::Mat mat = cv::cvarrToMat (image);
104 cv::Point start = cv::Point (corner0[i].x, corner0[i].y);
105 cv::circle (mat, start, 2, cv::Scalar(255), 2);
106 #endif
107 if (error[i] > _config.max_track_error)
108 continue;
109 if (fabs(corner0[i].y - corner1[i].y) >= _config.max_valid_offset_y)
110 continue;
111 if (corner1[i].x < 0.0f || corner1[i].x > img0_size.width)
112 continue;
113
114 float offset = corner1[i].x - corner0[i].x;
115 sum += offset;
116 ++count;
117 offsets.push_back (offset);
118
119 #if XCAM_CV_CAPI_FM_DEBUG
120 cv::line (mat, start, cv::Point(corner1[i].x + img0_size.width, corner1[i].y), cv::Scalar(255), 2);
121 #else
122 XCAM_UNUSED (image);
123 XCAM_UNUSED (img0_size);
124 #endif
125 }
126 }
127
128 void
calc_of_match(CvArr * image0,CvArr * image1,std::vector<CvPoint2D32f> & corner0,std::vector<CvPoint2D32f> & corner1,std::vector<char> & status,std::vector<float> & error,int & last_count,float & last_mean_offset,float & out_x_offset)129 CVCapiFeatureMatch::calc_of_match (
130 CvArr* image0, CvArr* image1,
131 std::vector<CvPoint2D32f> &corner0, std::vector<CvPoint2D32f> &corner1,
132 std::vector<char> &status, std::vector<float> &error,
133 int &last_count, float &last_mean_offset, float &out_x_offset)
134 {
135 CvMat debug_image;
136 CvSize img0_size = cvSize(((CvMat*)image0)->width, ((CvMat*)image0)->height);
137 XCAM_ASSERT (img0_size.height == ((CvMat*)image1)->height);
138 XCAM_UNUSED (image1);
139
140 std::vector<float> offsets;
141 float offset_sum = 0.0f;
142 int count = 0;
143 float mean_offset = 0.0f;
144 offsets.reserve (corner0.size ());
145
146 #if XCAM_CV_CAPI_FM_DEBUG
147 CvSize img1_size = cvSize(((CvMat*)image1)->width, ((CvMat*)image1)->height);
148 cv::Mat mat;
149 mat.create (img0_size.height, img0_size.width + img1_size.width, ((CvMat*)image0)->type);
150 debug_image = cvMat (img0_size.height, img0_size.width + img1_size.width, ((CvMat*)image0)->type, mat.ptr());
151 cv::cvarrToMat(image0, true).copyTo (mat (cv::Rect(0, 0, img0_size.width, img0_size.height)));
152 cv::cvarrToMat(image1, true).copyTo (mat (cv::Rect(img0_size.width, 0, img1_size.width, img1_size.height)));
153 #endif
154
155 get_valid_offsets (corner0, corner1, status, error,
156 offsets, offset_sum, count, &debug_image, img0_size);
157
158 #if XCAM_CV_CAPI_FM_DEBUG
159 XCAM_LOG_INFO ("FeatureMatch(idx:%d): valid offsets:%d", _fm_idx, offsets.size ());
160 char file_name[256] = {'\0'};
161 std::snprintf (file_name, 256, "fm_optical_flow_%d_%d.jpg", _frame_num, _fm_idx);
162 cv::imwrite (file_name, mat);
163 #endif
164
165 bool ret = get_mean_offset (offsets, offset_sum, count, mean_offset);
166 if (ret) {
167 if (fabs (mean_offset - last_mean_offset) < _config.delta_mean_offset) {
168 out_x_offset = out_x_offset * _config.offset_factor + mean_offset * (1.0f - _config.offset_factor);
169
170 if (fabs (out_x_offset) > _config.max_adjusted_offset)
171 out_x_offset = (out_x_offset > 0.0f) ? _config.max_adjusted_offset : (-_config.max_adjusted_offset);
172 }
173 }
174
175 last_count = count;
176 last_mean_offset = mean_offset;
177 }
178
179 void
detect_and_match(CvArr * img_left,CvArr * img_right,Rect & crop_left,Rect & crop_right,int & valid_count,float & mean_offset,float & x_offset,int dst_width)180 CVCapiFeatureMatch::detect_and_match (
181 CvArr* img_left, CvArr* img_right, Rect &crop_left, Rect &crop_right,
182 int &valid_count, float &mean_offset, float &x_offset, int dst_width)
183 {
184 std::vector<float> err;
185 std::vector<char> status;
186 std::vector<CvPoint2D32f> corner_left, corner_right;
187
188 CvSize win_size = cvSize (41, 41);
189
190 add_detected_data (img_left, corner_left);
191 int count = corner_left.size ();
192 if (corner_left.empty ()) {
193 return;
194 }
195
196 // find the corresponding points in img_right
197 corner_right.resize (count);
198 status.resize (count);
199 err.resize (count);
200
201 CvPoint2D32f* corner_points1 = &corner_left[0];
202 CvPoint2D32f* corner_points2 = &corner_right[0];
203 char* optflow_status = &status[0];
204 float* optflow_errs = &err[0];
205
206 cvCalcOpticalFlowPyrLK (
207 img_left, img_right, 0, 0, corner_points1, corner_points2, count, win_size, 3,
208 optflow_status, optflow_errs, cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 10, 0.01f), 0);
209
210 #if XCAM_CV_CAPI_FM_DEBUG
211 XCAM_LOG_INFO ("FeatureMatch(idx:%d): matched corners:%d", _fm_idx, count);
212 #endif
213
214 calc_of_match (img_left, img_right, corner_left, corner_right,
215 status, err, valid_count, mean_offset, x_offset);
216
217 adjust_stitch_area (dst_width, x_offset, crop_left, crop_right);
218
219 #if XCAM_CV_CAPI_FM_DEBUG
220 XCAM_LOG_INFO (
221 "FeatureMatch(idx:%d): stiching area: left_area(pos_x:%d, width:%d), right_area(pos_x:%d, width:%d)",
222 _fm_idx, crop_left.pos_x, crop_left.width, crop_right.pos_x, crop_right.width);
223 #endif
224 }
225
226 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)227 CVCapiFeatureMatch::optical_flow_feature_match (
228 const SmartPtr<VideoBuffer> &left_buf, const SmartPtr<VideoBuffer> &right_buf,
229 Rect &left_crop_rect, Rect &right_crop_rect, int dst_width)
230 {
231 CvMat left_img, right_img;
232
233 if (!get_crop_image (left_buf, left_crop_rect, _left_crop_image, left_img)
234 || !get_crop_image (right_buf, right_crop_rect, _right_crop_image, right_img))
235 return;
236
237 detect_and_match ((CvArr*)(&left_img), (CvArr*)(&right_img), left_crop_rect, right_crop_rect,
238 _valid_count, _mean_offset, _x_offset, dst_width);
239
240 #if XCAM_CV_CAPI_FM_DEBUG
241 XCAM_ASSERT (_fm_idx >= 0);
242
243 char frame_str[64] = {'\0'};
244 std::snprintf (frame_str, 64, "frame:%d", _frame_num);
245 char fm_idx_str[64] = {'\0'};
246 std::snprintf (fm_idx_str, 64, "fm_idx:%d", _fm_idx);
247
248 char img_name[256] = {'\0'};
249 std::snprintf (img_name, 256, "fm_in_stitch_area_%d_%d_0.jpg", _frame_num, _fm_idx);
250 debug_write_image (left_buf, left_crop_rect, img_name, frame_str, fm_idx_str);
251
252 std::snprintf (img_name, 256, "fm_in_stitch_area_%d_%d_1.jpg", _frame_num, _fm_idx);
253 debug_write_image (right_buf, right_crop_rect, img_name, frame_str, fm_idx_str);
254
255 XCAM_LOG_INFO ("FeatureMatch(idx:%d): frame number:%d done", _fm_idx, _frame_num);
256
257 _frame_num++;
258 #endif
259 }
260
261 #if XCAM_CV_CAPI_FM_DEBUG
262 static void
debug_write_image(const SmartPtr<VideoBuffer> & buf,const Rect & rect,char * img_name,char * frame_str,char * fm_idx_str)263 debug_write_image (
264 const SmartPtr<VideoBuffer> &buf, const Rect &rect, char *img_name, char *frame_str, char *fm_idx_str)
265 {
266 cv::Scalar color = cv::Scalar(0, 0, 255);
267 VideoBufferInfo info = buf->get_video_info ();
268
269 cv::Mat mat;
270 CVBaseClass cv_obj;
271 cv_obj.convert_to_mat (buf, mat);
272
273 cv::putText (mat, frame_str, cv::Point(rect.pos_x, 30), cv::FONT_HERSHEY_COMPLEX, 0.8f, color, 2, 8, false);
274 cv::putText (mat, fm_idx_str, cv::Point(rect.pos_x, 70), cv::FONT_HERSHEY_COMPLEX, 0.8f, color, 2, 8, false);
275
276 cv::line (mat, cv::Point(rect.pos_x, rect.pos_y), cv::Point(rect.pos_x + rect.width, rect.pos_y), color, 1);
277 cv::line (mat, cv::Point(rect.pos_x, rect.pos_y + rect.height),
278 cv::Point(rect.pos_x + rect.width, rect.pos_y + rect.height), color, 1);
279
280 cv::line (mat, cv::Point(rect.pos_x, 0), cv::Point(rect.pos_x, info.height), color, 2);
281 cv::line (mat, cv::Point(rect.pos_x + rect.width, 0), cv::Point(rect.pos_x + rect.width, info.height), color, 2);
282
283 cv::imwrite (img_name, mat);
284 }
285 #endif
286
287 }
288