// 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;
}
