@@ -60,7 +60,7 @@ PointField makeField(const std::string& name,
6060 return field;
6161}
6262
63- cv::Mat showImage (const cv::Mat& input, const DisplayConfig& config) {
63+ cv::Mat resizeImage (const cv::Mat& input, const DisplayConfig& config) {
6464 if (config.width_scale == 1 .0f && config.height_scale == 1 .0f ) {
6565 return input;
6666 }
@@ -82,49 +82,85 @@ void declare_config(DisplayConfig& config) {
8282 name (" DisplayConfig"
8383 field (config.width_scale , " width_scale"
8484 field (config.height_scale , " height_scale"
85+ field (config.overlay_alpha , " overlay_alpha"
86+ field (config.min_distance , " min_distance" " m"
87+ field (config.max_distance , " max_distance" " m"
88+ field (config.distance_colormap , " distance_colormap" " m"
89+ // overlay == 0.0 doesn't make sense, but overlay == 1.0 does
90+ checkInRange (config.overlay_alpha , 0 .0f , 1 .0f , " overlay_alpha" false , true );
8591}
8692
87- Image::SharedPtr makeImage (const std_msgs::msg::Header& header,
88- const InputData& sensor_data,
89- const CmapFunc& colormap,
90- const DisplayConfig& config) {
91- const auto & labels = sensor_data.label_image ;
93+ Image::SharedPtr convertImage (const std_msgs::msg::Header& header,
94+ const cv::Mat& img,
95+ const DisplayConfig& config) {
9296 cv_bridge::CvImagePtr msg (new cv_bridge::CvImage ());
9397 msg->header = header;
9498 msg->encoding = " rgb8"
95- cv::Mat img (labels.rows , labels.cols , CV_8UC3);
96- for (int r = 0 ; r < labels.rows ; ++r) {
97- for (int c = 0 ; c < labels.cols ; ++c) {
99+ msg->image = resizeImage (img, config);
100+ return msg->toImageMsg ();
101+ }
102+
103+ Image::SharedPtr makeImage (const std_msgs::msg::Header& header,
104+ const cv::Mat& img_in,
105+ const CmapFunc& colormap,
106+ const DisplayConfig& config) {
107+ cv::Mat img (img_in.rows , img_in.cols , CV_8UC3);
108+ for (int r = 0 ; r < img_in.rows ; ++r) {
109+ for (int c = 0 ; c < img_in.cols ; ++c) {
98110 auto pixel = img.ptr <uint8_t >(r, c);
99- const auto color = colormap (labels. at < int >( r, c) );
111+ const auto color = colormap (img_in, r, c);
100112 *pixel = color.r ;
101113 *(pixel + 1 ) = color.g ;
102114 *(pixel + 2 ) = color.b ;
103115 }
104116 }
105117
106- msg->image = showImage (img, config);
107- return msg->toImageMsg ();
118+ return convertImage (header, img, config);
108119}
109120
110- Image::SharedPtr makeDepthImage (const std_msgs::msg::Header& header,
111- const InputData& sensor_data,
112- const DisplayConfig& config) {
113- cv_bridge::CvImagePtr msg (new cv_bridge::CvImage ());
114- msg->header = header;
115- msg->encoding = " 32FC1"
116- msg->image = showImage (sensor_data.depth_image , config);
117- return msg->toImageMsg ();
121+ Image::SharedPtr makeOverlayImage (const std_msgs::msg::Header& header,
122+ const cv::Mat& img_in,
123+ const cv::Mat& color_in,
124+ const CmapFunc& colormap,
125+ const DisplayConfig& config) {
126+ if (color_in.empty ()) {
127+ return makeImage (header, img_in, colormap, config);
128+ }
129+
130+ cv::Mat img (img_in.rows , img_in.cols , CV_8UC3);
131+ for (int r = 0 ; r < img_in.rows ; ++r) {
132+ for (int c = 0 ; c < img_in.cols ; ++c) {
133+ auto pixel = img.ptr <uint8_t >(r, c);
134+ const auto c_in = colormap (img_in, r, c);
135+ const auto c_cv = color_in.at <cv::Vec3b>(r, c);
136+ const auto color =
137+ c_in.blend (spark_dsg::Color (c_cv[0 ], c_cv[1 ], c_cv[2 ]), config.overlay_alpha );
138+
139+ *pixel = color.r ;
140+ *(pixel + 1 ) = color.g ;
141+ *(pixel + 2 ) = color.b ;
142+ }
143+ }
144+
145+ return convertImage (header, img, config);
118146}
119147
120- Image::SharedPtr makeRangeImage (const std_msgs::msg::Header& header,
121- const InputData& sensor_data,
122- const DisplayConfig& config) {
123- cv_bridge::CvImagePtr msg (new cv_bridge::CvImage ());
124- msg->header = header;
125- msg->encoding = " 32FC1"
126- msg->image = showImage (sensor_data.range_image , config);
127- return msg->toImageMsg ();
148+ Image::SharedPtr makeDistImage (const std_msgs::msg::Header& header,
149+ const cv::Mat& distances,
150+ const DisplayConfig& config) {
151+ double min_v = 0.0 ;
152+ double max_v = std::numeric_limits<double >::infinity ();
153+ cv::minMaxIdx (distances, &min_v, &max_v);
154+ const float v_min = config.min_distance >= 0 .0f ? config.min_distance : min_v;
155+ const float v_max = config.max_distance >= 0 .0f ? config.max_distance : max_v;
156+ const visualizer::RangeColormap cmap (config.distance_colormap );
157+ return makeImage (
158+ header,
159+ distances,
160+ [&](const cv::Mat& img, int r, int c) {
161+ return cmap (img.at <float >(r, c), v_min, v_max);
162+ },
163+ config);
128164}
129165
130166// TODO(nathan) pcl_ros would avoid this but it causes compile issues (and would also
0 commit comments