diff --git a/include/publisher_vision.h b/include/publisher_vision.h index d278e777a60d5198ed06c7ae1f2ee0a22f23bb9d..1eb4e687b7ea5a8e13916b25dcf140085414f77e 100644 --- a/include/publisher_vision.h +++ b/include/publisher_vision.h @@ -53,7 +53,6 @@ enum COLOR {BLUE = 0, GREY, RED }; - class PublisherVisionDebug : public Publisher { public: @@ -73,21 +72,36 @@ class PublisherVisionDebug : public Publisher ProcessorVisualOdometryPtr processor_vision_; CaptureBasePtr last_capture_; - bool show_tracks_ID_; - bool show_landmarks_ID_; - int min_luminosity_; - int max_luminosity_; - double thickness_curr_ftr_; - int size_feature_curr_pix_; - double thickness_kfs_ftr_; - int size_feature_kfs_pix_; - double thickness_lmk_; - double size_text_ID_; - double thickness_track_; - COLOR color_tracks_features_; - COLOR color_tracks_features_preprocess_; - COLOR color_landmarks_; - COLOR color_last_keypoints_; + struct Feature { + double thickness_; + int size_pix_; + COLOR color_; + }; + + struct Tracks { + bool show_id_; + int size_id_; + double thickness_; + COLOR color_; + int min_luminosity_; + int max_luminosity_; + struct Feature feature_last_; + struct Feature feature_kfs_; + }; + struct Tracks_preprocess { + double thickness_; + COLOR color_; + }; + struct Landmarks { + bool show_id_; + int size_id_; + int size_pix_; + COLOR color_; + }; + + Tracks tracks_; + Tracks_preprocess tracks_preprocess_; + Landmarks landmarks_; image_transport::Publisher publisher_image_; image_transport::ImageTransport img_transport_; diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp index 2345c3e4401c145f394b6bedb707fe802dd9e593..ee68932038b6d02a5eb998c02004ca7965ab7fee 100644 --- a/src/publisher_vision.cpp +++ b/src/publisher_vision.cpp @@ -70,29 +70,34 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name, img_transport_(ros::NodeHandle()) { // if user do not provide processor's name, first processor of type PublisherVisionDebug is taken - auto processor_name = getParamWithDefault<std::string>(_server, prefix_ + "/processor_name", ""); - - show_tracks_ID_ = getParamWithDefault<bool>(_server, prefix_ + "/show_tracks_ID", false); - show_landmarks_ID_ = getParamWithDefault<bool>(_server, prefix_ + "/show_landmarks_ID", false); - min_luminosity_ = getParamWithDefault<int>(_server, prefix_ + "/min_luminosity", 100); - max_luminosity_ = getParamWithDefault<int>(_server, prefix_ + "/max_luminosity", 450); - thickness_curr_ftr_ = getParamWithDefault<double>(_server, prefix_ + "/thickness_curr_ftr", -1); - size_feature_curr_pix_ = getParamWithDefault<int>(_server, prefix_ + "/size_feature_curr_pix", 2); - thickness_kfs_ftr_ = getParamWithDefault<double>(_server, prefix_ + "/thickness_kfs_ftr", 0.1); - size_feature_kfs_pix_ = getParamWithDefault<int>(_server, prefix_ + "/size_feature_kfs_pix", 1); - thickness_lmk_ = getParamWithDefault<double>(_server, prefix_ + "/thickness_lmk", 5); - size_text_ID_ = getParamWithDefault<double>(_server, prefix_ + "/size_text_ID", 0.5); - thickness_track_ = getParamWithDefault<double>(_server, prefix_ + "/thickness_track", 1.5); - std::string str_color_tracks_features = getParamWithDefault<std::string>(_server, prefix_ + "/color_tracks_features", "CYAN"); - std::string str_color_tracks_features_preProcess = getParamWithDefault<std::string>(_server, prefix_ + "/color_tracks_features_preprocess", "CYAN"); - std::string str_color_landmarks = getParamWithDefault<std::string>(_server, prefix_ + "/color_landmarks", "CYAN"); - std::string str_color_last_keypoints = getParamWithDefault<std::string>(_server, prefix_ + "/color_last_keypoints", "CYAN"); - - // set color attributes - color_tracks_features_ = colorStringToEnum(str_color_tracks_features); - color_tracks_features_preprocess_ = colorStringToEnum(str_color_tracks_features); - color_landmarks_ = colorStringToEnum(str_color_landmarks); - color_last_keypoints_ = colorStringToEnum(str_color_last_keypoints); + auto processor_name = getParamWithDefault<std::string>(_server, prefix_ + "/processor_name", ""); + + //Tracks + tracks_.show_id_ = getParamWithDefault<bool>(_server, prefix_ + "/tracks/show_id", false); + tracks_.size_id_ = getParamWithDefault<int>(_server, prefix_ + "/tracks/size_id", 0.5); + tracks_.thickness_ = getParamWithDefault<double>(_server, prefix_ + "/tracks/thickness", 1.5); + std::string str_color_tracks = getParamWithDefault<std::string>(_server, prefix_ + "/tracks/color", "CYAN"); + tracks_.color_ = colorStringToEnum(str_color_tracks); + tracks_.min_luminosity_ = getParamWithDefault<int>(_server, prefix_ + "/tracks/min_lum", 100); + tracks_.max_luminosity_ = getParamWithDefault<int>(_server, prefix_ + "/tracks/max_lum", 450); + tracks_.min_luminosity_ = (tracks_.min_luminosity_ < 0 ? 0 : tracks_.min_luminosity_); + tracks_.max_luminosity_ = (tracks_.max_luminosity_ > 510 ? 510 : tracks_.max_luminosity_); + //Feature_last + tracks_.feature_last_.thickness_ = getParamWithDefault<double>(_server, prefix_ + "/tracks/feature_last/thickness", -1); + tracks_.feature_last_.size_pix_ = getParamWithDefault<int>(_server, prefix_ + "/tracks/feature_last/size_pix", 2); + std::string str_color_ftr_last = getParamWithDefault<std::string>(_server, prefix_ + "/tracks/feature_last/color", "RED"); + tracks_.feature_last_.color_ = colorStringToEnum(str_color_ftr_last); + //Feature_kfs + tracks_.feature_kfs_.thickness_ = getParamWithDefault<double>(_server, prefix_ + "/tracks/feature_kfs/thickness", 0.1); + tracks_.feature_kfs_.size_pix_ = getParamWithDefault<int>(_server, prefix_ + "/tracks/feature_kfs/size_pix", 1); + + //Landmarks + landmarks_.show_id_ = getParamWithDefault<bool>(_server, prefix_ + "/landmarks/show_id", false); + landmarks_.size_id_ = getParamWithDefault<int>(_server, prefix_ + "/landmarks/size_id", 0.5); + landmarks_.size_pix_ = getParamWithDefault<int>(_server, prefix_ + "/landmarks/size_pix", 5); + std::string str_color_landmarks_ = getParamWithDefault<std::string>(_server, prefix_ + "/landmarks/color", "CYAN"); + landmarks_.color_ = colorStringToEnum(str_color_landmarks_); + // search the processor if (processor_name == "") @@ -232,7 +237,7 @@ std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_ // alpha = 0.5; // put "half luminosity as a default value" } - double lum = (min_luminosity_ + (max_luminosity_ - min_luminosity_) * (1 - alpha)); + double lum = (tracks_.min_luminosity_ + (tracks_.max_luminosity_ - tracks_.min_luminosity_) * (1 - alpha)); if (lum > 255) { color[0] = 255; @@ -347,26 +352,27 @@ void PublisherVisionDebug::showTracks(cv::Mat _image, } // determine color and luminosity of track - std::vector<int> color = colorTrackAndFeatures(track.size(), - max_feature_in_track, - min_feature_in_track, - color_tracks_features_); + std::vector<int> color = colorTrackAndFeatures(track.size(), + max_feature_in_track, + min_feature_in_track, + tracks_.color_); + + std::vector<int> color_last_kpt = primaryColor(tracks_.feature_last_.color_); + // iterator to current feature in track Track::iterator ftr_current (track.find(time_capture)); - std::vector<int> color_last_kpt = primaryColor(color_last_keypoints_); - // draw current feature cv::circle(_image, cv::Point(ftr_current->second->getMeasurement(0), ftr_current->second->getMeasurement(1)), - size_feature_curr_pix_, + tracks_.feature_last_.size_pix_, CV_RGB(color_last_kpt[0], color_last_kpt[1], color_last_kpt[2]), - thickness_curr_ftr_); + tracks_.feature_last_.thickness_); // draw track ID close to current feature - if (show_tracks_ID_) //show features ID of current frame + if (tracks_.show_id_) //show features ID of current frame { int shift_text_x = 7; int shift_text_y = 10; @@ -375,7 +381,8 @@ void PublisherVisionDebug::showTracks(cv::Mat _image, std::to_string(ftr_current->second->trackId()), cv::Point(ftr_current->second->getMeasurement(0) - shift_text_x, ftr_current->second->getMeasurement(1) - shift_text_y), - 1, size_text_ID_, CV_RGB(color[0], color[1], color[2])); + 1, tracks_.size_id_, + CV_RGB(color_last_kpt[0], color_last_kpt[1], color_last_kpt[2])); } @@ -391,13 +398,19 @@ void PublisherVisionDebug::showTracks(cv::Mat _image, { // mark features of previous keyframes cv::circle(_image, cv::Point(ftr->second->getMeasurement(0), ftr->second->getMeasurement(1)), - size_feature_kfs_pix_, CV_RGB(color[0], color[1], color[2]), thickness_kfs_ftr_); + tracks_.feature_kfs_.size_pix_, + CV_RGB(color[0], color[1], color[2]), + tracks_.feature_kfs_.thickness_); } // draw line in between keyframes (as well as last KF and current frame) - cv::line(_image, cv::Point(ftr->second->getMeasurement(0), ftr->second->getMeasurement(1)), - cv::Point(previous->second->getMeasurement(0), previous->second->getMeasurement(1)), - CV_RGB(color[0], color[1], color[2]), thickness_track_); + cv::line(_image, + cv::Point(ftr->second->getMeasurement(0), + ftr->second->getMeasurement(1)), + cv::Point(previous->second->getMeasurement(0), + previous->second->getMeasurement(1)), + CV_RGB(color[0], color[1], color[2]), + tracks_.thickness_); } } @@ -461,54 +474,54 @@ Eigen::Vector2d PublisherVisionDebug::projectLandmarkHp(const Eigen::Matrix<doub Eigen::Vector2d pixel; pixel << pixel_hmg(0)/pixel_hmg(2), - pixel_hmg(1)/pixel_hmg(2); + pixel_hmg(1)/pixel_hmg(2); return pixel; } -void PublisherVisionDebug::showLandmark(cv::Mat _image, - const TrackMatrix& _track_matrix, - const FeatureBasePtr& _ftr) -{ - - // get stuff common to all landmarks - const auto& capture = _ftr->getCapture(); - const auto& Tcw = getTcwMatrix(capture); - const auto& prj_mat = getProjectionMatrix(capture); - const auto& trf_prj_mat = prj_mat * Tcw; - - // get stuff of this landmark - const auto& lmk = getAssociatedLandmark(_track_matrix, _ftr); - const auto& lmk_pixel = projectLandmarkHp(trf_prj_mat, lmk); - - if (lmk != nullptr) - { - cv::drawMarker(_image, cv::Point(lmk_pixel(0), lmk_pixel(1)), - CV_RGB(0, 0, 0), cv::MARKER_TILTED_CROSS, - thickness_lmk_, 1); - - if (show_landmarks_ID_) - { - unsigned int lmk_id = lmk->id(); - int shift_text_x = 7; - int shift_text_y = 10; - - cv::putText( - _image, - std::to_string(lmk_id), - cv::Point(lmk_pixel(0) - shift_text_x, - lmk_pixel(1) - shift_text_y), - 1, size_text_ID_, CV_RGB(0, 0, 0)); - } - } -} +// void PublisherVisionDebug::showLandmark(cv::Mat _image, + // const TrackMatrix& _track_matrix, + // const FeatureBasePtr& _ftr) +// { + + // // get stuff common to all landmarks + // const auto& capture = _ftr->getCapture(); + // const auto& Tcw = getTcwMatrix(capture); + // const auto& prj_mat = getProjectionMatrix(capture); + // const auto& trf_prj_mat = prj_mat * Tcw; + + // // get stuff of this landmark + // const auto& lmk = getAssociatedLandmark(_track_matrix, _ftr); + // const auto& lmk_pixel = projectLandmarkHp(trf_prj_mat, lmk); + + // if (lmk != nullptr) + // { + // cv::drawMarker(_image, cv::Point(lmk_pixel(0), lmk_pixel(1)), + // CV_RGB(0, 0, 0), cv::MARKER_TILTED_CROSS, + // thickness_lmk_, 1); + + // if (show_landmarks_ID_) + // { + // unsigned int lmk_id = lmk->id(); + // int shift_text_x = 7; + // int shift_text_y = 10; + + // cv::putText( + // _image, + // std::to_string(lmk_id), + // cv::Point(lmk_pixel(0) - shift_text_x, + // lmk_pixel(1) - shift_text_y), + // 1, size_text_ID_, CV_RGB(0, 0, 0)); + // } + // } +// } void PublisherVisionDebug::showLandmarks(cv::Mat _image, const TrackMatrix& _track_matrix, const CaptureBasePtr& _capture) { // get stuff common to all landmarks - std::vector<int> colorLmks = primaryColor(color_landmarks_); + std::vector<int> color_lmks = primaryColor(landmarks_.color_); const auto& Tcw = getTcwMatrix(_capture); const auto& prj_mat = getProjectionMatrix(_capture); @@ -526,73 +539,72 @@ void PublisherVisionDebug::showLandmarks(cv::Mat _image, const auto& lmk_pixel = projectLandmarkHp(trf_prj_mat, lmk); cv::drawMarker(_image, cv::Point(lmk_pixel(0), lmk_pixel(1)), - CV_RGB(colorLmks[0], colorLmks[1], colorLmks[2]), + CV_RGB(color_lmks[0], color_lmks[1], color_lmks[2]), cv::MARKER_TILTED_CROSS, - 5, 1); + landmarks_.size_pix_, 1); - if (show_landmarks_ID_) + if (landmarks_.show_id_) { unsigned int lmk_id = lmk->id(); int shift_text_x = 7; int shift_text_y = 10; - cv::putText( - _image, - std::to_string(lmk_id), - cv::Point(lmk_pixel(0) - shift_text_x, - lmk_pixel(1) - shift_text_y), - 1, size_text_ID_, - CV_RGB(colorLmks[0], colorLmks[1], colorLmks[2])); + cv::putText(_image, + std::to_string(lmk_id), + cv::Point(lmk_pixel(0) - shift_text_x, + lmk_pixel(1) - shift_text_y), + 1, landmarks_.size_id_, + CV_RGB(color_lmks[0], color_lmks[1], color_lmks[2])); } } } } -void PublisherVisionDebug::showTracksPreprocess(cv::Mat _image, - const CaptureImagePtr& _origin, - const CaptureImagePtr& _last) -{ - std::vector<int> color_track_preprocess = primaryColor(color_tracks_features_preprocess_); +// void PublisherVisionDebug::showTracksPreprocess(cv::Mat _image, + // const CaptureImagePtr& _origin, + // const CaptureImagePtr& _last) +// { + // std::vector<int> color_track_preprocess = primaryColor(color_tracks_features_preprocess_); - const auto& tracks_origin = _last->getTracksOrigin(); - const auto& kps_last = _last->getKeyPoints(); - const auto& kps_origin = _origin->getKeyPoints(); + // const auto& tracks_origin = _last->getTracksOrigin(); + // const auto& kps_last = _last->getKeyPoints(); + // const auto& kps_origin = _origin->getKeyPoints(); - for(auto it = tracks_origin.begin(); it != tracks_origin.end(); it++) - { + // for(auto it = tracks_origin.begin(); it != tracks_origin.end(); it++) + // { - const auto& it_ftr_origin = kps_origin.find(it->first); - const auto& it_ftr_last = kps_last.find(it->second); - - if (it_ftr_origin != kps_origin.end() && it_ftr_last != kps_last.end()) - { - const auto& ftr_origin = it_ftr_origin->second.getCvKeyPoint(); - const auto& ftr_last = it_ftr_last->second.getCvKeyPoint(); - - const auto& pos_ftr_origin = std::make_tuple(ftr_origin.pt.x, ftr_origin.pt.y) ; - const auto& pos_ftr_last = std::make_tuple(ftr_last.pt.x, ftr_last.pt.y); - - cv::circle(_image, - cv::Point(std::get<0>(pos_ftr_last), std::get<1>(pos_ftr_last)), - size_feature_curr_pix_, - CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]), - thickness_curr_ftr_); + // const auto& it_ftr_origin = kps_origin.find(it->first); + // const auto& it_ftr_last = kps_last.find(it->second); + + // if (it_ftr_origin != kps_origin.end() && it_ftr_last != kps_last.end()) + // { + // const auto& ftr_origin = it_ftr_origin->second.getCvKeyPoint(); + // const auto& ftr_last = it_ftr_last->second.getCvKeyPoint(); + + // const auto& pos_ftr_origin = std::make_tuple(ftr_origin.pt.x, ftr_origin.pt.y) ; + // const auto& pos_ftr_last = std::make_tuple(ftr_last.pt.x, ftr_last.pt.y); + + // cv::circle(_image, + // cv::Point(std::get<0>(pos_ftr_last), std::get<1>(pos_ftr_last)), + // size_feature_curr_pix_, + // CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]), + // thickness_curr_ftr_); - cv::circle(_image, - cv::Point(std::get<0>(pos_ftr_origin), std::get<1>(pos_ftr_origin)), - size_feature_kfs_pix_, - CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]), - thickness_kfs_ftr_); - - cv::line(_image, - cv::Point(std::get<0>(pos_ftr_last), std::get<1>(pos_ftr_last)), - cv::Point(std::get<0>(pos_ftr_origin), std::get<1>(pos_ftr_origin)), - CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]), - thickness_track_); - } - } -} + // cv::circle(_image, + // cv::Point(std::get<0>(pos_ftr_origin), std::get<1>(pos_ftr_origin)), + // size_feature_kfs_pix_, + // CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]), + // thickness_kfs_ftr_); + + // cv::line(_image, + // cv::Point(std::get<0>(pos_ftr_last), std::get<1>(pos_ftr_last)), + // cv::Point(std::get<0>(pos_ftr_origin), std::get<1>(pos_ftr_origin)), + // CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]), + // thickness_track_); + // } + // } +// } }