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 #include <map>
00036
00037 #include <cv_bridge/cv_bridge.h>
00038 #include <opencv2/imgproc/imgproc.hpp>
00039 #include <sensor_msgs/image_encodings.h>
00040 #include <boost/make_shared.hpp>
00041
00042 namespace enc = sensor_msgs::image_encodings;
00043
00044 namespace cv_bridge {
00045
00046 int getCvType(const std::string& encoding)
00047 {
00048
00049 if (encoding == enc::BGR8) return CV_8UC3;
00050 if (encoding == enc::MONO8) return CV_8UC1;
00051 if (encoding == enc::RGB8) return CV_8UC3;
00052 if (encoding == enc::MONO16) return CV_16UC1;
00053 if (encoding == enc::BGR16) return CV_16UC3;
00054 if (encoding == enc::RGB16) return CV_16UC3;
00055 if (encoding == enc::BGRA8) return CV_8UC4;
00056 if (encoding == enc::RGBA8) return CV_8UC4;
00057 if (encoding == enc::BGRA16) return CV_16UC4;
00058 if (encoding == enc::RGBA16) return CV_16UC4;
00059
00060
00061 if (encoding == enc::BAYER_RGGB8) return CV_8UC1;
00062 if (encoding == enc::BAYER_BGGR8) return CV_8UC1;
00063 if (encoding == enc::BAYER_GBRG8) return CV_8UC1;
00064 if (encoding == enc::BAYER_GRBG8) return CV_8UC1;
00065 if (encoding == enc::BAYER_RGGB16) return CV_16UC1;
00066 if (encoding == enc::BAYER_BGGR16) return CV_16UC1;
00067 if (encoding == enc::BAYER_GBRG16) return CV_16UC1;
00068 if (encoding == enc::BAYER_GRBG16) return CV_16UC1;
00069
00070
00071 if (encoding == enc::YUV422) return CV_8UC2;
00072
00073
00074 #define CHECK_ENCODING(code) \
00075 if (encoding == enc::TYPE_##code) return CV_##code \
00076
00077 #define CHECK_CHANNEL_TYPE(t) \
00078 CHECK_ENCODING(t##1); \
00079 CHECK_ENCODING(t##2); \
00080 CHECK_ENCODING(t##3); \
00081 CHECK_ENCODING(t##4); \
00082
00083
00084 CHECK_CHANNEL_TYPE(8UC);
00085 CHECK_CHANNEL_TYPE(8SC);
00086 CHECK_CHANNEL_TYPE(16UC);
00087 CHECK_CHANNEL_TYPE(16SC);
00088 CHECK_CHANNEL_TYPE(32SC);
00089 CHECK_CHANNEL_TYPE(32FC);
00090 CHECK_CHANNEL_TYPE(64FC);
00091
00092 #undef CHECK_CHANNEL_TYPE
00093 #undef CHECK_ENCODING
00094
00095 throw Exception("Unrecognized image encoding [" + encoding + "]");
00096 }
00097
00099
00100 enum Format { INVALID = -1, GRAY = 0, RGB, BGR, RGBA, BGRA, YUV422 };
00101
00102 Format getFormat(const std::string& encoding)
00103 {
00104 if (encoding == enc::BGR8) return BGR;
00105 if (encoding == enc::MONO8) return GRAY;
00106 if (encoding == enc::RGB8) return RGB;
00107 if (encoding == enc::MONO16) return GRAY;
00108 if (encoding == enc::BGR16) return BGR;
00109 if (encoding == enc::RGB16) return RGB;
00110 if (encoding == enc::BGRA8) return BGRA;
00111 if (encoding == enc::RGBA8) return RGBA;
00112 if (encoding == enc::BGRA16) return BGRA;
00113 if (encoding == enc::RGBA16) return RGBA;
00114 if (encoding == enc::YUV422) return YUV422;
00115
00116 if (encoding == enc::TYPE_8UC1) return GRAY;
00117 if (encoding == enc::TYPE_8SC1) return GRAY;
00118 if (encoding == enc::TYPE_16UC1) return GRAY;
00119 if (encoding == enc::TYPE_16SC1) return GRAY;
00120 if (encoding == enc::TYPE_32SC1) return GRAY;
00121 if (encoding == enc::TYPE_32FC1) return GRAY;
00122 if (encoding == enc::TYPE_64FC1) return GRAY;
00123
00124 if (encoding == enc::TYPE_8UC3) return RGB;
00125 if (encoding == enc::TYPE_8SC3) return RGB;
00126 if (encoding == enc::TYPE_16UC3) return RGB;
00127 if (encoding == enc::TYPE_16SC3) return RGB;
00128 if (encoding == enc::TYPE_32SC3) return RGB;
00129 if (encoding == enc::TYPE_32FC3) return RGB;
00130 if (encoding == enc::TYPE_64FC3) return RGB;
00131
00132 if (encoding == enc::TYPE_8UC4) return RGBA;
00133 if (encoding == enc::TYPE_8SC4) return RGBA;
00134 if (encoding == enc::TYPE_16UC4) return RGBA;
00135 if (encoding == enc::TYPE_16SC4) return RGBA;
00136 if (encoding == enc::TYPE_32SC4) return RGBA;
00137 if (encoding == enc::TYPE_32FC4) return RGBA;
00138 if (encoding == enc::TYPE_64FC4) return RGBA;
00139
00140
00141 return INVALID;
00142 }
00143
00144 static const int SAME_FORMAT = -1;
00145
00150 std::map<std::pair<Format, Format>, std::vector<int> > getConversionCodes() {
00151 std::map<std::pair<Format, Format>, std::vector<int> > res;
00152 for(int i=0; i<=5; ++i)
00153 res[std::pair<Format, Format>(Format(i),Format(i))].push_back(SAME_FORMAT);
00154 res[std::make_pair(GRAY, RGB)].push_back(CV_GRAY2RGB);
00155 res[std::make_pair(GRAY, BGR)].push_back(CV_GRAY2BGR);
00156 res[std::make_pair(GRAY, RGBA)].push_back(CV_GRAY2RGBA);
00157 res[std::make_pair(GRAY, BGRA)].push_back(CV_GRAY2BGRA);
00158
00159 res[std::make_pair(RGB, GRAY)].push_back(CV_RGB2GRAY);
00160 res[std::make_pair(RGB, BGR)].push_back(CV_RGB2BGR);
00161 res[std::make_pair(RGB, RGBA)].push_back(CV_RGB2RGBA);
00162 res[std::make_pair(RGB, BGRA)].push_back(CV_RGB2BGRA);
00163
00164 res[std::make_pair(BGR, GRAY)].push_back(CV_BGR2GRAY);
00165 res[std::make_pair(BGR, RGB)].push_back(CV_BGR2RGB);
00166 res[std::make_pair(BGR, RGBA)].push_back(CV_BGR2RGBA);
00167 res[std::make_pair(BGR, BGRA)].push_back(CV_BGR2BGRA);
00168
00169 res[std::make_pair(RGBA, GRAY)].push_back(CV_RGBA2GRAY);
00170 res[std::make_pair(RGBA, RGB)].push_back(CV_RGBA2RGB);
00171 res[std::make_pair(RGBA, BGR)].push_back(CV_RGBA2BGR);
00172 res[std::make_pair(RGBA, BGRA)].push_back(CV_RGBA2BGRA);
00173
00174 res[std::make_pair(BGRA, GRAY)].push_back(CV_BGRA2GRAY);
00175 res[std::make_pair(BGRA, RGB)].push_back(CV_BGRA2RGB);
00176 res[std::make_pair(BGRA, BGR)].push_back(CV_BGRA2BGR);
00177 res[std::make_pair(BGRA, RGBA)].push_back(CV_BGRA2RGBA);
00178
00179 res[std::make_pair(YUV422, GRAY)].push_back(CV_YUV2GRAY_UYVY);
00180 res[std::make_pair(YUV422, RGB)].push_back(CV_YUV2RGB_UYVY);
00181 res[std::make_pair(YUV422, BGR)].push_back(CV_YUV2BGR_UYVY);
00182 res[std::make_pair(YUV422, RGBA)].push_back(CV_YUV2RGBA_UYVY);
00183 res[std::make_pair(YUV422, BGRA)].push_back(CV_YUV2BGRA_UYVY);
00184
00185 return res;
00186 }
00187
00188 const std::vector<int> getConversionCode(std::string src_encoding, std::string dst_encoding)
00189 {
00190 Format src_format = getFormat(src_encoding);
00191 Format dst_format = getFormat(dst_encoding);
00192
00193 static const std::map<std::pair<Format, Format>, std::vector<int> > CONVERSION_CODES = getConversionCodes();
00194
00195 std::pair<Format, Format> key(src_format, dst_format);
00196 std::map<std::pair<Format, Format>, std::vector<int> >::const_iterator val = CONVERSION_CODES.find(key);
00197 if (val == CONVERSION_CODES.end())
00198 throw Exception("Unsupported conversion from [" + src_encoding +
00199 "] to [" + dst_encoding + "]");
00200 return val->second;
00201 }
00202
00203
00204 CvImagePtr toCvCopyImpl(const cv::Mat& source,
00205 const std_msgs::Header& src_header,
00206 const std::string& src_encoding,
00207 const std::string& dst_encoding)
00208 {
00210
00211
00212 CvImagePtr ptr = boost::make_shared<CvImage>();
00213 ptr->header = src_header;
00214
00215
00216 if (dst_encoding.empty() || dst_encoding == src_encoding)
00217 {
00218 ptr->encoding = src_encoding;
00219 source.copyTo(ptr->image);
00220 }
00221 else
00222 {
00223
00224 const std::vector<int> conversion_codes = getConversionCode(src_encoding, dst_encoding);
00225 cv::Mat image1 = source;
00226 cv::Mat image2;
00227 for(size_t i=0; i<conversion_codes.size(); ++i) {
00228 int conversion_code = conversion_codes[i];
00229 if (conversion_code == SAME_FORMAT)
00230 {
00231
00232 double alpha = 1.0;
00233 int src_depth = enc::bitDepth(src_encoding);
00234 int dst_depth = enc::bitDepth(dst_encoding);
00235
00236 if (src_depth == 8 && dst_depth == 16)
00237 alpha = 65535. / 255.;
00238 else if (src_depth == 16 && dst_depth == 8)
00239 alpha = 255. / 65535.;
00240 image1.convertTo(image2, getCvType(dst_encoding), alpha);
00241 }
00242 else
00243 {
00244
00245 cv::cvtColor(image1, image2, conversion_code);
00246 }
00247 image1 = image2;
00248 }
00249 ptr->image = image2;
00250 ptr->encoding = dst_encoding;
00251 }
00252
00253 return ptr;
00254 }
00255
00257
00258 sensor_msgs::ImagePtr CvImage::toImageMsg() const
00259 {
00260 sensor_msgs::ImagePtr ptr = boost::make_shared<sensor_msgs::Image>();
00261 toImageMsg(*ptr);
00262 return ptr;
00263 }
00264
00265 void CvImage::toImageMsg(sensor_msgs::Image& ros_image) const
00266 {
00267 ros_image.header = header;
00268 ros_image.height = image.rows;
00269 ros_image.width = image.cols;
00270 ros_image.encoding = encoding;
00271 ros_image.is_bigendian = false;
00272 ros_image.step = image.cols * image.elemSize();
00273 size_t size = ros_image.step * image.rows;
00274 ros_image.data.resize(size);
00275
00276 if (image.isContinuous())
00277 {
00278 memcpy((char*)(&ros_image.data[0]), image.data, size);
00279 }
00280 else
00281 {
00282
00283 uchar* ros_data_ptr = (uchar*)(&ros_image.data[0]);
00284 uchar* cv_data_ptr = image.data;
00285 for (int i = 0; i < image.rows; ++i)
00286 {
00287 memcpy(ros_data_ptr, cv_data_ptr, ros_image.step);
00288 ros_data_ptr += ros_image.step;
00289 cv_data_ptr += image.step;
00290 }
00291 }
00292 }
00293
00294
00295 CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr& source,
00296 const std::string& encoding)
00297 {
00298 return toCvCopy(*source, encoding);
00299 }
00300
00301 CvImagePtr toCvCopy(const sensor_msgs::Image& source,
00302 const std::string& encoding)
00303 {
00304
00305 int source_type = getCvType(source.encoding);
00306 const cv::Mat tmp((int)source.height, (int)source.width, source_type,
00307 const_cast<uint8_t*>(&source.data[0]), (size_t)source.step);
00308
00309 return toCvCopyImpl(tmp, source.header, source.encoding, encoding);
00310 }
00311
00312
00313 CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr& source,
00314 const std::string& encoding)
00315 {
00316 return toCvShare(*source, source, encoding);
00317 }
00318
00319 CvImageConstPtr toCvShare(const sensor_msgs::Image& source,
00320 const boost::shared_ptr<void const>& tracked_object,
00321 const std::string& encoding)
00322 {
00323 if (!encoding.empty() && source.encoding != encoding)
00324 return toCvCopy(source, encoding);
00325
00326 CvImagePtr ptr = boost::make_shared<CvImage>();
00327 ptr->header = source.header;
00328 ptr->encoding = source.encoding;
00329 ptr->tracked_object_ = tracked_object;
00330 int type = getCvType(source.encoding);
00331 ptr->image = cv::Mat(source.height, source.width, type,
00332 const_cast<uchar*>(&source.data[0]), source.step);
00333 return ptr;
00334 }
00335
00336 CvImagePtr cvtColor(const CvImageConstPtr& source,
00337 const std::string& encoding)
00338 {
00339 return toCvCopyImpl(source->image, source->header, source->encoding, encoding);
00340 }
00341
00342 }