-
Notifications
You must be signed in to change notification settings - Fork 597
/
cv_bridge.h
412 lines (356 loc) · 14.7 KB
/
cv_bridge.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc,
* Copyright (c) 2015, Tal Regev.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef CV_BRIDGE_CV_BRIDGE_H
#define CV_BRIDGE_CV_BRIDGE_H
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CompressedImage.h>
#include <sensor_msgs/image_encodings.h>
#include <ros/static_assert.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/types_c.h>
#include <stdexcept>
namespace cv_bridge {
class Exception : public std::runtime_error
{
public:
Exception(const std::string& description) : std::runtime_error(description) {}
};
class CvImage;
typedef boost::shared_ptr<CvImage> CvImagePtr;
typedef boost::shared_ptr<CvImage const> CvImageConstPtr;
//from: http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags)
typedef enum {
BMP, DIP,
JPG, JPEG, JPE,
JP2,
PNG,
PBM, PGM, PPM,
SR, RAS,
TIFF, TIF,
} Format;
/**
* \brief Image message class that is interoperable with sensor_msgs/Image but uses a
* more convenient cv::Mat representation for the image data.
*/
class CvImage
{
public:
std_msgs::Header header; //!< ROS header
std::string encoding; //!< Image encoding ("mono8", "bgr8", etc.)
cv::Mat image; //!< Image data for use with OpenCV
/**
* \brief Empty constructor.
*/
CvImage() {}
/**
* \brief Constructor.
*/
CvImage(const std_msgs::Header& header, const std::string& encoding,
const cv::Mat& image = cv::Mat())
: header(header), encoding(encoding), image(image)
{
}
/**
* \brief Convert this message to a ROS sensor_msgs::Image message.
*
* The returned sensor_msgs::Image message contains a copy of the image data.
*/
sensor_msgs::ImagePtr toImageMsg() const;
/**
* dst_format is compress the image to desire format.
* Default value is empty string that will convert to jpg format.
* can be: jpg, jp2, bmp, png, tif at the moment
* support this format from opencv:
* http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags)
*/
sensor_msgs::CompressedImagePtr toCompressedImageMsg(const Format dst_format = JPG) const;
/**
* \brief Copy the message data to a ROS sensor_msgs::Image message.
*
* This overload is intended mainly for aggregate messages such as stereo_msgs::DisparityImage,
* which contains a sensor_msgs::Image as a data member.
*/
void toImageMsg(sensor_msgs::Image& ros_image) const;
/**
* dst_format is compress the image to desire format.
* Default value is empty string that will convert to jpg format.
* can be: jpg, jp2, bmp, png, tif at the moment
* support this format from opencv:
* http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags)
*/
void toCompressedImageMsg(sensor_msgs::CompressedImage& ros_image, const Format dst_format = JPG) const;
typedef boost::shared_ptr<CvImage> Ptr;
typedef boost::shared_ptr<CvImage const> ConstPtr;
protected:
boost::shared_ptr<void const> tracked_object_; // for sharing ownership
/// @cond DOXYGEN_IGNORE
friend
CvImageConstPtr toCvShare(const sensor_msgs::Image& source,
const boost::shared_ptr<void const>& tracked_object,
const std::string& encoding);
/// @endcond
};
/**
* \brief Convert a sensor_msgs::Image message to an OpenCV-compatible CvImage, copying the
* image data.
*
* \param source A shared_ptr to a sensor_msgs::Image message
* \param encoding The desired encoding of the image data, one of the following strings:
* - \c "mono8"
* - \c "bgr8"
* - \c "bgra8"
* - \c "rgb8"
* - \c "rgba8"
* - \c "mono16"
*
* If \a encoding is the empty string (the default), the returned CvImage has the same encoding
* as \a source.
*/
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr& source,
const std::string& encoding = std::string());
CvImagePtr toCvCopy(const sensor_msgs::CompressedImageConstPtr& source,
const std::string& encoding = std::string());
/**
* \brief Convert a sensor_msgs::Image message to an OpenCV-compatible CvImage, copying the
* image data.
*
* \param source A sensor_msgs::Image message
* \param encoding The desired encoding of the image data, one of the following strings:
* - \c "mono8"
* - \c "bgr8"
* - \c "bgra8"
* - \c "rgb8"
* - \c "rgba8"
* - \c "mono16"
*
* If \a encoding is the empty string (the default), the returned CvImage has the same encoding
* as \a source.
* If the source is 8bit and the encoding 16 or vice-versa, a scaling is applied (65535/255 and
* 255/65535 respectively). Otherwise, no scaling is applied and the rules from the convertTo OpenCV
* function are applied (capping): http://docs.opencv.org/modules/core/doc/basic_structures.html#mat-convertto
*/
CvImagePtr toCvCopy(const sensor_msgs::Image& source,
const std::string& encoding = std::string());
CvImagePtr toCvCopy(const sensor_msgs::CompressedImage& source,
const std::string& encoding = std::string());
/**
* \brief Convert an immutable sensor_msgs::Image message to an OpenCV-compatible CvImage, sharing
* the image data if possible.
*
* If the source encoding and desired encoding are the same, the returned CvImage will share
* the image data with \a source without copying it. The returned CvImage cannot be modified, as that
* could modify the \a source data.
*
* \param source A shared_ptr to a sensor_msgs::Image message
* \param encoding The desired encoding of the image data, one of the following strings:
* - \c "mono8"
* - \c "bgr8"
* - \c "bgra8"
* - \c "rgb8"
* - \c "rgba8"
* - \c "mono16"
*
* If \a encoding is the empty string (the default), the returned CvImage has the same encoding
* as \a source.
*/
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr& source,
const std::string& encoding = std::string());
/**
* \brief Convert an immutable sensor_msgs::Image message to an OpenCV-compatible CvImage, sharing
* the image data if possible.
*
* If the source encoding and desired encoding are the same, the returned CvImage will share
* the image data with \a source without copying it. The returned CvImage cannot be modified, as that
* could modify the \a source data.
*
* This overload is useful when you have a shared_ptr to a message that contains a
* sensor_msgs::Image, and wish to share ownership with the containing message.
*
* \param source The sensor_msgs::Image message
* \param tracked_object A shared_ptr to an object owning the sensor_msgs::Image
* \param encoding The desired encoding of the image data, one of the following strings:
* - \c "mono8"
* - \c "bgr8"
* - \c "bgra8"
* - \c "rgb8"
* - \c "rgba8"
* - \c "mono16"
*
* If \a encoding is the empty string (the default), the returned CvImage has the same encoding
* as \a source.
*/
CvImageConstPtr toCvShare(const sensor_msgs::Image& source,
const boost::shared_ptr<void const>& tracked_object,
const std::string& encoding = std::string());
/**
* \brief Convert a CvImage to another encoding using the same rules as toCvCopy
*/
CvImagePtr cvtColor(const CvImageConstPtr& source,
const std::string& encoding);
/**
* \brief Converts an immutable sensor_msgs::Image message to another CvImage for display purposes,
* using practical conversion rules if needed.
*
* Data will be shared between input and output if possible.
*
* Recall: sensor_msgs::image_encodings::isColor and isMono tell whether an image contains R,G,B,A, mono
* (or any combination/subset) with 8 or 16 bit depth.
*
* The following rules apply:
* - if the output encoding is empty, the fact that the input image is mono or multiple-channel is
* preserved in the ouput image. The bit depth will be 8.
* - if the output encoding is not empty, it must have sensor_msgs::image_encodings::isColor and
* isMono return true. It must also be 8 bit in depth
* - if the input encoding is an OpenCV format (e.g. 8UC1), and if we have 1,3 or 4 channels, it is
* respectively converted to mono, BGR or BGRA.
*
* \param source A shared_ptr to a sensor_msgs::Image message
* \param encoding Either an encoding string that returns true in sensor_msgs::image_encodings::isColor
* isMono or the mpty string. If it is the empty string,
*
* \param do_dynamic_scaling If true, the image is dynamically scaled between its minimum and maximum value
* before being converted to its final encoding.
* \param min_image_value Independently from do_dynamic_scaling, if min_image_value and max_image_value are
* different, the image is scaled between these two values before being converted to its final encoding.
* \param max_image_value Maximum image value
*/
CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
const std::string& encoding_out = std::string(),
bool do_dynamic_scaling = false,
double min_image_value = 0.0,
double max_image_value = 0.0);
/**
* \brief Get the OpenCV type enum corresponding to the encoding.
*
* For example, "bgr8" -> CV_8UC3.
*/
int getCvType(const std::string& encoding);
} // namespace cv_bridge
// CvImage as a first class message type
// The rest of this file hooks into the roscpp serialization API to make CvImage
// a first-class message type you can publish and subscribe to directly.
// Unfortunately this doesn't yet work with image_transport, so don't rewrite all
// your callbacks to use CvImage! It might be useful for specific tasks, like
// processing bag files.
/// @cond DOXYGEN_IGNORE
namespace ros {
namespace message_traits {
template<> struct MD5Sum<cv_bridge::CvImage>
{
static const char* value() { return MD5Sum<sensor_msgs::Image>::value(); }
static const char* value(const cv_bridge::CvImage&) { return value(); }
static const uint64_t static_value1 = MD5Sum<sensor_msgs::Image>::static_value1;
static const uint64_t static_value2 = MD5Sum<sensor_msgs::Image>::static_value2;
// If the definition of sensor_msgs/Image changes, we'll get a compile error here.
ROS_STATIC_ASSERT(MD5Sum<sensor_msgs::Image>::static_value1 == 0x060021388200f6f0ULL);
ROS_STATIC_ASSERT(MD5Sum<sensor_msgs::Image>::static_value2 == 0xf447d0fcd9c64743ULL);
};
template<> struct DataType<cv_bridge::CvImage>
{
static const char* value() { return DataType<sensor_msgs::Image>::value(); }
static const char* value(const cv_bridge::CvImage&) { return value(); }
};
template<> struct Definition<cv_bridge::CvImage>
{
static const char* value() { return Definition<sensor_msgs::Image>::value(); }
static const char* value(const cv_bridge::CvImage&) { return value(); }
};
template<> struct HasHeader<cv_bridge::CvImage> : TrueType {};
} // namespace ros::message_traits
namespace serialization {
template<> struct Serializer<cv_bridge::CvImage>
{
/// @todo Still ignoring endianness...
template<typename Stream>
inline static void write(Stream& stream, const cv_bridge::CvImage& m)
{
stream.next(m.header);
stream.next((uint32_t)m.image.rows); // height
stream.next((uint32_t)m.image.cols); // width
stream.next(m.encoding);
uint8_t is_bigendian = 0;
stream.next(is_bigendian);
stream.next((uint32_t)m.image.step);
size_t data_size = m.image.step*m.image.rows;
stream.next((uint32_t)data_size);
if (data_size > 0)
memcpy(stream.advance(data_size), m.image.data, data_size);
}
template<typename Stream>
inline static void read(Stream& stream, cv_bridge::CvImage& m)
{
stream.next(m.header);
uint32_t height, width;
stream.next(height);
stream.next(width);
stream.next(m.encoding);
uint8_t is_bigendian;
stream.next(is_bigendian);
uint32_t step, data_size;
stream.next(step);
stream.next(data_size);
int type = cv_bridge::getCvType(m.encoding);
// Construct matrix pointing to the stream data, then copy it to m.image
cv::Mat tmp((int)height, (int)width, type, stream.advance(data_size), (size_t)step);
tmp.copyTo(m.image);
}
inline static uint32_t serializedLength(const cv_bridge::CvImage& m)
{
size_t data_size = m.image.step*m.image.rows;
return serializationLength(m.header) + serializationLength(m.encoding) + 17 + data_size;
}
};
} // namespace ros::serialization
namespace message_operations {
template<> struct Printer<cv_bridge::CvImage>
{
template<typename Stream>
static void stream(Stream& s, const std::string& indent, const cv_bridge::CvImage& m)
{
/// @todo Replicate printing for sensor_msgs::Image
}
};
} // namespace ros::message_operations
} // namespace ros
namespace cv_bridge {
inline std::ostream& operator<<(std::ostream& s, const CvImage& m)
{
ros::message_operations::Printer<CvImage>::stream(s, "", m);
return s;
}
} // namespace cv_bridge
/// @endcond
#endif