| 380 | } |
| 381 | |
| 382 | bool Keyframe::findConnection(Keyframe* old_kf) { |
| 383 | if (!old_kf->is_vio_keyframe_) return false; |
| 384 | |
| 385 | std::vector<cv::KeyPoint> matched_2d_cur; |
| 386 | std::vector<cv::Point2f> matched_2d_old; |
| 387 | std::vector<cv::Point2f> matched_2d_old_norm; |
| 388 | std::vector<cv::Point3f> matched_3d; |
| 389 | std::vector<Eigen::Vector3i> matched_ids; // Reloc |
| 390 | std::vector<uchar> status; |
| 391 | |
| 392 | matched_3d = point_3d; |
| 393 | matched_2d_cur = point_2d_uv; |
| 394 | matched_ids = point_ids_; |
| 395 | |
| 396 | if (params_.debug_mode_) { |
| 397 | cv::Mat old_img = UtilsOpenCV::DrawCircles(old_kf->image, old_kf->keypoints); |
| 398 | cv::Mat cur_image = UtilsOpenCV::DrawCircles(image, point_2d_uv); |
| 399 | std::string loop_candidate_directory = params_.debug_output_path_ + "/loop_candidates/"; |
| 400 | std::string filename = loop_candidate_directory + "loop_candidate_" + std::to_string(index) + "_" + |
| 401 | std::to_string(old_kf->index) + ".png"; |
| 402 | UtilsOpenCV::showImagesSideBySide(cur_image, old_img, "loop closing candidates", false, true, filename); |
| 403 | } |
| 404 | |
| 405 | searchByBRIEFDes(matched_2d_old, |
| 406 | matched_2d_old_norm, |
| 407 | status, |
| 408 | old_kf->brief_descriptors, |
| 409 | old_kf->keypoints, |
| 410 | old_kf->keypoints_norm); |
| 411 | reduceVector(matched_2d_old, status); |
| 412 | reduceVector(matched_3d, status); |
| 413 | reduceVector(matched_2d_cur, status); |
| 414 | reduceVector(matched_2d_old_norm, status); |
| 415 | reduceVector(matched_ids, status); |
| 416 | status.clear(); |
| 417 | |
| 418 | if (params_.debug_mode_) { |
| 419 | cv::Mat corners_match_image = |
| 420 | UtilsOpenCV::DrawCornersMatches(image, matched_2d_cur, old_kf->image, matched_2d_old, true); |
| 421 | std::string dscriptor_match_dir = params_.debug_output_path_ + "/descriptor_matched/"; |
| 422 | std::string filename = dscriptor_match_dir + "descriptor_match_" + std::to_string(index) + "_" + |
| 423 | std::to_string(old_kf->index) + ".png"; |
| 424 | cv::imwrite(filename, corners_match_image); |
| 425 | } |
| 426 | |
| 427 | // std::cout << "Size Before RANSAC: " << matched_2d_cur.size() << std::endl; |
| 428 | |
| 429 | // opengv::transformation_t T_w_c_old; |
| 430 | // if (LoopClosureUtils::geometricVerificationNister( |
| 431 | // matched_2d_cur, matched_2d_old, status, params_.loop_closure_params_.min_correspondences, &T_w_c_old)) { |
| 432 | // reduceVector(matched_2d_old, status); |
| 433 | // reduceVector(matched_3d, status); |
| 434 | // reduceVector(matched_2d_cur, status); |
| 435 | // reduceVector(matched_2d_old_norm, status); |
| 436 | // reduceVector(matched_ids, status); |
| 437 | // status.clear(); |
| 438 | |
| 439 | // if (params_.debug_image_) { |
no test coverage detected