46 static const int MONO_EITHER =
MONO |
RECT;
48 if (!(flags &
ALL))
return true;
51 const std::string& raw_encoding = raw_image->encoding;
52 int raw_type = CV_8UC1;
53 if (raw_encoding == enc::BGR8 || raw_encoding == enc::RGB8) {
58 const cv::Mat raw(raw_image->height, raw_image->width, raw_type,
59 const_cast<uint8_t*>(&raw_image->data[0]), raw_image->step);
66 if (raw_encoding.find(
"bayer") != std::string::npos) {
70 if (raw_encoding == enc::BAYER_RGGB8)
71 code = cv::COLOR_BayerBG2BGR;
72 else if (raw_encoding == enc::BAYER_BGGR8)
73 code = cv::COLOR_BayerRG2BGR;
74 else if (raw_encoding == enc::BAYER_GBRG8)
75 code = cv::COLOR_BayerGR2BGR;
76 else if (raw_encoding == enc::BAYER_GRBG8)
77 code = cv::COLOR_BayerGB2BGR;
79 ROS_ERROR(
"[image_proc] Unsupported encoding '%s'", raw_encoding.c_str());
82 cv::cvtColor(raw, output.
color, code);
85 if (flags & MONO_EITHER)
86 cv::cvtColor(output.
color, output.
mono, cv::COLOR_BGR2GRAY);
89 else if (raw_type == CV_8UC3) {
91 if (flags & MONO_EITHER) {
92 int code = (raw_encoding == enc::BGR8) ? cv::COLOR_BGR2GRAY : cv::COLOR_RGB2GRAY;
93 cv::cvtColor(output.
color, output.
mono, code);
97 else if (raw_encoding == enc::MONO8) {
99 if (flags & COLOR_EITHER) {
105 else if (raw_encoding == enc::TYPE_8UC3) {
106 ROS_ERROR(
"[image_proc] Ambiguous encoding '8UC3'. The camera driver " 107 "should set the encoding to 'bgr8' or 'rgb8'.");
112 ROS_ERROR(
"[image_proc] Unsupported encoding '%s'", raw_encoding.c_str());
std::string color_encoding
bool process(const sensor_msgs::ImageConstPtr &raw_image, const image_geometry::PinholeCameraModel &model, ImageSet &output, int flags=ALL) const
void rectifyImage(const cv::Mat &raw, cv::Mat &rectified, int interpolation=cv::INTER_LINEAR) const