cv_bridge.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * Copyright (c) 2015, Tal Regev.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Willow Garage nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35 
36 #include "boost/endian/conversion.hpp"
37 
38 #include <map>
39 
40 #include <boost/make_shared.hpp>
41 #include <boost/regex.hpp>
42 
43 #include <opencv2/imgcodecs.hpp>
44 #include <opencv2/imgproc/imgproc.hpp>
45 
46 #include <sensor_msgs/image_encodings.h>
47 
48 #include <cv_bridge/cv_bridge.h>
49 #include <cv_bridge/rgb_colors.h>
50 
51 namespace enc = sensor_msgs::image_encodings;
52 
53 namespace cv_bridge {
54 
55 static int depthStrToInt(const std::string depth) {
56  if (depth == "8U") {
57  return 0;
58  } else if (depth == "8S") {
59  return 1;
60  } else if (depth == "16U") {
61  return 2;
62  } else if (depth == "16S") {
63  return 3;
64  } else if (depth == "32S") {
65  return 4;
66  } else if (depth == "32F") {
67  return 5;
68  }
69  return 6;
70 }
71 
72 int getCvType(const std::string& encoding)
73 {
74  // Check for the most common encodings first
75  if (encoding == enc::BGR8) return CV_8UC3;
76  if (encoding == enc::MONO8) return CV_8UC1;
77  if (encoding == enc::RGB8) return CV_8UC3;
78  if (encoding == enc::MONO16) return CV_16UC1;
79  if (encoding == enc::BGR16) return CV_16UC3;
80  if (encoding == enc::RGB16) return CV_16UC3;
81  if (encoding == enc::BGRA8) return CV_8UC4;
82  if (encoding == enc::RGBA8) return CV_8UC4;
83  if (encoding == enc::BGRA16) return CV_16UC4;
84  if (encoding == enc::RGBA16) return CV_16UC4;
85 
86  // For bayer, return one-channel
87  if (encoding == enc::BAYER_RGGB8) return CV_8UC1;
88  if (encoding == enc::BAYER_BGGR8) return CV_8UC1;
89  if (encoding == enc::BAYER_GBRG8) return CV_8UC1;
90  if (encoding == enc::BAYER_GRBG8) return CV_8UC1;
91  if (encoding == enc::BAYER_RGGB16) return CV_16UC1;
92  if (encoding == enc::BAYER_BGGR16) return CV_16UC1;
93  if (encoding == enc::BAYER_GBRG16) return CV_16UC1;
94  if (encoding == enc::BAYER_GRBG16) return CV_16UC1;
95 
96  // Miscellaneous
97  if (encoding == enc::YUV422) return CV_8UC2;
98 
99  // Check all the generic content encodings
100  boost::cmatch m;
101 
102  if (boost::regex_match(encoding.c_str(), m,
103  boost::regex("(8U|8S|16U|16S|32S|32F|64F)C([0-9]+)"))) {
104  return CV_MAKETYPE(depthStrToInt(m[1].str()), atoi(m[2].str().c_str()));
105  }
106 
107  if (boost::regex_match(encoding.c_str(), m,
108  boost::regex("(8U|8S|16U|16S|32S|32F|64F)"))) {
109  return CV_MAKETYPE(depthStrToInt(m[1].str()), 1);
110  }
111 
112  throw Exception("Unrecognized image encoding [" + encoding + "]");
113 }
114 
116 
117 enum Encoding { INVALID = -1, GRAY = 0, RGB, BGR, RGBA, BGRA, YUV422, BAYER_RGGB, BAYER_BGGR, BAYER_GBRG, BAYER_GRBG};
118 
119 Encoding getEncoding(const std::string& encoding)
120 {
121  if ((encoding == enc::MONO8) || (encoding == enc::MONO16)) return GRAY;
122  if ((encoding == enc::BGR8) || (encoding == enc::BGR16)) return BGR;
123  if ((encoding == enc::RGB8) || (encoding == enc::RGB16)) return RGB;
124  if ((encoding == enc::BGRA8) || (encoding == enc::BGRA16)) return BGRA;
125  if ((encoding == enc::RGBA8) || (encoding == enc::RGBA16)) return RGBA;
126  if (encoding == enc::YUV422) return YUV422;
127 
128  if ((encoding == enc::BAYER_RGGB8) || (encoding == enc::BAYER_RGGB16)) return BAYER_RGGB;
129  if ((encoding == enc::BAYER_BGGR8) || (encoding == enc::BAYER_BGGR16)) return BAYER_BGGR;
130  if ((encoding == enc::BAYER_GBRG8) || (encoding == enc::BAYER_GBRG16)) return BAYER_GBRG;
131  if ((encoding == enc::BAYER_GRBG8) || (encoding == enc::BAYER_GRBG16)) return BAYER_GRBG;
132 
133  // We don't support conversions to/from other types
134  return INVALID;
135 }
136 
137 static const int SAME_FORMAT = -1;
138 
143 std::map<std::pair<Encoding, Encoding>, std::vector<int> > getConversionCodes() {
144  std::map<std::pair<Encoding, Encoding>, std::vector<int> > res;
145  for(int i=0; i<=5; ++i)
146  res[std::pair<Encoding, Encoding>(Encoding(i),Encoding(i))].push_back(SAME_FORMAT);
147 
148  res[std::make_pair(GRAY, RGB)].push_back(cv::COLOR_GRAY2RGB);
149  res[std::make_pair(GRAY, BGR)].push_back(cv::COLOR_GRAY2BGR);
150  res[std::make_pair(GRAY, RGBA)].push_back(cv::COLOR_GRAY2RGBA);
151  res[std::make_pair(GRAY, BGRA)].push_back(cv::COLOR_GRAY2BGRA);
152 
153  res[std::make_pair(RGB, GRAY)].push_back(cv::COLOR_RGB2GRAY);
154  res[std::make_pair(RGB, BGR)].push_back(cv::COLOR_RGB2BGR);
155  res[std::make_pair(RGB, RGBA)].push_back(cv::COLOR_RGB2RGBA);
156  res[std::make_pair(RGB, BGRA)].push_back(cv::COLOR_RGB2BGRA);
157 
158  res[std::make_pair(BGR, GRAY)].push_back(cv::COLOR_BGR2GRAY);
159  res[std::make_pair(BGR, RGB)].push_back(cv::COLOR_BGR2RGB);
160  res[std::make_pair(BGR, RGBA)].push_back(cv::COLOR_BGR2RGBA);
161  res[std::make_pair(BGR, BGRA)].push_back(cv::COLOR_BGR2BGRA);
162 
163  res[std::make_pair(RGBA, GRAY)].push_back(cv::COLOR_RGBA2GRAY);
164  res[std::make_pair(RGBA, RGB)].push_back(cv::COLOR_RGBA2RGB);
165  res[std::make_pair(RGBA, BGR)].push_back(cv::COLOR_RGBA2BGR);
166  res[std::make_pair(RGBA, BGRA)].push_back(cv::COLOR_RGBA2BGRA);
167 
168  res[std::make_pair(BGRA, GRAY)].push_back(cv::COLOR_BGRA2GRAY);
169  res[std::make_pair(BGRA, RGB)].push_back(cv::COLOR_BGRA2RGB);
170  res[std::make_pair(BGRA, BGR)].push_back(cv::COLOR_BGRA2BGR);
171  res[std::make_pair(BGRA, RGBA)].push_back(cv::COLOR_BGRA2RGBA);
172 
173  res[std::make_pair(YUV422, GRAY)].push_back(cv::COLOR_YUV2GRAY_UYVY);
174  res[std::make_pair(YUV422, RGB)].push_back(cv::COLOR_YUV2RGB_UYVY);
175  res[std::make_pair(YUV422, BGR)].push_back(cv::COLOR_YUV2BGR_UYVY);
176  res[std::make_pair(YUV422, RGBA)].push_back(cv::COLOR_YUV2RGBA_UYVY);
177  res[std::make_pair(YUV422, BGRA)].push_back(cv::COLOR_YUV2BGRA_UYVY);
178 
179  // Deal with Bayer
180  res[std::make_pair(BAYER_RGGB, GRAY)].push_back(cv::COLOR_BayerBG2GRAY);
181  res[std::make_pair(BAYER_RGGB, RGB)].push_back(cv::COLOR_BayerBG2RGB);
182  res[std::make_pair(BAYER_RGGB, BGR)].push_back(cv::COLOR_BayerBG2BGR);
183 
184  res[std::make_pair(BAYER_BGGR, GRAY)].push_back(cv::COLOR_BayerRG2GRAY);
185  res[std::make_pair(BAYER_BGGR, RGB)].push_back(cv::COLOR_BayerRG2RGB);
186  res[std::make_pair(BAYER_BGGR, BGR)].push_back(cv::COLOR_BayerRG2BGR);
187 
188  res[std::make_pair(BAYER_GBRG, GRAY)].push_back(cv::COLOR_BayerGR2GRAY);
189  res[std::make_pair(BAYER_GBRG, RGB)].push_back(cv::COLOR_BayerGR2RGB);
190  res[std::make_pair(BAYER_GBRG, BGR)].push_back(cv::COLOR_BayerGR2BGR);
191 
192  res[std::make_pair(BAYER_GRBG, GRAY)].push_back(cv::COLOR_BayerGB2GRAY);
193  res[std::make_pair(BAYER_GRBG, RGB)].push_back(cv::COLOR_BayerGB2RGB);
194  res[std::make_pair(BAYER_GRBG, BGR)].push_back(cv::COLOR_BayerGB2BGR);
195 
196  return res;
197 }
198 
199 const std::vector<int> getConversionCode(std::string src_encoding, std::string dst_encoding)
200 {
201  Encoding src_encod = getEncoding(src_encoding);
202  Encoding dst_encod = getEncoding(dst_encoding);
203  bool is_src_color_format = enc::isColor(src_encoding) || enc::isMono(src_encoding) ||
204  enc::isBayer(src_encoding) || (src_encoding == enc::YUV422);
205  bool is_dst_color_format = enc::isColor(dst_encoding) || enc::isMono(dst_encoding) ||
206  enc::isBayer(dst_encoding) || (dst_encoding == enc::YUV422);
207  bool is_num_channels_the_same = (enc::numChannels(src_encoding) == enc::numChannels(dst_encoding));
208 
209  // If we have no color info in the source, we can only convert to the same format which
210  // was resolved in the previous condition. Otherwise, fail
211  if (!is_src_color_format) {
212  if (is_dst_color_format)
213  throw Exception("[" + src_encoding + "] is not a color format. but [" + dst_encoding +
214  "] is. The conversion does not make sense");
215  if (!is_num_channels_the_same)
216  throw Exception("[" + src_encoding + "] and [" + dst_encoding + "] do not have the same number of channel");
217  return std::vector<int>(1, SAME_FORMAT);
218  }
219 
220  // If we are converting from a color type to a non color type, we can only do so if we stick
221  // to the number of channels
222  if (!is_dst_color_format) {
223  if (!is_num_channels_the_same)
224  throw Exception("[" + src_encoding + "] is a color format but [" + dst_encoding + "] " +
225  "is not so they must have the same OpenCV type, CV_8UC3, CV_16UC1 ....");
226  return std::vector<int>(1, SAME_FORMAT);
227  }
228 
229  // If we are converting from a color type to another type, then everything is fine
230  static const std::map<std::pair<Encoding, Encoding>, std::vector<int> > CONVERSION_CODES = getConversionCodes();
231 
232  std::pair<Encoding, Encoding> key(src_encod, dst_encod);
233  std::map<std::pair<Encoding, Encoding>, std::vector<int> >::const_iterator val = CONVERSION_CODES.find(key);
234  if (val == CONVERSION_CODES.end())
235  throw Exception("Unsupported conversion from [" + src_encoding +
236  "] to [" + dst_encoding + "]");
237 
238  // And deal with depth differences if the colors are different
239  std::vector<int> res = val->second;
240  if ((enc::bitDepth(src_encoding) != enc::bitDepth(dst_encoding)) && (getEncoding(src_encoding) != getEncoding(dst_encoding)))
241  res.push_back(SAME_FORMAT);
242 
243  return res;
244 }
245 
247 
248 // Converts a ROS Image to a cv::Mat by sharing the data or changing its endianness if needed
249 cv::Mat matFromImage(const sensor_msgs::Image& source)
250 {
251  int source_type = getCvType(source.encoding);
252  int byte_depth = enc::bitDepth(source.encoding) / 8;
253  int num_channels = enc::numChannels(source.encoding);
254 
255  if (source.step < source.width * byte_depth * num_channels)
256  {
257  std::stringstream ss;
258  ss << "Image is wrongly formed: step < width * byte_depth * num_channels or " << source.step << " != " <<
259  source.width << " * " << byte_depth << " * " << num_channels;
260  throw Exception(ss.str());
261  }
262 
263  if (source.height * source.step != source.data.size())
264  {
265  std::stringstream ss;
266  ss << "Image is wrongly formed: height * step != size or " << source.height << " * " <<
267  source.step << " != " << source.data.size();
268  throw Exception(ss.str());
269  }
270 
271  // If the endianness is the same as locally, share the data
272  cv::Mat mat(source.height, source.width, source_type, const_cast<uchar*>(&source.data[0]), source.step);
273  if ((boost::endian::order::native == boost::endian::order::big && source.is_bigendian) ||
274  (boost::endian::order::native == boost::endian::order::little && !source.is_bigendian) ||
275  byte_depth == 1)
276  return mat;
277 
278  // Otherwise, reinterpret the data as bytes and switch the channels accordingly
279  mat = cv::Mat(source.height, source.width, CV_MAKETYPE(CV_8U, num_channels*byte_depth),
280  const_cast<uchar*>(&source.data[0]), source.step);
281  cv::Mat mat_swap(source.height, source.width, mat.type());
282 
283  std::vector<int> fromTo;
284  fromTo.reserve(num_channels*byte_depth);
285  for(int i = 0; i < num_channels; ++i)
286  for(int j = 0; j < byte_depth; ++j)
287  {
288  fromTo.push_back(byte_depth*i + j);
289  fromTo.push_back(byte_depth*i + byte_depth - 1 - j);
290  }
291  cv::mixChannels(std::vector<cv::Mat>(1, mat), std::vector<cv::Mat>(1, mat_swap), fromTo);
292 
293  // Interpret mat_swap back as the proper type
294  mat_swap.reshape(num_channels);
295 
296  return mat_swap;
297 }
298 
299 // Internal, used by toCvCopy and cvtColor
300 CvImagePtr toCvCopyImpl(const cv::Mat& source,
301  const std_msgs::Header& src_header,
302  const std::string& src_encoding,
303  const std::string& dst_encoding)
304 {
305  // Copy metadata
306  CvImagePtr ptr = boost::make_shared<CvImage>();
307  ptr->header = src_header;
308 
309  // Copy to new buffer if same encoding requested
310  if (dst_encoding.empty() || dst_encoding == src_encoding)
311  {
312  ptr->encoding = src_encoding;
313  source.copyTo(ptr->image);
314  }
315  else
316  {
317  // Convert the source data to the desired encoding
318  const std::vector<int> conversion_codes = getConversionCode(src_encoding, dst_encoding);
319  cv::Mat image1 = source;
320  cv::Mat image2;
321  for(size_t i=0; i<conversion_codes.size(); ++i) {
322  int conversion_code = conversion_codes[i];
323  if (conversion_code == SAME_FORMAT)
324  {
325  // Same number of channels, but different bit depth
326  int src_depth = enc::bitDepth(src_encoding);
327  int dst_depth = enc::bitDepth(dst_encoding);
328  // Keep the number of channels for now but changed to the final depth
329  int image2_type = CV_MAKETYPE(CV_MAT_DEPTH(getCvType(dst_encoding)), image1.channels());
330 
331  // Do scaling between CV_8U [0,255] and CV_16U [0,65535] images.
332  if (src_depth == 8 && dst_depth == 16)
333  image1.convertTo(image2, image2_type, 65535. / 255.);
334  else if (src_depth == 16 && dst_depth == 8)
335  image1.convertTo(image2, image2_type, 255. / 65535.);
336  else
337  image1.convertTo(image2, image2_type);
338  }
339  else
340  {
341  // Perform color conversion
342  cv::cvtColor(image1, image2, conversion_code);
343  }
344  image1 = image2;
345  }
346  ptr->image = image2;
347  ptr->encoding = dst_encoding;
348  }
349 
350  return ptr;
351 }
352 
354 
355 sensor_msgs::ImagePtr CvImage::toImageMsg() const
356 {
357  sensor_msgs::ImagePtr ptr = boost::make_shared<sensor_msgs::Image>();
358  toImageMsg(*ptr);
359  return ptr;
360 }
361 
362 void CvImage::toImageMsg(sensor_msgs::Image& ros_image) const
363 {
364  ros_image.header = header;
365  ros_image.height = image.rows;
366  ros_image.width = image.cols;
367  ros_image.encoding = encoding;
368  ros_image.is_bigendian = (boost::endian::order::native == boost::endian::order::big);
369  ros_image.step = image.cols * image.elemSize();
370  size_t size = ros_image.step * image.rows;
371  ros_image.data.resize(size);
372 
373  if (image.isContinuous())
374  {
375  memcpy((char*)(&ros_image.data[0]), image.data, size);
376  }
377  else
378  {
379  // Copy by row by row
380  uchar* ros_data_ptr = (uchar*)(&ros_image.data[0]);
381  uchar* cv_data_ptr = image.data;
382  for (int i = 0; i < image.rows; ++i)
383  {
384  memcpy(ros_data_ptr, cv_data_ptr, ros_image.step);
385  ros_data_ptr += ros_image.step;
386  cv_data_ptr += image.step;
387  }
388  }
389 }
390 
391 // Deep copy data, returnee is mutable
392 CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr& source,
393  const std::string& encoding)
394 {
395  return toCvCopy(*source, encoding);
396 }
397 
398 CvImagePtr toCvCopy(const sensor_msgs::Image& source,
399  const std::string& encoding)
400 {
401  // Construct matrix pointing to source data
402  return toCvCopyImpl(matFromImage(source), source.header, source.encoding, encoding);
403 }
404 
405 // Share const data, returnee is immutable
406 CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr& source,
407  const std::string& encoding)
408 {
409  return toCvShare(*source, source, encoding);
410 }
411 
412 CvImageConstPtr toCvShare(const sensor_msgs::Image& source,
413  const boost::shared_ptr<void const>& tracked_object,
414  const std::string& encoding)
415 {
416  // If the encoding different or the endianness different, you have to copy
417  if ((!encoding.empty() && source.encoding != encoding) || (source.is_bigendian &&
418  (boost::endian::order::native != boost::endian::order::big)))
419  return toCvCopy(source, encoding);
420 
421  CvImagePtr ptr = boost::make_shared<CvImage>();
422  ptr->header = source.header;
423  ptr->encoding = source.encoding;
424  ptr->tracked_object_ = tracked_object;
425  ptr->image = matFromImage(source);
426  return ptr;
427 }
428 
429 CvImagePtr cvtColor(const CvImageConstPtr& source,
430  const std::string& encoding)
431 {
432  return toCvCopyImpl(source->image, source->header, source->encoding, encoding);
433 }
434 
436 
437 sensor_msgs::CompressedImagePtr CvImage::toCompressedImageMsg(const Format dst_format) const
438 {
439  sensor_msgs::CompressedImagePtr ptr = boost::make_shared<sensor_msgs::CompressedImage>();
440  toCompressedImageMsg(*ptr,dst_format);
441  return ptr;
442 }
443 
444 std::string getFormat(Format format) {
445 
446  switch (format) {
447  case DIB:
448  return "dib";
449  case BMP:
450  return "bmp";
451  case JPG:
452  return "jpg";
453  case JPEG:
454  return "jpeg";
455  case JPE:
456  return "jpe";
457  case JP2:
458  return "jp2";
459  case PNG:
460  return "png";
461  case PBM:
462  return "pbm";
463  case PGM:
464  return "pgm";
465  case PPM:
466  return "ppm";
467  case RAS:
468  return "ras";
469  case SR:
470  return "sr";
471  case TIF:
472  return "tif";
473  case TIFF:
474  return "tiff";
475  }
476 
477  throw Exception("Unrecognized image format");
478 }
479 
480 void CvImage::toCompressedImageMsg(sensor_msgs::CompressedImage& ros_image, const Format dst_format) const
481 {
482  ros_image.header = header;
483  cv::Mat image;
484  if (encoding == enc::BGR8 || encoding == enc::BGRA8)
485  {
486  image = this->image;
487  }
488  else
489  {
490  CvImagePtr tempThis = boost::make_shared<CvImage>(*this);
491  CvImagePtr temp;
492  if (enc::hasAlpha(encoding))
493  {
494  temp = cvtColor(tempThis, enc::BGRA8);
495  }
496  else
497  {
498  temp = cvtColor(tempThis, enc::BGR8);
499  }
500  image = temp->image;
501  }
502 
503  std::string format = getFormat(dst_format);
504  ros_image.format = format;
505  cv::imencode("." + format, image, ros_image.data);
506 }
507 
508 // Deep copy data, returnee is mutable
509 CvImagePtr toCvCopy(const sensor_msgs::CompressedImageConstPtr& source,
510  const std::string& encoding)
511 {
512  return toCvCopy(*source, encoding);
513 }
514 
515 CvImagePtr toCvCopy(const sensor_msgs::CompressedImage& source, const std::string& encoding)
516 {
517  // Construct matrix pointing to source data
518  const cv::Mat_<uchar> in(1, source.data.size(), const_cast<uchar*>(&source.data[0]));
519  // Loads as BGR or BGRA.
520  const cv::Mat rgb_a = cv::imdecode(in, cv::IMREAD_UNCHANGED);
521 
522  switch (rgb_a.channels())
523  {
524  case 4:
525  return toCvCopyImpl(rgb_a, source.header, enc::BGRA8, encoding);
526  break;
527  case 3:
528  return toCvCopyImpl(rgb_a, source.header, enc::BGR8, encoding);
529  break;
530  case 1:
531  return toCvCopyImpl(rgb_a, source.header, enc::MONO8, encoding);
532  break;
533  default:
534  return CvImagePtr();
535  }
536 }
537 
538 CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
539  const std::string& encoding_out,
540  const CvtColorForDisplayOptions options)
541 {
542  double min_image_value = options.min_image_value;
543  double max_image_value = options.max_image_value;
544 
545  if (!source)
546  throw Exception("cv_bridge.cvtColorForDisplay() called with empty image.");
547  // let's figure out what to do with the empty encoding
548  std::string encoding = encoding_out;
549  if (encoding.empty())
550  {
551  try
552  {
553  // Let's decide upon an output format
554  if (enc::numChannels(source->encoding) == 1)
555  {
556  if ((source->encoding == enc::TYPE_32SC1) ||
557  (enc::bitDepth(source->encoding) == 8) ||
558  (enc::bitDepth(source->encoding) == 16) ||
559  (enc::bitDepth(source->encoding) == 32))
560  encoding = enc::BGR8;
561  else
562  throw std::runtime_error("Unsupported depth of the source encoding " + encoding);
563  }
564  else
565  {
566  // We choose BGR by default here as we assume people will use OpenCV
567  if ((enc::bitDepth(source->encoding) == 8) ||
568  (enc::bitDepth(source->encoding) == 16))
569  encoding = enc::BGR8;
570  else
571  throw std::runtime_error("Unsupported depth of the source encoding " + encoding);
572  }
573  }
574  // We could have cv_bridge exception or std_runtime_error from sensor_msgs::image_codings routines
575  catch (const std::runtime_error& e)
576  {
577  throw Exception("cv_bridge.cvtColorForDisplay() output encoding is empty and cannot be guessed.");
578  }
579  }
580  else
581  {
582  if ((!enc::isColor(encoding_out) && !enc::isMono(encoding_out)) ||
583  (enc::bitDepth(encoding) != 8))
584  throw Exception("cv_bridge.cvtColorForDisplay() does not have an output encoding that is color or mono, and has is bit in depth");
585 
586  }
587 
588  // Convert label to bgr image
589  if (encoding == sensor_msgs::image_encodings::BGR8 &&
590  source->encoding == enc::TYPE_32SC1)
591  {
592  CvImagePtr result(new CvImage());
593  result->header = source->header;
594  result->encoding = encoding;
595  result->image = cv::Mat(source->image.rows, source->image.cols, CV_8UC3);
596  for (size_t j = 0; j < source->image.rows; ++j) {
597  for (size_t i = 0; i < source->image.cols; ++i) {
598  int label = source->image.at<int>(j, i);
599  if (label == options.bg_label) { // background label
600  result->image.at<cv::Vec3b>(j, i) = cv::Vec3b(0, 0, 0);
601  }
602  else
603  {
604  cv::Vec3d rgb = rgb_colors::getRGBColor(label);
605  // result image should be BGR
606  result->image.at<cv::Vec3b>(j, i) = cv::Vec3b(int(rgb[2] * 255), int(rgb[1] * 255), int(rgb[0] * 255));
607  }
608  }
609  }
610  return result;
611  }
612 
613  // Perform scaling if asked for
614  if (options.do_dynamic_scaling)
615  {
616  cv::minMaxLoc(source->image, &min_image_value, &max_image_value);
617  if (min_image_value == max_image_value)
618  {
619  CvImagePtr result(new CvImage());
620  result->header = source->header;
621  result->encoding = encoding;
622  if (enc::bitDepth(encoding) == 1)
623  {
624  result->image = cv::Mat(source->image.size(), CV_8UC1);
625  result->image.setTo(255./2.);
626  } else {
627  result->image = cv::Mat(source->image.size(), CV_8UC3);
628  result->image.setTo(cv::Scalar(1., 1., 1.)*255./2.);
629  }
630  return result;
631  }
632  }
633 
634  if (min_image_value != max_image_value)
635  {
636  if (enc::numChannels(source->encoding) != 1)
637  throw Exception("cv_bridge.cvtColorForDisplay() scaling for images with more than one channel is unsupported");
638  CvImagePtr img_scaled(new CvImage());
639  img_scaled->header = source->header;
640  if (options.colormap == -1) {
641  img_scaled->encoding = enc::MONO8;
642  cv::Mat(source->image-min_image_value).convertTo(img_scaled->image, CV_8UC1, 255.0 /
643  (max_image_value - min_image_value));
644  } else {
645  img_scaled->encoding = enc::BGR8;
646  cv::Mat(source->image-min_image_value).convertTo(img_scaled->image, CV_8UC3, 255.0 /
647  (max_image_value - min_image_value));
648  cv::applyColorMap(img_scaled->image, img_scaled->image, options.colormap);
649  // Fill black color to the nan region.
650  if (source->encoding == enc::TYPE_32FC1) {
651  for (size_t j = 0; j < source->image.rows; ++j) {
652  for (size_t i = 0; i < source->image.cols; ++i) {
653  float float_value = source->image.at<float>(j, i);
654  if (std::isnan(float_value)) {
655  img_scaled->image.at<cv::Vec3b>(j, i) = cv::Vec3b(0, 0, 0);
656  }
657  }
658  }
659  }
660  }
661  return cvtColor(img_scaled, encoding);
662  }
663 
664  // If no color conversion is possible, we must "guess" the input format
665  CvImagePtr source_typed(new CvImage());
666  source_typed->image = source->image;
667  source_typed->header = source->header;
668  source_typed->encoding = source->encoding;
669 
670  // If we get the OpenCV format, if we have 1,3 or 4 channels, we are most likely in mono, BGR or BGRA modes
671  if (source->encoding == "CV_8UC1")
672  source_typed->encoding = enc::MONO8;
673  else if (source->encoding == "16UC1")
674  source_typed->encoding = enc::MONO16;
675  else if (source->encoding == "CV_8UC3")
676  source_typed->encoding = enc::BGR8;
677  else if (source->encoding == "CV_8UC4")
678  source_typed->encoding = enc::BGRA8;
679  else if (source->encoding == "CV_16UC3")
680  source_typed->encoding = enc::BGR8;
681  else if (source->encoding == "CV_16UC4")
682  source_typed->encoding = enc::BGRA8;
683 
684  // If no conversion is needed, don't convert
685  if (source_typed->encoding == encoding)
686  return source;
687 
688  try
689  {
690  // Now that the output is a proper color format, try to see if any conversion is possible
691  return cvtColor(source_typed, encoding);
692  }
693  catch (cv_bridge::Exception& e)
694  {
695  throw Exception("cv_bridge.cvtColorForDisplay() while trying to convert image from '" + source->encoding + "' to '" + encoding + "' an exception was thrown (" + e.what() + ")");
696  }
697 }
698 
699 } //namespace cv_bridge
sensor_msgs::image_encodings
cv_bridge::toCvShare
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
Convert an immutable sensor_msgs::Image message to an OpenCV-compatible CvImage, sharing the image da...
Definition: cv_bridge.cpp:406
sensor_msgs::image_encodings::BGR8
const std::string BGR8
cv_bridge::JPG
Definition: cv_bridge.h:64
cv_bridge::JPE
Definition: cv_bridge.h:64
cv_bridge::TIF
Definition: cv_bridge.h:69
cv_bridge::CvImagePtr
boost::shared_ptr< CvImage > CvImagePtr
Definition: cv_bridge.h:56
cv_bridge::TIFF
Definition: cv_bridge.h:69
cv_bridge::Exception
Definition: cv_bridge.h:50
cv_bridge::DIB
Definition: cv_bridge.h:63
cv_bridge::CvImage::encoding
std::string encoding
Image encoding ("mono8", "bgr8", etc.)
Definition: cv_bridge.h:80
cv_bridge::PBM
Definition: cv_bridge.h:67
cv_bridge::PPM
Definition: cv_bridge.h:67
boost::shared_ptr
cv_bridge::CvtColorForDisplayOptions::bg_label
int bg_label
Definition: cv_bridge.h:261
cv_bridge.h
cv_bridge::CvtColorForDisplayOptions::colormap
int colormap
Definition: cv_bridge.h:260
cv_bridge::CvtColorForDisplayOptions
Definition: cv_bridge.h:250
cv_bridge::cvtColor
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)
Convert a CvImage to another encoding using the same rules as toCvCopy.
Definition: cv_bridge.cpp:429
cv_bridge::JPEG
Definition: cv_bridge.h:64
cv_bridge::CvImage::CvImage
CvImage()
Empty constructor.
Definition: cv_bridge.h:86
cv_bridge::toCvCopy
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
Convert a sensor_msgs::Image message to an OpenCV-compatible CvImage, copying the image data...
Definition: cv_bridge.cpp:392
cv_bridge::SR
Definition: cv_bridge.h:68
cv_bridge::RAS
Definition: cv_bridge.h:68
cv_bridge
Definition: cv_bridge.h:48
cv_bridge::depthStrToInt
static int depthStrToInt(const std::string depth)
Definition: cv_bridge.cpp:55
cv_bridge::CvImage::toCompressedImageMsg
sensor_msgs::CompressedImagePtr toCompressedImageMsg(const Format dst_format=JPG) const
Definition: cv_bridge.cpp:437
cv_bridge::CvImage::image
cv::Mat image
Image data for use with OpenCV.
Definition: cv_bridge.h:81
cv_bridge::getCvType
int getCvType(const std::string &encoding)
Get the OpenCV type enum corresponding to the encoding.
Definition: cv_bridge.cpp:72
cv_bridge::JP2
Definition: cv_bridge.h:65
cv_bridge::PGM
Definition: cv_bridge.h:67
cv_bridge::CvtColorForDisplayOptions::do_dynamic_scaling
bool do_dynamic_scaling
Definition: cv_bridge.h:257
YUV422
const std::string YUV422
cv_bridge::cvtColorForDisplay
CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr &source, const std::string &encoding=std::string(), const CvtColorForDisplayOptions options=CvtColorForDisplayOptions())
Converts an immutable sensor_msgs::Image message to another CvImage for display purposes, using practical conversion rules if needed.
Definition: cv_bridge.cpp:538
image_encodings.h
cv_bridge::CvtColorForDisplayOptions::max_image_value
double max_image_value
Definition: cv_bridge.h:259
cv_bridge::rgb_colors::GRAY
Definition: rgb_colors.h:106
cv_bridge::rgb_colors::getRGBColor
cv::Vec3d getRGBColor(const int color)
get rgb color with enum.
Definition: rgb_colors.cpp:46
cv_bridge::CvtColorForDisplayOptions::min_image_value
double min_image_value
Definition: cv_bridge.h:258
rgb_colors.h
cv_bridge::CvImage::toImageMsg
sensor_msgs::ImagePtr toImageMsg() const
Convert this message to a ROS sensor_msgs::Image message.
Definition: cv_bridge.cpp:355
cv_bridge::getFormat
std::string getFormat(Format format)
Definition: cv_bridge.cpp:444
cv_bridge::Format
Format
Definition: cv_bridge.h:62
cv_bridge::CvImage::header
std_msgs::Header header
ROS header.
Definition: cv_bridge.h:79
cv_bridge::BMP
Definition: cv_bridge.h:63
cv_bridge::PNG
Definition: cv_bridge.h:66

cv_bridge
Author(s): Patrick Mihelich, James Bowman
autogenerated on Thu Dec 12 2019 03:52:01