cv_bridge.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include <map>
00036 
00037 #include <boost/make_shared.hpp>
00038 
00039 #include <opencv2/imgproc/imgproc.hpp>
00040 
00041 #include <sensor_msgs/image_encodings.h>
00042 
00043 #include <cv_bridge/cv_bridge.h>
00044 
00045 namespace enc = sensor_msgs::image_encodings;
00046 
00047 namespace cv_bridge {
00048 
00049 int getCvType(const std::string& encoding)
00050 {
00051   // Check for the most common encodings first
00052   if (encoding == enc::BGR8)   return CV_8UC3;
00053   if (encoding == enc::MONO8)  return CV_8UC1;
00054   if (encoding == enc::RGB8)   return CV_8UC3;
00055   if (encoding == enc::MONO16) return CV_16UC1;
00056   if (encoding == enc::BGR16)  return CV_16UC3;
00057   if (encoding == enc::RGB16)  return CV_16UC3;
00058   if (encoding == enc::BGRA8)  return CV_8UC4;
00059   if (encoding == enc::RGBA8)  return CV_8UC4;
00060   if (encoding == enc::BGRA16) return CV_16UC4;
00061   if (encoding == enc::RGBA16) return CV_16UC4;
00062 
00063   // For bayer, return one-channel
00064   if (encoding == enc::BAYER_RGGB8) return CV_8UC1;
00065   if (encoding == enc::BAYER_BGGR8) return CV_8UC1;
00066   if (encoding == enc::BAYER_GBRG8) return CV_8UC1;
00067   if (encoding == enc::BAYER_GRBG8) return CV_8UC1;
00068   if (encoding == enc::BAYER_RGGB16) return CV_16UC1;
00069   if (encoding == enc::BAYER_BGGR16) return CV_16UC1;
00070   if (encoding == enc::BAYER_GBRG16) return CV_16UC1;
00071   if (encoding == enc::BAYER_GRBG16) return CV_16UC1;
00072 
00073   // Miscellaneous
00074   if (encoding == enc::YUV422) return CV_8UC2;
00075 
00076   // Check all the generic content encodings
00077 #define CHECK_ENCODING(code)                            \
00078   if (encoding == enc::TYPE_##code) return CV_##code    \
00079   /***/
00080 #define CHECK_CHANNEL_TYPE(t)                   \
00081   CHECK_ENCODING(t##1);                         \
00082   CHECK_ENCODING(t##2);                         \
00083   CHECK_ENCODING(t##3);                         \
00084   CHECK_ENCODING(t##4);                         \
00085   /***/
00086 
00087   CHECK_CHANNEL_TYPE(8UC);
00088   CHECK_CHANNEL_TYPE(8SC);
00089   CHECK_CHANNEL_TYPE(16UC);
00090   CHECK_CHANNEL_TYPE(16SC);
00091   CHECK_CHANNEL_TYPE(32SC);
00092   CHECK_CHANNEL_TYPE(32FC);
00093   CHECK_CHANNEL_TYPE(64FC);
00094 
00095 #undef CHECK_CHANNEL_TYPE
00096 #undef CHECK_ENCODING
00097 
00098   throw Exception("Unrecognized image encoding [" + encoding + "]");
00099 }
00100 
00102 
00103 enum Format { INVALID = -1, GRAY = 0, RGB, BGR, RGBA, BGRA, YUV422, BAYER_RGGB, BAYER_BGGR, BAYER_GBRG, BAYER_GRBG};
00104 
00105 Format getFormat(const std::string& encoding)
00106 {
00107   if ((encoding == enc::MONO8) || (encoding == enc::MONO16)) return GRAY;
00108   if ((encoding == enc::BGR8) || (encoding == enc::BGR16))  return BGR;
00109   if ((encoding == enc::RGB8) || (encoding == enc::RGB16))  return RGB;
00110   if ((encoding == enc::BGRA8) || (encoding == enc::BGRA16))  return BGRA;
00111   if ((encoding == enc::RGBA8) || (encoding == enc::RGBA16))  return RGBA;
00112   if (encoding == enc::YUV422) return YUV422;
00113 
00114   if ((encoding == enc::BAYER_RGGB8) || (encoding == enc::BAYER_RGGB16)) return BAYER_RGGB;
00115   if ((encoding == enc::BAYER_BGGR8) || (encoding == enc::BAYER_BGGR16)) return BAYER_BGGR;
00116   if ((encoding == enc::BAYER_GBRG8) || (encoding == enc::BAYER_GBRG16)) return BAYER_GBRG;
00117   if ((encoding == enc::BAYER_GRBG8) || (encoding == enc::BAYER_GRBG16)) return BAYER_GRBG;
00118 
00119   // We don't support conversions to/from other types
00120   return INVALID;
00121 }
00122 
00123 static const int SAME_FORMAT = -1;
00124 
00129 std::map<std::pair<Format, Format>, std::vector<int> > getConversionCodes() {
00130   std::map<std::pair<Format, Format>, std::vector<int> > res;
00131   for(int i=0; i<=5; ++i)
00132     res[std::pair<Format, Format>(Format(i),Format(i))].push_back(SAME_FORMAT);
00133   res[std::make_pair(GRAY, RGB)].push_back(CV_GRAY2RGB);
00134   res[std::make_pair(GRAY, BGR)].push_back(CV_GRAY2BGR);
00135   res[std::make_pair(GRAY, RGBA)].push_back(CV_GRAY2RGBA);
00136   res[std::make_pair(GRAY, BGRA)].push_back(CV_GRAY2BGRA);
00137 
00138   res[std::make_pair(RGB, GRAY)].push_back(CV_RGB2GRAY);
00139   res[std::make_pair(RGB, BGR)].push_back(CV_RGB2BGR);
00140   res[std::make_pair(RGB, RGBA)].push_back(CV_RGB2RGBA);
00141   res[std::make_pair(RGB, BGRA)].push_back(CV_RGB2BGRA);
00142 
00143   res[std::make_pair(BGR, GRAY)].push_back(CV_BGR2GRAY);
00144   res[std::make_pair(BGR, RGB)].push_back(CV_BGR2RGB);
00145   res[std::make_pair(BGR, RGBA)].push_back(CV_BGR2RGBA);
00146   res[std::make_pair(BGR, BGRA)].push_back(CV_BGR2BGRA);
00147 
00148   res[std::make_pair(RGBA, GRAY)].push_back(CV_RGBA2GRAY);
00149   res[std::make_pair(RGBA, RGB)].push_back(CV_RGBA2RGB);
00150   res[std::make_pair(RGBA, BGR)].push_back(CV_RGBA2BGR);
00151   res[std::make_pair(RGBA, BGRA)].push_back(CV_RGBA2BGRA);
00152 
00153   res[std::make_pair(BGRA, GRAY)].push_back(CV_BGRA2GRAY);
00154   res[std::make_pair(BGRA, RGB)].push_back(CV_BGRA2RGB);
00155   res[std::make_pair(BGRA, BGR)].push_back(CV_BGRA2BGR);
00156   res[std::make_pair(BGRA, RGBA)].push_back(CV_BGRA2RGBA);
00157 
00158   res[std::make_pair(YUV422, GRAY)].push_back(CV_YUV2GRAY_UYVY);
00159   res[std::make_pair(YUV422, RGB)].push_back(CV_YUV2RGB_UYVY);
00160   res[std::make_pair(YUV422, BGR)].push_back(CV_YUV2BGR_UYVY);
00161   res[std::make_pair(YUV422, RGBA)].push_back(CV_YUV2RGBA_UYVY);
00162   res[std::make_pair(YUV422, BGRA)].push_back(CV_YUV2BGRA_UYVY);
00163 
00164   // Deal with Bayer
00165   res[std::make_pair(BAYER_RGGB, GRAY)].push_back(CV_BayerBG2GRAY);
00166   res[std::make_pair(BAYER_RGGB, RGB)].push_back(CV_BayerBG2RGB);
00167   res[std::make_pair(BAYER_RGGB, BGR)].push_back(CV_BayerBG2BGR);
00168 
00169   res[std::make_pair(BAYER_BGGR, GRAY)].push_back(CV_BayerRG2GRAY);
00170   res[std::make_pair(BAYER_BGGR, RGB)].push_back(CV_BayerRG2RGB);
00171   res[std::make_pair(BAYER_BGGR, BGR)].push_back(CV_BayerRG2BGR);
00172 
00173   res[std::make_pair(BAYER_GBRG, GRAY)].push_back(CV_BayerGR2GRAY);
00174   res[std::make_pair(BAYER_GBRG, RGB)].push_back(CV_BayerGR2RGB);
00175   res[std::make_pair(BAYER_GBRG, BGR)].push_back(CV_BayerGR2BGR);
00176 
00177   res[std::make_pair(BAYER_GRBG, GRAY)].push_back(CV_BayerGB2GRAY);
00178   res[std::make_pair(BAYER_GRBG, RGB)].push_back(CV_BayerGB2RGB);
00179   res[std::make_pair(BAYER_GRBG, BGR)].push_back(CV_BayerGB2BGR);
00180 
00181   return res;
00182 }
00183 
00184 const std::vector<int> getConversionCode(std::string src_encoding, std::string dst_encoding)
00185 {
00186   Format src_format = getFormat(src_encoding);
00187   Format dst_format = getFormat(dst_encoding);
00188   bool is_src_color_format = sensor_msgs::image_encodings::isColor(src_encoding) ||
00189                              sensor_msgs::image_encodings::isMono(src_encoding) ||
00190                              sensor_msgs::image_encodings::isBayer(src_encoding) ||
00191                              (src_encoding == sensor_msgs::image_encodings::YUV422);
00192   bool is_dst_color_format = sensor_msgs::image_encodings::isColor(dst_encoding) ||
00193                              sensor_msgs::image_encodings::isMono(dst_encoding) ||
00194                              sensor_msgs::image_encodings::isBayer(dst_encoding) ||
00195                              (dst_encoding == sensor_msgs::image_encodings::YUV422);
00196   bool is_num_channels_the_same = (sensor_msgs::image_encodings::numChannels(src_encoding) == sensor_msgs::image_encodings::numChannels(dst_encoding));
00197 
00198   // If we have no color info in the source, we can only convert to the same format which
00199   // was resolved in the previous condition. Otherwise, fail
00200   if (!is_src_color_format) {
00201     if (is_dst_color_format)
00202       throw Exception("[" + src_encoding + "] is not a color format. but [" + dst_encoding +
00203                       "] is. The conversion does not make sense");
00204     if (!is_num_channels_the_same)
00205       throw Exception("[" + src_encoding + "] and [" + dst_encoding + "] do not have the same number of channel");
00206     return std::vector<int>(1, SAME_FORMAT);
00207   }
00208 
00209   // If we are converting from a color type to a non color type, we can only do so if we stick
00210   // to the number of channels
00211   if (!is_dst_color_format) {
00212     if (!is_num_channels_the_same)
00213       throw Exception("[" + src_encoding + "] is a color format but [" + dst_encoding + "] " +
00214                       "is not so they must have the same OpenCV type, CV_8UC3, CV16UC1 ....");
00215     return std::vector<int>(1, SAME_FORMAT);
00216   }
00217 
00218   // If we are converting from a color type to another type, then everything is fine
00219   static const std::map<std::pair<Format, Format>, std::vector<int> > CONVERSION_CODES = getConversionCodes();
00220 
00221   std::pair<Format, Format> key(src_format, dst_format);
00222   std::map<std::pair<Format, Format>, std::vector<int> >::const_iterator val = CONVERSION_CODES.find(key);
00223   if (val == CONVERSION_CODES.end())
00224     throw Exception("Unsupported conversion from [" + src_encoding +
00225                       "] to [" + dst_encoding + "]");
00226 
00227   // And deal with depth differences
00228   std::vector<int> res = val->second;
00229   if (enc::bitDepth(src_encoding) != enc::bitDepth(dst_encoding))
00230     res.push_back(SAME_FORMAT);
00231 
00232   return val->second;
00233 }
00234 
00235 cv::Mat matFromImage(const sensor_msgs::Image& source)
00236 {
00237   int source_type = getCvType(source.encoding);
00238   int byte_depth = enc::bitDepth(source.encoding) / 8;
00239   int num_channels = enc::numChannels(source.encoding);
00240 
00241   if (source.step < source.width * byte_depth * num_channels)
00242   {
00243     std::stringstream ss;
00244     ss << "Image is wrongly formed: step < width * byte_depth * num_channels  or  " << source.step << " != " <<
00245         source.width << " * " << byte_depth << " * " << num_channels;
00246     throw Exception(ss.str());
00247   }
00248 
00249   if (source.height * source.step != source.data.size())
00250   {
00251     std::stringstream ss;
00252     ss << "Image is wrongly formed: height * step != size  or  " << source.height << " * " <<
00253               source.step << " != " << source.data.size();
00254     throw Exception(ss.str());
00255   }
00256 
00257   return cv::Mat(source.height, source.width, source_type,
00258                        const_cast<uchar*>(&source.data[0]), source.step);
00259 }
00260 
00261 // Internal, used by toCvCopy and cvtColor
00262 CvImagePtr toCvCopyImpl(const cv::Mat& source,
00263                         const std_msgs::Header& src_header,
00264                         const std::string& src_encoding,
00265                         const std::string& dst_encoding)
00266 {
00268   
00269   // Copy metadata
00270   CvImagePtr ptr = boost::make_shared<CvImage>();
00271   ptr->header = src_header;
00272   
00273   // Copy to new buffer if same encoding requested
00274   if (dst_encoding.empty() || dst_encoding == src_encoding)
00275   {
00276     ptr->encoding = src_encoding;
00277     source.copyTo(ptr->image);
00278   }
00279   else
00280   {
00281     // Convert the source data to the desired encoding
00282     const std::vector<int> conversion_codes = getConversionCode(src_encoding, dst_encoding);
00283     cv::Mat image1 = source;
00284     cv::Mat image2;
00285     for(size_t i=0; i<conversion_codes.size(); ++i) {
00286       int conversion_code = conversion_codes[i];
00287       if (conversion_code == SAME_FORMAT)
00288       {
00289         // Same number of channels, but different bit depth
00290         double alpha = 1.0;
00291         int src_depth = enc::bitDepth(src_encoding);
00292         int dst_depth = enc::bitDepth(dst_encoding);
00293         // Do scaling between CV_8U [0,255] and CV_16U [0,65535] images.
00294         if (src_depth == 8 && dst_depth == 16)
00295           image1.convertTo(image2, getCvType(dst_encoding), 65535. / 255.);
00296         else if (src_depth == 16 && dst_depth == 8)
00297           image1.convertTo(image2, getCvType(dst_encoding), 255. / 65535.);
00298         else
00299           image1.convertTo(image2, getCvType(dst_encoding));
00300       }
00301       else
00302       {
00303         // Perform color conversion
00304         cv::cvtColor(image1, image2, conversion_code);
00305       }
00306       image1 = image2;
00307     }
00308     ptr->image = image2;
00309     ptr->encoding = dst_encoding;
00310   }
00311 
00312   return ptr;
00313 }
00314 
00316 
00317 sensor_msgs::ImagePtr CvImage::toImageMsg() const
00318 {
00319   sensor_msgs::ImagePtr ptr = boost::make_shared<sensor_msgs::Image>();
00320   toImageMsg(*ptr);
00321   return ptr;
00322 }
00323 
00324 void CvImage::toImageMsg(sensor_msgs::Image& ros_image) const
00325 {
00326   ros_image.header = header;
00327   ros_image.height = image.rows;
00328   ros_image.width = image.cols;
00329   ros_image.encoding = encoding;
00330   ros_image.is_bigendian = false;
00331   ros_image.step = image.cols * image.elemSize();
00332   size_t size = ros_image.step * image.rows;
00333   ros_image.data.resize(size);
00334 
00335   if (image.isContinuous())
00336   {
00337     memcpy((char*)(&ros_image.data[0]), image.data, size);
00338   }
00339   else
00340   {
00341     // Copy by row by row
00342     uchar* ros_data_ptr = (uchar*)(&ros_image.data[0]);
00343     uchar* cv_data_ptr = image.data;
00344     for (int i = 0; i < image.rows; ++i)
00345     {
00346       memcpy(ros_data_ptr, cv_data_ptr, ros_image.step);
00347       ros_data_ptr += ros_image.step;
00348       cv_data_ptr += image.step;
00349     }
00350   }
00351 }
00352 
00353 // Deep copy data, returnee is mutable
00354 CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr& source,
00355                     const std::string& encoding)
00356 {
00357   return toCvCopy(*source, encoding);
00358 }
00359 
00360 CvImagePtr toCvCopy(const sensor_msgs::Image& source,
00361                     const std::string& encoding)
00362 {
00363   // Construct matrix pointing to source data
00364   return toCvCopyImpl(matFromImage(source), source.header, source.encoding, encoding);
00365 }
00366 
00367 // Share const data, returnee is immutable
00368 CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr& source,
00369                           const std::string& encoding)
00370 {
00371   return toCvShare(*source, source, encoding);
00372 }
00373 
00374 CvImageConstPtr toCvShare(const sensor_msgs::Image& source,
00375                           const boost::shared_ptr<void const>& tracked_object,
00376                           const std::string& encoding)
00377 {
00378   if (!encoding.empty() && source.encoding != encoding)
00379     return toCvCopy(source, encoding);
00380 
00381   CvImagePtr ptr = boost::make_shared<CvImage>();
00382   ptr->header = source.header;
00383   ptr->encoding = source.encoding;
00384   ptr->tracked_object_ = tracked_object;
00385   ptr->image = matFromImage(source);
00386   return ptr;
00387 }
00388 
00389 CvImagePtr cvtColor(const CvImageConstPtr& source,
00390                     const std::string& encoding)
00391 {
00392   return toCvCopyImpl(source->image, source->header, source->encoding, encoding);
00393 }
00394 
00395 } //namespace cv_bridge


cv_bridge
Author(s): Patrick Mihelich, James Bowman
autogenerated on Mon Oct 6 2014 08:47:23