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 (at) intel.com> 19 * Author: Yinhang Liu <yinhangx.liu (at) intel.com> 20 * Author: Zong Wei <wei.zong (at) 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 38 CVCapiFeatureMatch::CVCapiFeatureMatch () 39 : FeatureMatch() 40 { 41 } 42 43 bool 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 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 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 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 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 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 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
