diff --git a/include/vision/processor/processor_tracker_feature_trifocal.h b/include/vision/processor/processor_tracker_feature_trifocal.h index 907ada0d77caffbc692d54a4e321e90a255d8221..0e85182e40d146a944f467ad1d7c385b2d62a7ba 100644 --- a/include/vision/processor/processor_tracker_feature_trifocal.h +++ b/include/vision/processor/processor_tracker_feature_trifocal.h @@ -20,13 +20,15 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeatureTrifocal); struct ParamsProcessorTrackerFeatureTrifocal : public ParamsProcessorTrackerFeature { - std::string yaml_file_params_vision_utils; + std::string yaml_file_params_vision_utils; + + int n_cells_h; + int n_cells_v; + int min_response_new_feature; + double pixel_noise_std; ///< std noise of the pixel + int min_track_length_for_factor; ///< Minimum track length of a matched feature to create a factor + bool debug_view; - int n_cells_h; - int n_cells_v; - int min_response_new_feature; - double pixel_noise_std; ///< std noise of the pixel - int min_track_length_for_factor; ///< Minimum track length of a matched feature to create a factor ParamsProcessorTrackerFeatureTrifocal() = default; ParamsProcessorTrackerFeatureTrifocal(std::string _unique_name, const ParamsServer& _server): ParamsProcessorTrackerFeature(_unique_name, _server) @@ -37,6 +39,7 @@ struct ParamsProcessorTrackerFeatureTrifocal : public ParamsProcessorTrackerFeat min_response_new_feature = _server.getParam<int>(_unique_name + "/min_response_new_feature"); pixel_noise_std = _server.getParam<double>(_unique_name + "/pixel_noise_std"); min_track_length_for_factor = _server.getParam<int>(_unique_name + "/min_track_length_for_factor"); + debug_view = _server.getParam<bool>(_unique_name + "/debug_view"); } std::string print() const override { @@ -46,7 +49,8 @@ struct ParamsProcessorTrackerFeatureTrifocal : public ParamsProcessorTrackerFeat + "n_cells_v: " + std::to_string(n_cells_v) + "\n" + "min_response_new_feature: " + std::to_string(min_response_new_feature) + "\n" + "pixel_noise_std: " + std::to_string(pixel_noise_std) + "\n" - + "min_track_length_for_factor: " + std::to_string(min_track_length_for_factor) + "\n"; + + "min_track_length_for_factor: " + std::to_string(min_track_length_for_factor) + "\n" + + "debug_view: " + std::to_string(debug_view) + "\n"; } }; diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp index dc079241ca7903e7ec882674ab08524463c99c09..6d5426b0c84e315aed4024514b48f78a6805b4f2 100644 --- a/src/processor/processor_tracker_feature_trifocal.cpp +++ b/src/processor/processor_tracker_feature_trifocal.cpp @@ -54,16 +54,20 @@ ProcessorTrackerFeatureTrifocal::ProcessorTrackerFeatureTrifocal(ParamsProcessor std::string mat_name = vision_utils::readYamlType(params_tracker_feature_trifocal_->yaml_file_params_vision_utils, "matcher"); mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_tracker_feature_trifocal_->yaml_file_params_vision_utils); -// // DEBUG VIEW - cv::startWindowThread(); - cv::namedWindow("DEBUG VIEW", cv::WINDOW_NORMAL); -// cv::namedWindow("DEBUG MATCHES", cv::WINDOW_NORMAL); + // DEBUG VIEW + if (params_tracker_feature_trifocal_->debug_view) + { + cv::startWindowThread(); + cv::namedWindow("DEBUG VIEW", cv::WINDOW_NORMAL); + //cv::namedWindow("DEBUG MATCHES", cv::WINDOW_NORMAL); + } } // Destructor ProcessorTrackerFeatureTrifocal::~ProcessorTrackerFeatureTrifocal() { -// cv::destroyAllWindows(); + if (params_tracker_feature_trifocal_->debug_view) + cv::destroyAllWindows(); } bool ProcessorTrackerFeatureTrifocal::isInlier(const cv::KeyPoint& _kp_last, const cv::KeyPoint& _kp_incoming) @@ -316,17 +320,20 @@ void ProcessorTrackerFeatureTrifocal::preProcess() for (auto match : capture_image_incoming_->matches_from_precedent_) capture_image_last_->map_index_to_next_[match.trainIdx] = match.queryIdx; // map[last] = incoming - // DEBUG -// cv::Mat img_last = (std::static_pointer_cast<CaptureImage>(last_ptr_))->getImage(); -// cv::Mat img_incoming = (std::static_pointer_cast<CaptureImage>(incoming_ptr_))->getImage(); + // DEBUG VIEW +// if (params_tracker_feature_trifocal_->debug_view) +// { +// cv::Mat img_last = (std::static_pointer_cast<CaptureImage>(last_ptr_))->getImage(); +// cv::Mat img_incoming = (std::static_pointer_cast<CaptureImage>(incoming_ptr_))->getImage(); // -// cv::putText(img_last, "LAST", cv::Point(img_last.cols/2,20), cv::FONT_HERSHEY_PLAIN, 1.0, CV_RGB(255,0,0), 10.0); -// cv::putText(img_incoming, "INCOMING",cv::Point(img_last.cols/2,20), cv::FONT_HERSHEY_PLAIN, 1.0, CV_RGB(255,0,0), 10.0); +// cv::putText(img_last, "LAST", cv::Point(img_last.cols/2,20), cv::FONT_HERSHEY_PLAIN, 1.0, CV_RGB(255,0,0), 10.0); +// cv::putText(img_incoming, "INCOMING",cv::Point(img_last.cols/2,20), cv::FONT_HERSHEY_PLAIN, 1.0, CV_RGB(255,0,0), 10.0); // -// cv::Mat img_matches; -// cv::drawMatches(img_incoming, capture_incoming_->keypoints_, img_last, capture_last_->keypoints_, capture_incoming_->matches_from_precedent_, img_matches); -// cv::imshow("DEBUG MATCHES", img_matches); -// cv::waitKey(0); +// cv::Mat img_matches; +// cv::drawMatches(img_incoming, capture_incoming_->keypoints_, img_last, capture_last_->keypoints_, capture_incoming_->matches_from_precedent_, img_matches); +// cv::imshow("DEBUG MATCHES", img_matches); +// cv::waitKey(0); +// } } } @@ -356,11 +363,14 @@ void ProcessorTrackerFeatureTrifocal::postProcess() kp_enh_tracks[feat_id] = kp_enh_track; } - // DEBUG - image_debug_ = vision_utils::buildImageProcessed((std::static_pointer_cast<CaptureImage>(last_ptr_))->getImage(), kp_enh_tracks); + // DEBUG VIEW + if (params_tracker_feature_trifocal_->debug_view) + { + image_debug_ = vision_utils::buildImageProcessed((std::static_pointer_cast<CaptureImage>(last_ptr_))->getImage(), kp_enh_tracks); - cv::imshow("DEBUG VIEW", image_debug_); - cv::waitKey(1); + cv::imshow("DEBUG VIEW", image_debug_); + cv::waitKey(1); + } } FactorBasePtr ProcessorTrackerFeatureTrifocal::emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) diff --git a/test/gtest_factor_trifocal.cpp b/test/gtest_factor_trifocal.cpp index 099d1cf307a0c25b0ebfaf41e147d9e9a3975176..7a99912b69007fde28cbef92584c9c9ca6851d38 100644 --- a/test/gtest_factor_trifocal.cpp +++ b/test/gtest_factor_trifocal.cpp @@ -132,6 +132,7 @@ class FactorTrifocalTest : public testing::Test{ params_tracker_feature_trifocal_trifocal->max_new_features = 5; params_tracker_feature_trifocal_trifocal->min_features_for_keyframe = 5; params_tracker_feature_trifocal_trifocal->yaml_file_params_vision_utils = wolf_root + "/demos/processor_tracker_feature_trifocal_vision_utils.yaml"; + params_tracker_feature_trifocal_trifocal->debug_view = false; ProcessorBasePtr proc = problem->installProcessor("ProcessorTrackerFeatureTrifocal", "trifocal", camera, params_tracker_feature_trifocal_trifocal); proc_trifocal = std::static_pointer_cast<ProcessorTrackerFeatureTrifocal>(proc); diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp index 97c244cea83734ef546fbdbd7e04bbea8e2683f8..2e3d097e6c3e539bb3bc3ed7cc3f6ef29d5d1a52 100644 --- a/test/gtest_processor_tracker_feature_trifocal.cpp +++ b/test/gtest_processor_tracker_feature_trifocal.cpp @@ -93,6 +93,7 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback) params_tracker_feature_trifocal->n_cells_h = 10; params_tracker_feature_trifocal->n_cells_v = 10; params_tracker_feature_trifocal->yaml_file_params_vision_utils = wolf_root + "/demos/processor_tracker_feature_trifocal_vision_utils.yaml"; + params_tracker_feature_trifocal->debug_view = false; // ProcessorTrackerFeatureTrifocalPtr proc_trk = make_shared<ProcessorTrackerFeatureTrifocal>(params_tracker_feature_trifocal); auto proc_trk = std::static_pointer_cast<ProcessorTrackerFeatureTrifocal>(ProcessorBase::emplace<ProcessorTrackerFeatureTrifocal>(sens_trk, params_tracker_feature_trifocal));