diff --git a/cv_bridge/src/cv_bridge.cpp b/cv_bridge/src/cv_bridge.cpp index e763e5da7..e1e44de83 100644 --- a/cv_bridge/src/cv_bridge.cpp +++ b/cv_bridge/src/cv_bridge.cpp @@ -54,6 +54,23 @@ namespace enc = sensor_msgs::image_encodings; namespace cv_bridge { +static inline bool isPlanar(const std::string & encoding) +{ + return encoding == enc::NV12 || encoding == enc::NV21 || encoding == enc::NV24; +} + +static inline float getHeightScaling(const std::string & encoding) +{ + if (isPlanar(encoding)) { + if (encoding == enc::NV12 || + encoding == enc::NV21) + { + return 1.5f; + } + } + return 1.0f; +} + static int depthStrToInt(const std::string depth) { if (depth == "8U") { @@ -99,6 +116,7 @@ int getCvType(const std::string & encoding) // Miscellaneous if (encoding == enc::YUV422) {return CV_8UC2;} if (encoding == enc::YUV422_YUY2) {return CV_8UC2;} + if (encoding == enc::NV12) {return CV_8UC1;} // Check all the generic content encodings std::cmatch m; @@ -120,7 +138,7 @@ int getCvType(const std::string & encoding) /// @cond DOXYGEN_IGNORE -enum Encoding { INVALID = -1, GRAY = 0, RGB, BGR, RGBA, BGRA, YUV422, YUV422_YUY2, BAYER_RGGB, BAYER_BGGR, +enum Encoding { INVALID = -1, GRAY = 0, RGB, BGR, RGBA, BGRA, YUV422, YUV422_YUY2, NV12, BAYER_RGGB, BAYER_BGGR, BAYER_GBRG, BAYER_GRBG, MAX_ENCODING}; Encoding getEncoding(const std::string & encoding) @@ -132,6 +150,7 @@ Encoding getEncoding(const std::string & encoding) if ((encoding == enc::RGBA8) || (encoding == enc::RGBA16)) {return RGBA;} if (encoding == enc::YUV422) {return YUV422;} if (encoding == enc::YUV422_YUY2) {return YUV422_YUY2;} + if (encoding == enc::NV12) {return NV12;} if ((encoding == enc::BAYER_RGGB8) || (encoding == enc::BAYER_RGGB16)) {return BAYER_RGGB;} if ((encoding == enc::BAYER_BGGR8) || (encoding == enc::BAYER_BGGR16)) {return BAYER_BGGR;} @@ -192,6 +211,12 @@ std::map, std::vector> getConversionCodes() res[std::make_pair(YUV422_YUY2, RGBA)].push_back(cv::COLOR_YUV2RGBA_YUY2); res[std::make_pair(YUV422_YUY2, BGRA)].push_back(cv::COLOR_YUV2BGRA_YUY2); + res[std::make_pair(NV12, GRAY)].push_back(cv::COLOR_YUV2GRAY_NV12); + res[std::make_pair(NV12, RGB)].push_back(cv::COLOR_YUV2RGB_NV12); + res[std::make_pair(NV12, BGR)].push_back(cv::COLOR_YUV2BGR_NV12); + res[std::make_pair(NV12, RGBA)].push_back(cv::COLOR_YUV2RGBA_NV12); + res[std::make_pair(NV12, BGRA)].push_back(cv::COLOR_YUV2BGRA_NV12); + // Deal with Bayer res[std::make_pair(BAYER_RGGB, GRAY)].push_back(cv::COLOR_BayerBG2GRAY); res[std::make_pair(BAYER_RGGB, RGB)].push_back(cv::COLOR_BayerBG2RGB); @@ -217,9 +242,9 @@ const std::vector getConversionCode(std::string src_encoding, std::string d Encoding src_encod = getEncoding(src_encoding); Encoding dst_encod = getEncoding(dst_encoding); bool is_src_color_format = enc::isColor(src_encoding) || enc::isMono(src_encoding) || - enc::isBayer(src_encoding) || (src_encoding == enc::YUV422) || (src_encoding == enc::YUV422_YUY2); + enc::isBayer(src_encoding) || (src_encoding == enc::YUV422) || (src_encoding == enc::YUV422_YUY2) || (src_encoding == enc::NV12); bool is_dst_color_format = enc::isColor(dst_encoding) || enc::isMono(dst_encoding) || - enc::isBayer(dst_encoding) || (dst_encoding == enc::YUV422) || (dst_encoding == enc::YUV422_YUY2); + enc::isBayer(dst_encoding) || (dst_encoding == enc::YUV422) || (dst_encoding == enc::YUV422_YUY2) || (dst_encoding == enc::NV12); bool is_num_channels_the_same = (enc::numChannels(src_encoding) == enc::numChannels(dst_encoding)); @@ -288,15 +313,15 @@ cv::Mat matFromImage(const sensor_msgs::msg::Image & source) throw Exception(ss.str()); } - if (source.height * source.step != source.data.size()) { + if (source.height * source.step * getHeightScaling(source.encoding) != source.data.size()) { std::stringstream ss; - ss << "Image is wrongly formed: height * step != size or " << source.height << " * " << - source.step << " != " << source.data.size(); + ss << "Image is wrongly formed: height * step * HeightScaling != size or " << source.height << " * " << + source.step << " * " << getHeightScaling(source.encoding) << " != " << 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(&source.data[0]), + cv::Mat mat(source.height * getHeightScaling(source.encoding), source.width, source_type, const_cast(&source.data[0]), source.step); if ((rcpputils::endian::native == rcpputils::endian::big && source.is_bigendian) || @@ -389,7 +414,7 @@ sensor_msgs::msg::Image::SharedPtr CvImage::toImageMsg() const void CvImage::toImageMsg(sensor_msgs::msg::Image & ros_image) const { ros_image.header = header; - ros_image.height = image.rows; + ros_image.height = image.rows / getHeightScaling(encoding); ros_image.width = image.cols; ros_image.encoding = encoding; ros_image.is_bigendian = (rcpputils::endian::native == rcpputils::endian::big); diff --git a/cv_bridge/test/utest2.cpp b/cv_bridge/test/utest2.cpp index 7121b04b4..7a79283d1 100644 --- a/cv_bridge/test/utest2.cpp +++ b/cv_bridge/test/utest2.cpp @@ -45,6 +45,23 @@ using namespace sensor_msgs::image_encodings; +static inline bool isPlanar(const std::string & encoding) +{ + return encoding == NV12 || encoding == NV21 || encoding == NV24; +} + +static inline float getHeightScaling(const std::string & encoding) +{ + if (isPlanar(encoding)) { + if (encoding == NV12 || + encoding == NV21) + { + return 1.5f; + } + } + return 1.0f; +} + bool isUnsigned(const std::string & encoding) { return encoding == RGB8 || encoding == RGBA8 || encoding == RGB16 || encoding == RGBA16 || @@ -71,8 +88,8 @@ getEncodings() TYPE_64FC1, /*TYPE_64FC2,*/ TYPE_64FC3, TYPE_64FC4, // BAYER_RGGB8, BAYER_BGGR8, BAYER_GBRG8, BAYER_GRBG8, // BAYER_RGGB16, BAYER_BGGR16, BAYER_GBRG16, BAYER_GRBG16, - YUV422, YUV422_YUY2}; - return std::vector(encodings, encodings + 48 - 8 - 7); + YUV422, YUV422_YUY2, NV12}; + return std::vector(encodings, encodings + 48 - 8 - 6); } TEST(OpencvTests, testCase_encode_decode) @@ -82,8 +99,9 @@ TEST(OpencvTests, testCase_encode_decode) std::string src_encoding = encodings[i]; bool is_src_color_format = isColor(src_encoding) || isMono(src_encoding) || (src_encoding == sensor_msgs::image_encodings::YUV422) || - (src_encoding == sensor_msgs::image_encodings::YUV422_YUY2); - cv::Mat image_original(cv::Size(400, 400), cv_bridge::getCvType(src_encoding)); + (src_encoding == sensor_msgs::image_encodings::YUV422_YUY2) || + (src_encoding == sensor_msgs::image_encodings::NV12); + cv::Mat image_original(cv::Size(400, 400 * getHeightScaling(src_encoding)), cv_bridge::getCvType(src_encoding)); cv::RNG r(77); r.fill(image_original, cv::RNG::UNIFORM, 0, 127); @@ -97,7 +115,8 @@ TEST(OpencvTests, testCase_encode_decode) std::string dst_encoding = encodings[j]; bool is_dst_color_format = isColor(dst_encoding) || isMono(dst_encoding) || (dst_encoding == sensor_msgs::image_encodings::YUV422) || - (dst_encoding == sensor_msgs::image_encodings::YUV422_YUY2); + (dst_encoding == sensor_msgs::image_encodings::YUV422_YUY2) || + (dst_encoding == sensor_msgs::image_encodings::NV12); bool is_num_channels_the_same = (numChannels(src_encoding) == numChannels(dst_encoding)); cv_bridge::CvImageConstPtr cv_image; @@ -127,21 +146,24 @@ TEST(OpencvTests, testCase_encode_decode) EXPECT_THROW((void)cvtColor(cv_image, src_encoding)->image, cv_bridge::Exception); continue; } - // We do not support conversion to YUV422 for now, except from YUV422 + // We do not support conversion to YUV422/NV12 for now, except from YUV422/NV12 if (((dst_encoding == YUV422) && (src_encoding != YUV422)) || - ((dst_encoding == YUV422_YUY2) && (src_encoding != YUV422_YUY2))) { + ((dst_encoding == YUV422_YUY2) && (src_encoding != YUV422_YUY2)) || + ((dst_encoding == NV12) && (src_encoding != NV12))) { EXPECT_THROW(cv_bridge::toCvShare(image_msg, dst_encoding), cv_bridge::Exception); continue; } cv_image = cv_bridge::toCvShare(image_msg, dst_encoding); - // We do not support conversion to YUV422 for now, except from YUV422 + // We do not support conversion to YUV422/NV12 for now, except from YUV422/NV12 if (((src_encoding == YUV422) && (dst_encoding != YUV422)) || - ((src_encoding == YUV422_YUY2) && (dst_encoding != YUV422_YUY2))){ + ((src_encoding == YUV422_YUY2) && (dst_encoding != YUV422_YUY2)) || + ((src_encoding == NV12) && (dst_encoding != NV12))){ EXPECT_THROW((void)cvtColor(cv_image, src_encoding)->image, cv_bridge::Exception); continue; } + } // And convert back to a cv::Mat image_back = cvtColor(cv_image, src_encoding)->image;