| 55 | } |
| 56 | |
| 57 | void LoopClosure::run() { |
| 58 | while (!shutdown_) { |
| 59 | std::unique_ptr<KeyframeInfo> keyframe_info = nullptr; |
| 60 | bool new_vio_keyframe_available = keyframe_tracking_queue_.pop(keyframe_info); |
| 61 | if (new_vio_keyframe_available) { |
| 62 | CHECK(keyframe_info); |
| 63 | Timestamp stamp; |
| 64 | Eigen::Matrix3d rotation; |
| 65 | Eigen::Vector3d translation; |
| 66 | uint32_t primitive_keyframes = 0; |
| 67 | bool got_keyframe = false; |
| 68 | |
| 69 | if (params_.health_params_.enabled) { |
| 70 | switching_estimator_->addKeyframeInfo(*keyframe_info.get()); |
| 71 | Timestamp prim_stamp; |
| 72 | cv::Mat primitive_estimator_pose; |
| 73 | bool got_primitive_pose = false; |
| 74 | got_primitive_pose = primitive_estimator_poses_buffer_.getNearestValueToTime( |
| 75 | keyframe_info->timestamp_, 50000000UL, &primitive_estimator_pose, &prim_stamp); |
| 76 | if (got_primitive_pose) { |
| 77 | Eigen::Matrix4d primitive_estimator_pose_eigen; |
| 78 | cv::cv2eigen(primitive_estimator_pose, primitive_estimator_pose_eigen); |
| 79 | switching_estimator_->addPrimitiveEstimatorPose(prim_stamp, primitive_estimator_pose_eigen); |
| 80 | } |
| 81 | |
| 82 | if (params_.debug_mode_ && primitive_publish_callback_) { |
| 83 | std::pair<Timestamp, Eigen::Matrix4d> latest_primitive_estimator_pose; |
| 84 | bool robust_estimator_initialized = |
| 85 | switching_estimator_->getLatestPrimitiveEstimatorPose(latest_primitive_estimator_pose); |
| 86 | if (robust_estimator_initialized) { |
| 87 | primitive_publish_callback_(latest_primitive_estimator_pose); |
| 88 | } |
| 89 | } |
| 90 | |
| 91 | got_keyframe = switching_estimator_->getRobustPose(stamp, translation, rotation); |
| 92 | primitive_keyframes = switching_estimator_->getPrimitiveKFCount(); |
| 93 | |
| 94 | } else { |
| 95 | rotation = keyframe_info->rotation_; |
| 96 | translation = keyframe_info->translation_; |
| 97 | stamp = keyframe_info->timestamp_; |
| 98 | got_keyframe = true; |
| 99 | } |
| 100 | if (got_keyframe) { |
| 101 | // VLOG(10) << "VIO keyframe at: " << stamp; |
| 102 | uint32_t combined_kf_index = keyframe_info->keyframe_index_ + primitive_keyframes; |
| 103 | std::map<Keyframe*, int> KFcounter; |
| 104 | for (size_t i = 0; i < keyframe_info->keyfame_points_.size(); ++i) { |
| 105 | double quality = static_cast<double>(keyframe_info->tracking_info_.points_quality_[i]); |
| 106 | for (auto observed_kf_index : keyframe_info->point_covisibilities_[i]) { |
| 107 | observed_kf_index += primitive_keyframes; |
| 108 | if (kfMapper_.find(observed_kf_index) != kfMapper_.end()) { |
| 109 | Keyframe* observed_kf = |
| 110 | kfMapper_.find(observed_kf_index)->second; // Keyframe where this point_3d has been observed |
| 111 | KFcounter[observed_kf]++; |
| 112 | } |
| 113 | } |
| 114 | } |
nothing calls this directly
no test coverage detected