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 (at) intel.com> 19 * Author: Yinhang Liu <yinhangx.liu (at) 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 36 CVFeatureMatch::CVFeatureMatch () 37 : CVBaseClass () 38 , FeatureMatch () 39 { 40 XCAM_ASSERT (_cv_context.ptr ()); 41 } 42 43 bool 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 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 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 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 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 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 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