| 257 | |
| 258 | |
| 259 | bool KeyFrame::findConnection(KeyFrame* old_kf) |
| 260 | { |
| 261 | TicToc tmp_t; |
| 262 | //printf("find Connection\n"); |
| 263 | vector<cv::Point2f> matched_2d_cur, matched_2d_old; |
| 264 | vector<cv::Point2f> matched_2d_cur_norm, matched_2d_old_norm; |
| 265 | vector<cv::Point3f> matched_3d; |
| 266 | vector<double> matched_id; |
| 267 | vector<uchar> status; |
| 268 | |
| 269 | matched_3d = point_3d; |
| 270 | matched_2d_cur = point_2d_uv; |
| 271 | matched_2d_cur_norm = point_2d_norm; |
| 272 | matched_id = point_id; |
| 273 | |
| 274 | TicToc t_match; |
| 275 | #if 0 |
| 276 | if (DEBUG_IMAGE) |
| 277 | { |
| 278 | cv::Mat gray_img, loop_match_img; |
| 279 | cv::Mat old_img = old_kf->image; |
| 280 | cv::hconcat(image, old_img, gray_img); |
| 281 | cvtColor(gray_img, loop_match_img, CV_GRAY2RGB); |
| 282 | for(int i = 0; i< (int)point_2d_uv.size(); i++) |
| 283 | { |
| 284 | cv::Point2f cur_pt = point_2d_uv[i]; |
| 285 | cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0)); |
| 286 | } |
| 287 | for(int i = 0; i< (int)old_kf->keypoints.size(); i++) |
| 288 | { |
| 289 | cv::Point2f old_pt = old_kf->keypoints[i].pt; |
| 290 | old_pt.x += COL; |
| 291 | cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0)); |
| 292 | } |
| 293 | ostringstream path; |
| 294 | path << "/home/tony-ws1/raw_data/loop_image/" |
| 295 | << index << "-" |
| 296 | << old_kf->index << "-" << "0raw_point.jpg"; |
| 297 | cv::imwrite( path.str().c_str(), loop_match_img); |
| 298 | } |
| 299 | #endif |
| 300 | //printf("search by des\n"); |
| 301 | searchByBRIEFDes(matched_2d_old, matched_2d_old_norm, status, old_kf->brief_descriptors, old_kf->keypoints, old_kf->keypoints_norm); |
| 302 | reduceVector(matched_2d_cur, status); |
| 303 | reduceVector(matched_2d_old, status); |
| 304 | reduceVector(matched_2d_cur_norm, status); |
| 305 | reduceVector(matched_2d_old_norm, status); |
| 306 | reduceVector(matched_3d, status); |
| 307 | reduceVector(matched_id, status); |
| 308 | //printf("search by des finish\n"); |
| 309 | |
| 310 | #if 0 |
| 311 | if (DEBUG_IMAGE) |
| 312 | { |
| 313 | int gap = 10; |
| 314 | cv::Mat gap_image(ROW, gap, CV_8UC1, cv::Scalar(255, 255, 255)); |
| 315 | cv::Mat gray_img, loop_match_img; |
| 316 | cv::Mat old_img = old_kf->image; |