cv_bridge: CvBridge.h Source File

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009, 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 #ifndef CVBRIDGE_HH
00036 #define CVBRIDGE_HH
00037 
00038 #include <stdexcept>
00039 #include "sensor_msgs/Image.h"
00040 #include "opencv/cxcore.h"
00041 #include "opencv/cv.h"
00042 
00043 namespace sensor_msgs
00044 {
00045   class CvBridgeException: public std::runtime_error
00046   {
00047   public:
00048     CvBridgeException(const std::string errorDescription) : std::runtime_error(errorDescription) { ; };
00049   };
00050 
00054   class CvBridge
00055   {
00056   public:
00057 
00058     // @cond DOXYGEN_IGNORE
00059 
00060     IplImage* img_;
00061     IplImage* rosimg_;
00062     IplImage* cvtimg_;
00063 
00064     CvBridge() : img_(0), rosimg_(0), cvtimg_(0)
00065     {
00066       rosimg_ = cvCreateImageHeader( cvSize(0,0), IPL_DEPTH_8U, 1 );
00067     }
00068 
00069     ~CvBridge()
00070     {
00071       if (rosimg_) {
00072         cvReleaseImageHeader(&rosimg_);
00073         rosimg_ = 0;
00074       }
00075 
00076       if (cvtimg_) {
00077         cvReleaseImage(&cvtimg_);
00078         cvtimg_ = 0;
00079       }
00080     }
00081 
00082     bool reallocIfNeeded_(IplImage** img, CvSize sz, int depth, int channels)
00083     {
00084       if ((*img) != 0)
00085         if ((*img)->width     != sz.width  ||
00086             (*img)->height    != sz.height ||
00087             (*img)->depth     != depth     ||
00088             (*img)->nChannels != channels)
00089         {
00090           cvReleaseImage(img);
00091           *img = 0;
00092         }
00093 
00094       if (*img == 0)
00095       {
00096         *img = cvCreateImage(sz, depth, channels);
00097         return true;
00098       }
00099       return false;
00100     }
00101 
00102     bool reallocIfNeeded(IplImage** img, int depth = -1, int channels = -1)
00103     {
00104       if (depth == -1)
00105         depth = img_->depth;
00106       if (channels == -1)
00107         channels = img_->nChannels;
00108       return reallocIfNeeded_(img, cvGetSize(img_), depth, channels);
00109     }
00110 
00111     int encoding_as_cvtype(std::string encoding)
00112     {
00113       if (encoding == "8UC1") return CV_8UC1;
00114       if (encoding == "8UC2") return CV_8UC2;
00115       if (encoding == "8UC3") return CV_8UC3;
00116       if (encoding == "8UC4") return CV_8UC4;
00117       if (encoding == "8SC1") return CV_8SC1;
00118       if (encoding == "8SC2") return CV_8SC2;
00119       if (encoding == "8SC3") return CV_8SC3;
00120       if (encoding == "8SC4") return CV_8SC4;
00121       if (encoding == "16UC1") return CV_16UC1;
00122       if (encoding == "16UC2") return CV_16UC2;
00123       if (encoding == "16UC3") return CV_16UC3;
00124       if (encoding == "16UC4") return CV_16UC4;
00125       if (encoding == "16SC1") return CV_16SC1;
00126       if (encoding == "16SC2") return CV_16SC2;
00127       if (encoding == "16SC3") return CV_16SC3;
00128       if (encoding == "16SC4") return CV_16SC4;
00129       if (encoding == "32SC1") return CV_32SC1;
00130       if (encoding == "32SC2") return CV_32SC2;
00131       if (encoding == "32SC3") return CV_32SC3;
00132       if (encoding == "32SC4") return CV_32SC4;
00133       if (encoding == "32FC1") return CV_32FC1;
00134       if (encoding == "32FC2") return CV_32FC2;
00135       if (encoding == "32FC3") return CV_32FC3;
00136       if (encoding == "32FC4") return CV_32FC4;
00137       if (encoding == "64FC1") return CV_64FC1;
00138       if (encoding == "64FC2") return CV_64FC2;
00139       if (encoding == "64FC3") return CV_64FC3;
00140       if (encoding == "64FC4") return CV_64FC4;
00141       if (encoding == "rgb8") return CV_8UC3;
00142       if (encoding == "bgr8") return CV_8UC3;
00143       if (encoding == "rgba8") return CV_8UC4;
00144       if (encoding == "bgra8") return CV_8UC4;
00145       if (encoding == "mono8") return CV_8UC1;
00146       if (encoding == "mono16") return CV_16UC1;
00147       return -1;
00148     }
00149 
00150     std::string encoding_as_fmt(std::string encoding)
00151     {
00152       std::string fmt;
00153       int source_channels = CV_MAT_CN(encoding_as_cvtype(encoding));
00154       if (source_channels == -1)
00155         fmt == "";
00156       else if (source_channels == 1)
00157         fmt = "GRAY";
00158       else if ("rgb8" == encoding)
00159         fmt = "RGB";
00160       else if ("rgba8" == encoding)
00161         fmt = "RGBA";
00162       else if (source_channels == 3)
00163         fmt = "BGR";
00164       else if (source_channels == 4)
00165         fmt = "BGRA";
00166       return fmt;
00167     }
00168     // @endcond
00169 
00170 
00177     inline IplImage* toIpl()
00178     {
00179       return img_;
00180     }
00181 
00190     bool fromImage(const Image& rosimg, std::string desired_encoding = "passthrough")
00191     {
00192       CvMat cvmHeader;
00193       
00194       // cvSetData(rosimg_, const_cast<uint8_t*>(&(rosimg.data[0])), rosimg.step);
00195 
00196       int source_type = encoding_as_cvtype(rosimg.encoding);
00197 
00198       cvInitMatHeader(&cvmHeader, rosimg.height, rosimg.width, source_type, const_cast<uint8_t*>(&(rosimg.data[0])), rosimg.step);
00199       cvGetImage(&cvmHeader, rosimg_);
00200 
00201       // Check that the message encoding is legal
00202       if (encoding_as_cvtype(rosimg.encoding) == -1)
00203         return false;
00204 
00205       if (desired_encoding == "passthrough") {
00206         img_ = rosimg_;
00207       } else {
00208         // Might need to do a conversion.  sourcefmt and destfmt can be
00209         // one of GRAY, RGB, BGR, RGBA, BGRA.
00210         std::string sourcefmt = encoding_as_fmt(rosimg.encoding);
00211         std::string destfmt = encoding_as_fmt(desired_encoding);
00212         int destination_type = encoding_as_cvtype(desired_encoding);
00213 
00214         if ((sourcefmt == destfmt) && (source_type == destination_type)) {
00215           img_ = rosimg_;
00216         } else {
00217           img_ = rosimg_;  // realloc uses this as a hidden argument.
00218           if (desired_encoding != "mono16")
00219             reallocIfNeeded(&cvtimg_, IPL_DEPTH_8U, CV_MAT_CN(destination_type));
00220           else
00221             reallocIfNeeded(&cvtimg_, IPL_DEPTH_16U, CV_MAT_CN(destination_type));
00222           void *sourceimg;
00223           CvMat *same_depth = NULL;
00224           if ((source_type & CV_MAT_DEPTH_MASK) != (destination_type & CV_MAT_DEPTH_MASK)) {
00225             // need to convert depth, but not number of channels, which happens below
00226             same_depth = cvCreateMat(rosimg.height,
00227                 rosimg.width,
00228                 CV_MAKETYPE((destination_type & CV_MAT_TYPE_MASK), CV_MAT_CN(source_type)));
00229             cvConvertScale(rosimg_, same_depth);
00230             sourceimg = same_depth;
00231           } else {
00232             sourceimg = rosimg_;
00233           }
00234           if (sourcefmt == destfmt) {
00235             cvConvertScale(sourceimg, cvtimg_);
00236           } else {
00237             if (sourcefmt == "") {
00238               return false;
00239             }
00240             if (sourcefmt == "GRAY") {
00241               if (destfmt == "RGB")
00242                 cvCvtColor(sourceimg, cvtimg_, CV_GRAY2RGB);
00243               if (destfmt == "BGR")
00244                 cvCvtColor(sourceimg, cvtimg_, CV_GRAY2BGR);
00245               if (destfmt == "RGBA")
00246                 cvCvtColor(sourceimg, cvtimg_, CV_GRAY2RGBA);
00247               if (destfmt == "BGRA")
00248                 cvCvtColor(sourceimg, cvtimg_, CV_GRAY2BGRA);
00249             }
00250             if (sourcefmt == "RGB") {
00251               if (destfmt == "GRAY")
00252                 cvCvtColor(sourceimg, cvtimg_, CV_RGB2GRAY);
00253               if (destfmt == "BGR")
00254                 cvCvtColor(sourceimg, cvtimg_, CV_RGB2BGR);
00255               if (destfmt == "RGBA")
00256                 cvCvtColor(sourceimg, cvtimg_, CV_RGB2RGBA);
00257               if (destfmt == "BGRA")
00258                 cvCvtColor(sourceimg, cvtimg_, CV_RGB2BGRA);
00259             }
00260             if (sourcefmt == "BGR") {
00261               if (destfmt == "GRAY")
00262                 cvCvtColor(sourceimg, cvtimg_, CV_BGR2GRAY);
00263               if (destfmt == "RGB")
00264                 cvCvtColor(sourceimg, cvtimg_, CV_BGR2RGB);
00265               if (destfmt == "RGBA")
00266                 cvCvtColor(sourceimg, cvtimg_, CV_BGR2RGBA);
00267               if (destfmt == "BGRA")
00268                 cvCvtColor(sourceimg, cvtimg_, CV_BGR2BGRA);
00269             }
00270             if (sourcefmt == "RGBA") {
00271               if (destfmt == "GRAY")
00272                 cvCvtColor(sourceimg, cvtimg_, CV_RGBA2GRAY);
00273               if (destfmt == "RGB")
00274                 cvCvtColor(sourceimg, cvtimg_, CV_RGBA2RGB);
00275               if (destfmt == "BGR")
00276                 cvCvtColor(sourceimg, cvtimg_, CV_RGBA2BGR);
00277               if (destfmt == "BGRA")
00278                 cvCvtColor(sourceimg, cvtimg_, CV_RGBA2BGRA);
00279             }
00280             if (sourcefmt == "BGRA") {
00281               if (destfmt == "GRAY")
00282                 cvCvtColor(sourceimg, cvtimg_, CV_BGRA2GRAY);
00283               if (destfmt == "RGB")
00284                 cvCvtColor(sourceimg, cvtimg_, CV_BGRA2RGB);
00285               if (destfmt == "BGR")
00286                 cvCvtColor(sourceimg, cvtimg_, CV_BGRA2BGR);
00287               if (destfmt == "RGBA")
00288                 cvCvtColor(sourceimg, cvtimg_, CV_BGRA2RGBA);
00289             }
00290           }
00291           if (same_depth != NULL) {
00292               cvReleaseMat(&same_depth);
00293           }
00294           img_ = cvtimg_;
00295         }
00296       }
00297       return true;
00298     }
00299 
00309     static bool fromIpltoRosImage(const IplImage* source, sensor_msgs::Image& dest, std::string encoding = "passthrough")
00310     {
00311       CvMat header, *cvm;
00312 
00313       cvm = cvGetMat(source, &header);
00314       // dest.type = cvm->type & (CV_MAT_TYPE_MASK | CV_MAT_DEPTH_MASK);
00315       dest.encoding = encoding;
00316 
00317       if (encoding == "passthrough") {
00318         switch (cvm->type & (CV_MAT_TYPE_MASK | CV_MAT_DEPTH_MASK)) {
00319         case CV_8UC1: dest.encoding = "8UC1"; break;
00320         case CV_8UC2: dest.encoding = "8UC2"; break;
00321         case CV_8UC3: dest.encoding = "8UC3"; break;
00322         case CV_8UC4: dest.encoding = "8UC4"; break;
00323         case CV_8SC1: dest.encoding = "8SC1"; break;
00324         case CV_8SC2: dest.encoding = "8SC2"; break;
00325         case CV_8SC3: dest.encoding = "8SC3"; break;
00326         case CV_8SC4: dest.encoding = "8SC4"; break;
00327         case CV_16UC1: dest.encoding = "16UC1"; break;
00328         case CV_16UC2: dest.encoding = "16UC2"; break;
00329         case CV_16UC3: dest.encoding = "16UC3"; break;
00330         case CV_16UC4: dest.encoding = "16UC4"; break;
00331         case CV_16SC1: dest.encoding = "16SC1"; break;
00332         case CV_16SC2: dest.encoding = "16SC2"; break;
00333         case CV_16SC3: dest.encoding = "16SC3"; break;
00334         case CV_16SC4: dest.encoding = "16SC4"; break;
00335         case CV_32SC1: dest.encoding = "32SC1"; break;
00336         case CV_32SC2: dest.encoding = "32SC2"; break;
00337         case CV_32SC3: dest.encoding = "32SC3"; break;
00338         case CV_32SC4: dest.encoding = "32SC4"; break;
00339         case CV_32FC1: dest.encoding = "32FC1"; break;
00340         case CV_32FC2: dest.encoding = "32FC2"; break;
00341         case CV_32FC3: dest.encoding = "32FC3"; break;
00342         case CV_32FC4: dest.encoding = "32FC4"; break;
00343         case CV_64FC1: dest.encoding = "64FC1"; break;
00344         case CV_64FC2: dest.encoding = "64FC2"; break;
00345         case CV_64FC3: dest.encoding = "64FC3"; break;
00346         case CV_64FC4: dest.encoding = "64FC4"; break;
00347         default: assert(0);
00348         }
00349       } else {
00350         int ct = cvm->type & (CV_MAT_TYPE_MASK | CV_MAT_DEPTH_MASK);
00351         if (encoding == "rgb8") {
00352           if (ct != CV_8UC3)
00353             return false;
00354         } else if (encoding == "rgba8") {
00355           if (ct != CV_8UC4)
00356             return false;
00357         } else if (encoding == "bgr8") {
00358           if (ct != CV_8UC3)
00359             return false;
00360         } else if (encoding == "bgra8") {
00361           if (ct != CV_8UC4)
00362             return false;
00363         } else if (encoding == "mono8") {
00364           if (ct != CV_8UC1)
00365             return false;
00366         } else if (encoding == "mono16") {
00367           if (ct != CV_16UC1)
00368             return false;
00369         } else {
00370           return false;
00371         }
00372 
00373         dest.encoding = encoding;
00374       }
00375 
00376       dest.width = cvm->width;
00377       dest.height = cvm->height;
00378       dest.step = cvm->step;
00379       dest.data.resize(cvm->step * cvm->height);
00380       memcpy((char*)(&dest.data[0]), source->imageData, cvm->step * cvm->height);
00381       return true;
00382     }
00383 
00407     static sensor_msgs::Image::Ptr cvToImgMsg(const IplImage* source, std::string encoding = "passthrough")
00408     {
00409       sensor_msgs::Image::Ptr rosimg(new sensor_msgs::Image);
00410       if (!fromIpltoRosImage(source, *rosimg, encoding))
00411         throw CvBridgeException("Conversion to OpenCV image failed");
00412       return rosimg;
00413     }
00414 
00442     IplImage* imgMsgToCv(sensor_msgs::Image::ConstPtr rosimg, std::string desired_encoding = "passthrough")
00443     {
00444       if (!fromImage(*rosimg, desired_encoding))
00445         throw CvBridgeException("Conversion to OpenCV image failed");
00446       return toIpl();
00447     }
00448   };
00449 }
00450 
00451 #endif