00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include "boost/endian/conversion.hpp"
00037
00038 #include <map>
00039
00040 #include <boost/make_shared.hpp>
00041 #include <boost/regex.hpp>
00042
00043 #include <opencv2/imgproc/imgproc.hpp>
00044
00045 #include <opencv2/highgui/highgui.hpp>
00046
00047 #include <sensor_msgs/image_encodings.h>
00048
00049 #include <cv_bridge/cv_bridge.h>
00050 #include <cv_bridge/rgb_colors.h>
00051
00052 namespace enc = sensor_msgs::image_encodings;
00053
00054 namespace cv_bridge {
00055
00056 static int depthStrToInt(const std::string depth) {
00057 if (depth == "8U") {
00058 return 0;
00059 } else if (depth == "8S") {
00060 return 1;
00061 } else if (depth == "16U") {
00062 return 2;
00063 } else if (depth == "16S") {
00064 return 3;
00065 } else if (depth == "32S") {
00066 return 4;
00067 } else if (depth == "32F") {
00068 return 5;
00069 }
00070 return 6;
00071 }
00072
00073 int getCvType(const std::string& encoding)
00074 {
00075
00076 if (encoding == enc::BGR8) return CV_8UC3;
00077 if (encoding == enc::MONO8) return CV_8UC1;
00078 if (encoding == enc::RGB8) return CV_8UC3;
00079 if (encoding == enc::MONO16) return CV_16UC1;
00080 if (encoding == enc::BGR16) return CV_16UC3;
00081 if (encoding == enc::RGB16) return CV_16UC3;
00082 if (encoding == enc::BGRA8) return CV_8UC4;
00083 if (encoding == enc::RGBA8) return CV_8UC4;
00084 if (encoding == enc::BGRA16) return CV_16UC4;
00085 if (encoding == enc::RGBA16) return CV_16UC4;
00086
00087
00088 if (encoding == enc::BAYER_RGGB8) return CV_8UC1;
00089 if (encoding == enc::BAYER_BGGR8) return CV_8UC1;
00090 if (encoding == enc::BAYER_GBRG8) return CV_8UC1;
00091 if (encoding == enc::BAYER_GRBG8) return CV_8UC1;
00092 if (encoding == enc::BAYER_RGGB16) return CV_16UC1;
00093 if (encoding == enc::BAYER_BGGR16) return CV_16UC1;
00094 if (encoding == enc::BAYER_GBRG16) return CV_16UC1;
00095 if (encoding == enc::BAYER_GRBG16) return CV_16UC1;
00096
00097
00098 if (encoding == enc::YUV422) return CV_8UC2;
00099
00100
00101 boost::cmatch m;
00102
00103 if (boost::regex_match(encoding.c_str(), m,
00104 boost::regex("(8U|8S|16U|16S|32S|32F|64F)C([0-9]+)"))) {
00105 return CV_MAKETYPE(depthStrToInt(m[1].str()), atoi(m[2].str().c_str()));
00106 }
00107
00108 if (boost::regex_match(encoding.c_str(), m,
00109 boost::regex("(8U|8S|16U|16S|32S|32F|64F)"))) {
00110 return CV_MAKETYPE(depthStrToInt(m[1].str()), 1);
00111 }
00112
00113 throw Exception("Unrecognized image encoding [" + encoding + "]");
00114 }
00115
00117
00118 enum Encoding { INVALID = -1, GRAY = 0, RGB, BGR, RGBA, BGRA, YUV422, BAYER_RGGB, BAYER_BGGR, BAYER_GBRG, BAYER_GRBG};
00119
00120 Encoding getEncoding(const std::string& encoding)
00121 {
00122 if ((encoding == enc::MONO8) || (encoding == enc::MONO16)) return GRAY;
00123 if ((encoding == enc::BGR8) || (encoding == enc::BGR16)) return BGR;
00124 if ((encoding == enc::RGB8) || (encoding == enc::RGB16)) return RGB;
00125 if ((encoding == enc::BGRA8) || (encoding == enc::BGRA16)) return BGRA;
00126 if ((encoding == enc::RGBA8) || (encoding == enc::RGBA16)) return RGBA;
00127 if (encoding == enc::YUV422) return YUV422;
00128
00129 if ((encoding == enc::BAYER_RGGB8) || (encoding == enc::BAYER_RGGB16)) return BAYER_RGGB;
00130 if ((encoding == enc::BAYER_BGGR8) || (encoding == enc::BAYER_BGGR16)) return BAYER_BGGR;
00131 if ((encoding == enc::BAYER_GBRG8) || (encoding == enc::BAYER_GBRG16)) return BAYER_GBRG;
00132 if ((encoding == enc::BAYER_GRBG8) || (encoding == enc::BAYER_GRBG16)) return BAYER_GRBG;
00133
00134
00135 return INVALID;
00136 }
00137
00138 static const int SAME_FORMAT = -1;
00139
00144 std::map<std::pair<Encoding, Encoding>, std::vector<int> > getConversionCodes() {
00145 std::map<std::pair<Encoding, Encoding>, std::vector<int> > res;
00146 for(int i=0; i<=5; ++i)
00147 res[std::pair<Encoding, Encoding>(Encoding(i),Encoding(i))].push_back(SAME_FORMAT);
00148
00149 res[std::make_pair(GRAY, RGB)].push_back(cv::COLOR_GRAY2RGB);
00150 res[std::make_pair(GRAY, BGR)].push_back(cv::COLOR_GRAY2BGR);
00151 res[std::make_pair(GRAY, RGBA)].push_back(cv::COLOR_GRAY2RGBA);
00152 res[std::make_pair(GRAY, BGRA)].push_back(cv::COLOR_GRAY2BGRA);
00153
00154 res[std::make_pair(RGB, GRAY)].push_back(cv::COLOR_RGB2GRAY);
00155 res[std::make_pair(RGB, BGR)].push_back(cv::COLOR_RGB2BGR);
00156 res[std::make_pair(RGB, RGBA)].push_back(cv::COLOR_RGB2RGBA);
00157 res[std::make_pair(RGB, BGRA)].push_back(cv::COLOR_RGB2BGRA);
00158
00159 res[std::make_pair(BGR, GRAY)].push_back(cv::COLOR_BGR2GRAY);
00160 res[std::make_pair(BGR, RGB)].push_back(cv::COLOR_BGR2RGB);
00161 res[std::make_pair(BGR, RGBA)].push_back(cv::COLOR_BGR2RGBA);
00162 res[std::make_pair(BGR, BGRA)].push_back(cv::COLOR_BGR2BGRA);
00163
00164 res[std::make_pair(RGBA, GRAY)].push_back(cv::COLOR_RGBA2GRAY);
00165 res[std::make_pair(RGBA, RGB)].push_back(cv::COLOR_RGBA2RGB);
00166 res[std::make_pair(RGBA, BGR)].push_back(cv::COLOR_RGBA2BGR);
00167 res[std::make_pair(RGBA, BGRA)].push_back(cv::COLOR_RGBA2BGRA);
00168
00169 res[std::make_pair(BGRA, GRAY)].push_back(cv::COLOR_BGRA2GRAY);
00170 res[std::make_pair(BGRA, RGB)].push_back(cv::COLOR_BGRA2RGB);
00171 res[std::make_pair(BGRA, BGR)].push_back(cv::COLOR_BGRA2BGR);
00172 res[std::make_pair(BGRA, RGBA)].push_back(cv::COLOR_BGRA2RGBA);
00173
00174 res[std::make_pair(YUV422, GRAY)].push_back(cv::COLOR_YUV2GRAY_UYVY);
00175 res[std::make_pair(YUV422, RGB)].push_back(cv::COLOR_YUV2RGB_UYVY);
00176 res[std::make_pair(YUV422, BGR)].push_back(cv::COLOR_YUV2BGR_UYVY);
00177 res[std::make_pair(YUV422, RGBA)].push_back(cv::COLOR_YUV2RGBA_UYVY);
00178 res[std::make_pair(YUV422, BGRA)].push_back(cv::COLOR_YUV2BGRA_UYVY);
00179
00180
00181 res[std::make_pair(BAYER_RGGB, GRAY)].push_back(cv::COLOR_BayerBG2GRAY);
00182 res[std::make_pair(BAYER_RGGB, RGB)].push_back(cv::COLOR_BayerBG2RGB);
00183 res[std::make_pair(BAYER_RGGB, BGR)].push_back(cv::COLOR_BayerBG2BGR);
00184
00185 res[std::make_pair(BAYER_BGGR, GRAY)].push_back(cv::COLOR_BayerRG2GRAY);
00186 res[std::make_pair(BAYER_BGGR, RGB)].push_back(cv::COLOR_BayerRG2RGB);
00187 res[std::make_pair(BAYER_BGGR, BGR)].push_back(cv::COLOR_BayerRG2BGR);
00188
00189 res[std::make_pair(BAYER_GBRG, GRAY)].push_back(cv::COLOR_BayerGR2GRAY);
00190 res[std::make_pair(BAYER_GBRG, RGB)].push_back(cv::COLOR_BayerGR2RGB);
00191 res[std::make_pair(BAYER_GBRG, BGR)].push_back(cv::COLOR_BayerGR2BGR);
00192
00193 res[std::make_pair(BAYER_GRBG, GRAY)].push_back(cv::COLOR_BayerGB2GRAY);
00194 res[std::make_pair(BAYER_GRBG, RGB)].push_back(cv::COLOR_BayerGB2RGB);
00195 res[std::make_pair(BAYER_GRBG, BGR)].push_back(cv::COLOR_BayerGB2BGR);
00196
00197 return res;
00198 }
00199
00200 const std::vector<int> getConversionCode(std::string src_encoding, std::string dst_encoding)
00201 {
00202 Encoding src_encod = getEncoding(src_encoding);
00203 Encoding dst_encod = getEncoding(dst_encoding);
00204 bool is_src_color_format = enc::isColor(src_encoding) || enc::isMono(src_encoding) ||
00205 enc::isBayer(src_encoding) || (src_encoding == enc::YUV422);
00206 bool is_dst_color_format = enc::isColor(dst_encoding) || enc::isMono(dst_encoding) ||
00207 enc::isBayer(dst_encoding) || (dst_encoding == enc::YUV422);
00208 bool is_num_channels_the_same = (enc::numChannels(src_encoding) == enc::numChannels(dst_encoding));
00209
00210
00211
00212 if (!is_src_color_format) {
00213 if (is_dst_color_format)
00214 throw Exception("[" + src_encoding + "] is not a color format. but [" + dst_encoding +
00215 "] is. The conversion does not make sense");
00216 if (!is_num_channels_the_same)
00217 throw Exception("[" + src_encoding + "] and [" + dst_encoding + "] do not have the same number of channel");
00218 return std::vector<int>(1, SAME_FORMAT);
00219 }
00220
00221
00222
00223 if (!is_dst_color_format) {
00224 if (!is_num_channels_the_same)
00225 throw Exception("[" + src_encoding + "] is a color format but [" + dst_encoding + "] " +
00226 "is not so they must have the same OpenCV type, CV_8UC3, CV16UC1 ....");
00227 return std::vector<int>(1, SAME_FORMAT);
00228 }
00229
00230
00231 static const std::map<std::pair<Encoding, Encoding>, std::vector<int> > CONVERSION_CODES = getConversionCodes();
00232
00233 std::pair<Encoding, Encoding> key(src_encod, dst_encod);
00234 std::map<std::pair<Encoding, Encoding>, std::vector<int> >::const_iterator val = CONVERSION_CODES.find(key);
00235 if (val == CONVERSION_CODES.end())
00236 throw Exception("Unsupported conversion from [" + src_encoding +
00237 "] to [" + dst_encoding + "]");
00238
00239
00240 std::vector<int> res = val->second;
00241 if ((enc::bitDepth(src_encoding) != enc::bitDepth(dst_encoding)) && (getEncoding(src_encoding) != getEncoding(dst_encoding)))
00242 res.push_back(SAME_FORMAT);
00243
00244 return res;
00245 }
00246
00248
00249
00250 cv::Mat matFromImage(const sensor_msgs::Image& source)
00251 {
00252 int source_type = getCvType(source.encoding);
00253 int byte_depth = enc::bitDepth(source.encoding) / 8;
00254 int num_channels = enc::numChannels(source.encoding);
00255
00256 if (source.step < source.width * byte_depth * num_channels)
00257 {
00258 std::stringstream ss;
00259 ss << "Image is wrongly formed: step < width * byte_depth * num_channels or " << source.step << " != " <<
00260 source.width << " * " << byte_depth << " * " << num_channels;
00261 throw Exception(ss.str());
00262 }
00263
00264 if (source.height * source.step != source.data.size())
00265 {
00266 std::stringstream ss;
00267 ss << "Image is wrongly formed: height * step != size or " << source.height << " * " <<
00268 source.step << " != " << source.data.size();
00269 throw Exception(ss.str());
00270 }
00271
00272
00273 cv::Mat mat(source.height, source.width, source_type, const_cast<uchar*>(&source.data[0]), source.step);
00274 if ((boost::endian::order::native == boost::endian::order::big && source.is_bigendian) ||
00275 (boost::endian::order::native == boost::endian::order::little && !source.is_bigendian) ||
00276 byte_depth == 1)
00277 return mat;
00278
00279
00280 mat = cv::Mat(source.height, source.width, CV_MAKETYPE(CV_8U, num_channels*byte_depth),
00281 const_cast<uchar*>(&source.data[0]), source.step);
00282 cv::Mat mat_swap(source.height, source.width, mat.type());
00283
00284 std::vector<int> fromTo;
00285 fromTo.reserve(num_channels*byte_depth);
00286 for(int i = 0; i < num_channels; ++i)
00287 for(int j = 0; j < byte_depth; ++j)
00288 {
00289 fromTo.push_back(byte_depth*i + j);
00290 fromTo.push_back(byte_depth*i + byte_depth - 1 - j);
00291 }
00292 cv::mixChannels(std::vector<cv::Mat>(1, mat), std::vector<cv::Mat>(1, mat_swap), fromTo);
00293
00294
00295 mat_swap = cv::Mat(source.height, source.width, source_type, mat_swap.data, mat_swap.step);
00296
00297 return mat_swap;
00298 }
00299
00300
00301 CvImagePtr toCvCopyImpl(const cv::Mat& source,
00302 const std_msgs::Header& src_header,
00303 const std::string& src_encoding,
00304 const std::string& dst_encoding)
00305 {
00306
00307 CvImagePtr ptr = boost::make_shared<CvImage>();
00308 ptr->header = src_header;
00309
00310
00311 if (dst_encoding.empty() || dst_encoding == src_encoding)
00312 {
00313 ptr->encoding = src_encoding;
00314 source.copyTo(ptr->image);
00315 }
00316 else
00317 {
00318
00319 const std::vector<int> conversion_codes = getConversionCode(src_encoding, dst_encoding);
00320 cv::Mat image1 = source;
00321 cv::Mat image2;
00322 for(size_t i=0; i<conversion_codes.size(); ++i) {
00323 int conversion_code = conversion_codes[i];
00324 if (conversion_code == SAME_FORMAT)
00325 {
00326
00327 int src_depth = enc::bitDepth(src_encoding);
00328 int dst_depth = enc::bitDepth(dst_encoding);
00329
00330 int image2_type = CV_MAKETYPE(CV_MAT_DEPTH(getCvType(dst_encoding)), image1.channels());
00331
00332
00333 if (src_depth == 8 && dst_depth == 16)
00334 image1.convertTo(image2, image2_type, 65535. / 255.);
00335 else if (src_depth == 16 && dst_depth == 8)
00336 image1.convertTo(image2, image2_type, 255. / 65535.);
00337 else
00338 image1.convertTo(image2, image2_type);
00339 }
00340 else
00341 {
00342
00343 cv::cvtColor(image1, image2, conversion_code);
00344 }
00345 image1 = image2;
00346 }
00347 ptr->image = image2;
00348 ptr->encoding = dst_encoding;
00349 }
00350
00351 return ptr;
00352 }
00353
00355
00356 sensor_msgs::ImagePtr CvImage::toImageMsg() const
00357 {
00358 sensor_msgs::ImagePtr ptr = boost::make_shared<sensor_msgs::Image>();
00359 toImageMsg(*ptr);
00360 return ptr;
00361 }
00362
00363 void CvImage::toImageMsg(sensor_msgs::Image& ros_image) const
00364 {
00365 ros_image.header = header;
00366 ros_image.height = image.rows;
00367 ros_image.width = image.cols;
00368 ros_image.encoding = encoding;
00369 ros_image.is_bigendian = (boost::endian::order::native == boost::endian::order::big);
00370 ros_image.step = image.cols * image.elemSize();
00371 size_t size = ros_image.step * image.rows;
00372 ros_image.data.resize(size);
00373
00374 if (image.isContinuous())
00375 {
00376 memcpy((char*)(&ros_image.data[0]), image.data, size);
00377 }
00378 else
00379 {
00380
00381 uchar* ros_data_ptr = (uchar*)(&ros_image.data[0]);
00382 uchar* cv_data_ptr = image.data;
00383 for (int i = 0; i < image.rows; ++i)
00384 {
00385 memcpy(ros_data_ptr, cv_data_ptr, ros_image.step);
00386 ros_data_ptr += ros_image.step;
00387 cv_data_ptr += image.step;
00388 }
00389 }
00390 }
00391
00392
00393 CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr& source,
00394 const std::string& encoding)
00395 {
00396 return toCvCopy(*source, encoding);
00397 }
00398
00399 CvImagePtr toCvCopy(const sensor_msgs::Image& source,
00400 const std::string& encoding)
00401 {
00402
00403 return toCvCopyImpl(matFromImage(source), source.header, source.encoding, encoding);
00404 }
00405
00406
00407 CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr& source,
00408 const std::string& encoding)
00409 {
00410 return toCvShare(*source, source, encoding);
00411 }
00412
00413 CvImageConstPtr toCvShare(const sensor_msgs::Image& source,
00414 const boost::shared_ptr<void const>& tracked_object,
00415 const std::string& encoding)
00416 {
00417
00418 if ((!encoding.empty() && source.encoding != encoding) || (source.is_bigendian &&
00419 (boost::endian::order::native != boost::endian::order::big)))
00420 return toCvCopy(source, encoding);
00421
00422 CvImagePtr ptr = boost::make_shared<CvImage>();
00423 ptr->header = source.header;
00424 ptr->encoding = source.encoding;
00425 ptr->tracked_object_ = tracked_object;
00426 ptr->image = matFromImage(source);
00427 return ptr;
00428 }
00429
00430 CvImagePtr cvtColor(const CvImageConstPtr& source,
00431 const std::string& encoding)
00432 {
00433 return toCvCopyImpl(source->image, source->header, source->encoding, encoding);
00434 }
00435
00437
00438 sensor_msgs::CompressedImagePtr CvImage::toCompressedImageMsg(const Format dst_format) const
00439 {
00440 sensor_msgs::CompressedImagePtr ptr = boost::make_shared<sensor_msgs::CompressedImage>();
00441 toCompressedImageMsg(*ptr,dst_format);
00442 return ptr;
00443 }
00444
00445 std::string getFormat(Format format) {
00446
00447 switch (format) {
00448 case DIB:
00449 return "dib";
00450 case BMP:
00451 return "bmp";
00452 case JPG:
00453 return "jpg";
00454 case JPEG:
00455 return "jpeg";
00456 case JPE:
00457 return "jpe";
00458 case JP2:
00459 return "jp2";
00460 case PNG:
00461 return "png";
00462 case PBM:
00463 return "pbm";
00464 case PGM:
00465 return "pgm";
00466 case PPM:
00467 return "ppm";
00468 case RAS:
00469 return "ras";
00470 case SR:
00471 return "sr";
00472 case TIF:
00473 return "tif";
00474 case TIFF:
00475 return "tiff";
00476 }
00477
00478 throw Exception("Unrecognized image format");
00479 }
00480
00481 void CvImage::toCompressedImageMsg(sensor_msgs::CompressedImage& ros_image, const Format dst_format) const
00482 {
00483 ros_image.header = header;
00484 cv::Mat image;
00485 if (encoding == enc::BGR8 || encoding == enc::BGRA8)
00486 {
00487 image = this->image;
00488 }
00489 else
00490 {
00491 CvImagePtr tempThis = boost::make_shared<CvImage>(*this);
00492 CvImagePtr temp;
00493 if (enc::hasAlpha(encoding))
00494 {
00495 temp = cvtColor(tempThis, enc::BGRA8);
00496 }
00497 else
00498 {
00499 temp = cvtColor(tempThis, enc::BGR8);
00500 }
00501 image = temp->image;
00502 }
00503
00504 std::string format = getFormat(dst_format);
00505 ros_image.format = format;
00506 cv::imencode("." + format, image, ros_image.data);
00507 }
00508
00509
00510 CvImagePtr toCvCopy(const sensor_msgs::CompressedImageConstPtr& source,
00511 const std::string& encoding)
00512 {
00513 return toCvCopy(*source, encoding);
00514 }
00515
00516 CvImagePtr toCvCopy(const sensor_msgs::CompressedImage& source, const std::string& encoding)
00517 {
00518
00519 const cv::Mat_<uchar> in(1, source.data.size(), const_cast<uchar*>(&source.data[0]));
00520
00521 const cv::Mat rgb_a = cv::imdecode(in, cv::IMREAD_UNCHANGED);
00522
00523 switch (rgb_a.channels())
00524 {
00525 case 4:
00526 return toCvCopyImpl(rgb_a, source.header, enc::BGRA8, encoding);
00527 break;
00528 case 3:
00529 return toCvCopyImpl(rgb_a, source.header, enc::BGR8, encoding);
00530 break;
00531 case 1:
00532 return toCvCopyImpl(rgb_a, source.header, enc::MONO8, encoding);
00533 break;
00534 default:
00535 return CvImagePtr();
00536 }
00537 }
00538
00539 CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
00540 const std::string& encoding_out,
00541 const CvtColorForDisplayOptions options)
00542 {
00543 double min_image_value = options.min_image_value;
00544 double max_image_value = options.max_image_value;
00545
00546 if (!source)
00547 throw Exception("cv_bridge.cvtColorForDisplay() called with empty image.");
00548
00549 std::string encoding = encoding_out;
00550 if (encoding.empty())
00551 {
00552 try
00553 {
00554
00555 if (enc::numChannels(source->encoding) == 1)
00556 {
00557 if ((source->encoding == enc::TYPE_32SC1) ||
00558 (enc::bitDepth(source->encoding) == 8) ||
00559 (enc::bitDepth(source->encoding) == 16) ||
00560 (enc::bitDepth(source->encoding) == 32))
00561 encoding = enc::BGR8;
00562 else
00563 throw std::runtime_error("Unsupported depth of the source encoding " + encoding);
00564 }
00565 else
00566 {
00567
00568 if ((enc::bitDepth(source->encoding) == 8) ||
00569 (enc::bitDepth(source->encoding) == 16))
00570 encoding = enc::BGR8;
00571 else
00572 throw std::runtime_error("Unsupported depth of the source encoding " + encoding);
00573 }
00574 }
00575
00576 catch (const std::runtime_error& e)
00577 {
00578 throw Exception("cv_bridge.cvtColorForDisplay() output encoding is empty and cannot be guessed.");
00579 }
00580 }
00581 else
00582 {
00583 if ((!enc::isColor(encoding_out) && !enc::isMono(encoding_out)) ||
00584 (enc::bitDepth(encoding) != 8))
00585 throw Exception("cv_bridge.cvtColorForDisplay() does not have an output encoding that is color or mono, and has is bit in depth");
00586
00587 }
00588
00589
00590 if (encoding == sensor_msgs::image_encodings::BGR8 &&
00591 source->encoding == enc::TYPE_32SC1)
00592 {
00593 CvImagePtr result(new CvImage());
00594 result->header = source->header;
00595 result->encoding = encoding;
00596 result->image = cv::Mat(source->image.rows, source->image.cols, CV_8UC3);
00597 for (size_t j = 0; j < source->image.rows; ++j) {
00598 for (size_t i = 0; i < source->image.cols; ++i) {
00599 int label = source->image.at<int>(j, i);
00600 if (label == options.bg_label) {
00601 result->image.at<cv::Vec3b>(j, i) = cv::Vec3b(0, 0, 0);
00602 }
00603 else
00604 {
00605 cv::Vec3d rgb = rgb_colors::getRGBColor(label);
00606
00607 result->image.at<cv::Vec3b>(j, i) = cv::Vec3b(int(rgb[2] * 255), int(rgb[1] * 255), int(rgb[0] * 255));
00608 }
00609 }
00610 }
00611 return result;
00612 }
00613
00614
00615 if (options.do_dynamic_scaling)
00616 {
00617 cv::minMaxLoc(source->image, &min_image_value, &max_image_value);
00618 if (min_image_value == max_image_value)
00619 {
00620 CvImagePtr result(new CvImage());
00621 result->header = source->header;
00622 result->encoding = encoding;
00623 if (enc::bitDepth(encoding) == 1)
00624 {
00625 result->image = cv::Mat(source->image.size(), CV_8UC1);
00626 result->image.setTo(255./2.);
00627 } else {
00628 result->image = cv::Mat(source->image.size(), CV_8UC3);
00629 result->image.setTo(cv::Scalar(1., 1., 1.)*255./2.);
00630 }
00631 return result;
00632 }
00633 }
00634
00635 if (min_image_value != max_image_value)
00636 {
00637 if (enc::numChannels(source->encoding) != 1)
00638 throw Exception("cv_bridge.cvtColorForDisplay() scaling for images with more than one channel is unsupported");
00639 CvImagePtr img_scaled(new CvImage());
00640 img_scaled->header = source->header;
00641 if (options.colormap == -1) {
00642 img_scaled->encoding = enc::MONO8;
00643 cv::Mat(source->image-min_image_value).convertTo(img_scaled->image, CV_8UC1, 255.0 /
00644 (max_image_value - min_image_value));
00645 } else {
00646 img_scaled->encoding = enc::BGR8;
00647 cv::Mat(source->image-min_image_value).convertTo(img_scaled->image, CV_8UC3, 255.0 /
00648 (max_image_value - min_image_value));
00649 cv::applyColorMap(img_scaled->image, img_scaled->image, options.colormap);
00650
00651 if (source->encoding == enc::TYPE_32FC1) {
00652 for (size_t j = 0; j < source->image.rows; ++j) {
00653 for (size_t i = 0; i < source->image.cols; ++i) {
00654 float float_value = source->image.at<float>(j, i);
00655 if (std::isnan(float_value)) {
00656 img_scaled->image.at<cv::Vec3b>(j, i) = cv::Vec3b(0, 0, 0);
00657 }
00658 }
00659 }
00660 }
00661 }
00662 return cvtColor(img_scaled, encoding);
00663 }
00664
00665
00666 CvImagePtr source_typed(new CvImage());
00667 source_typed->image = source->image;
00668 source_typed->header = source->header;
00669 source_typed->encoding = source->encoding;
00670
00671
00672 if (source->encoding == "CV_8UC1")
00673 source_typed->encoding = enc::MONO8;
00674 else if (source->encoding == "16UC1")
00675 source_typed->encoding = enc::MONO16;
00676 else if (source->encoding == "CV_8UC3")
00677 source_typed->encoding = enc::BGR8;
00678 else if (source->encoding == "CV_8UC4")
00679 source_typed->encoding = enc::BGRA8;
00680 else if (source->encoding == "CV_16UC3")
00681 source_typed->encoding = enc::BGR8;
00682 else if (source->encoding == "CV_16UC4")
00683 source_typed->encoding = enc::BGRA8;
00684
00685
00686 if (source_typed->encoding == encoding)
00687 return source;
00688
00689 try
00690 {
00691
00692 return cvtColor(source_typed, encoding);
00693 }
00694 catch (cv_bridge::Exception& e)
00695 {
00696 throw Exception("cv_bridge.cvtColorForDisplay() while trying to convert image from '" + source->encoding + "' to '" + encoding + "' an exception was thrown (" + e.what() + ")");
00697 }
00698 }
00699
00700 }