diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp index 578301ba4561eb079d7a866c715563c43ae9068d..cf538bf35aafb761d7f69507294c3cea3efdae2b 100644 --- a/src/publisher_vision.cpp +++ b/src/publisher_vision.cpp @@ -47,7 +47,7 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name, last_capture_(nullptr), it_(ros::NodeHandle()) { - // if user do not provide processor's name, first processor of type ProcessorTrackerLandmarkApriltag is taken + // 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", ""); // search the processor @@ -167,23 +167,25 @@ std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_ // std::cout << "_max_feature_in_tracks: " << _max_feature_in_tracks << '\n'; if (_max_feature_in_tracks != _min_feature_in_tracks) { - alpha = ((double)_nb_feature_in_track - _min_feature_in_tracks) - / (_max_feature_in_tracks - _min_feature_in_tracks); // to interval [0,1] + alpha = (double)(_nb_feature_in_track - _min_feature_in_tracks) + / (double)(_max_feature_in_tracks - _min_feature_in_tracks); // to interval [0,1] } else { - alpha = 0.5; // put "half luminosity as a default value" + alpha = 1.0; // put "full luminosity as a default value" +// alpha = 0.5; // put "half luminosity as a default value" } - if ((_min_lum + (_max_lum - _min_lum) * (1 - alpha)) > 255) + double lum = (_min_lum + (_max_lum - _min_lum) * alpha); + if (lum > 255) { color[0] = 255; - color[1] = round(_min_lum + (_max_lum - _min_lum) * (1 - alpha)) - 255; - color[2] = round(_min_lum + (_max_lum - _min_lum) * (1 - alpha)) - 255; + color[1] = round(lum) - 255; + color[2] = round(lum) - 255; } else { - color[0] = round(_min_lum + (_max_lum - _min_lum) * (1 - alpha)); + color[0] = round(lum); color[1] = 0; color[2] = 0; }