// Internal, used by toCvCopy and cvtColor CvImagePtr toCvCopyImpl(const cv::Mat& source, const std_msgs::Header& src_header, const std::string& src_encoding, const std::string& dst_encoding) { /// @todo Handle endianness - e.g. 16-bit dc1394 camera images are big-endian // Copy metadata CvImagePtr ptr = boost::make_shared<CvImage>(); ptr->header = src_header; // Copy to new buffer if same encoding requested if (dst_encoding.empty() || dst_encoding == src_encoding) { ptr->encoding = src_encoding; source.copyTo(ptr->image); } else { // Convert the source data to the desired encoding const std::vector<int> conversion_codes = getConversionCode(src_encoding, dst_encoding); cv::Mat image1 = source; cv::Mat image2; for(size_t i=0; i<conversion_codes.size(); ++i) { int conversion_code = conversion_codes[i]; if (conversion_code == SAME_FORMAT) { // Same number of channels, but different bit depth double alpha = 1.0; int src_depth = enc::bitDepth(src_encoding); int dst_depth = enc::bitDepth(dst_encoding); // Do scaling between CV_8U [0,255] and CV_16U [0,65535] images. if (src_depth == 8 && dst_depth == 16) image1.convertTo(image2, getCvType(dst_encoding), 65535. / 255.); else if (src_depth == 16 && dst_depth == 8) image1.convertTo(image2, getCvType(dst_encoding), 255. / 65535.); else image1.convertTo(image2, getCvType(dst_encoding)); } else { // Perform color conversion cv::cvtColor(image1, image2, conversion_code); } image1 = image2; } ptr->image = image2; ptr->encoding = dst_encoding; } return ptr; }
CvImagePtr toCvCopy(const sensor_msgs::Image& source, const std::string& encoding) { // Construct matrix pointing to source data int source_type = getCvType(source.encoding); const cv::Mat tmp((int)source.height, (int)source.width, source_type, const_cast<uint8_t*>(&source.data[0]), (size_t)source.step); return toCvCopyImpl(tmp, source.header, source.encoding, encoding); }
// Converts a ROS Image to a cv::Mat by sharing the data or chaning its endianness if needed cv::Mat matFromImage(const sensor_msgs::Image& source) { int source_type = getCvType(source.encoding); int byte_depth = enc::bitDepth(source.encoding) / 8; int num_channels = enc::numChannels(source.encoding); if (source.step < source.width * byte_depth * num_channels) { std::stringstream ss; ss << "Image is wrongly formed: step < width * byte_depth * num_channels or " << source.step << " != " << source.width << " * " << byte_depth << " * " << num_channels; throw Exception(ss.str()); } if (source.height * source.step != source.data.size()) { std::stringstream ss; ss << "Image is wrongly formed: height * step != size or " << source.height << " * " << source.step << " != " << source.data.size(); throw Exception(ss.str()); } // If the endianness is the same as locally, share the data cv::Mat mat(source.height, source.width, source_type, const_cast<uchar*>(&source.data[0]), source.step); if ((boost::endian::order::native == boost::endian::order::big && source.is_bigendian) || (boost::endian::order::native == boost::endian::order::little && !source.is_bigendian) || byte_depth == 1) return mat; // Otherwise, reinterpret the data as bytes and switch the channels accordingly mat = cv::Mat(source.height, source.width, CV_MAKETYPE(CV_8U, num_channels*byte_depth), const_cast<uchar*>(&source.data[0]), source.step); cv::Mat mat_swap(source.height, source.width, mat.type()); std::vector<int> fromTo; fromTo.reserve(num_channels*byte_depth); for(int i = 0; i < num_channels; ++i) for(int j = 0; j < byte_depth; ++j) { fromTo.push_back(byte_depth*i + j); fromTo.push_back(byte_depth*i + byte_depth - 1 - j); } cv::mixChannels(std::vector<cv::Mat>(1, mat), std::vector<cv::Mat>(1, mat_swap), fromTo); // Interpret mat_swap back as the proper type mat_swap = cv::Mat(source.height, source.width, source_type, mat_swap.data, mat_swap.step); return mat_swap; }
CvImageConstPtr toCvShare(const sensor_msgs::Image& source, const boost::shared_ptr<void const>& tracked_object, const std::string& encoding) { if (!encoding.empty() && source.encoding != encoding) return toCvCopy(source, encoding); CvImagePtr ptr = boost::make_shared<CvImage>(); ptr->header = source.header; ptr->encoding = source.encoding; ptr->tracked_object_ = tracked_object; int type = getCvType(source.encoding); ptr->image = cv::Mat(source.height, source.width, type, const_cast<uchar*>(&source.data[0]), source.step); return ptr; }