diff --git a/cv_bridge/src/cv_bridge.cpp b/cv_bridge/src/cv_bridge.cpp index dd11c78d2..6f7041913 100644 --- a/cv_bridge/src/cv_bridge.cpp +++ b/cv_bridge/src/cv_bridge.cpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include "sensor_msgs/image_encodings.hpp" #include "rcpputils/endian.hpp" #include @@ -97,8 +97,12 @@ int getCvType(const std::string & encoding) if (encoding == enc::BAYER_GRBG16) {return CV_16UC1;} // Miscellaneous + if (encoding == enc::UYVY) {return CV_8UC2;} if (encoding == enc::YUV422) {return CV_8UC2;} + if (encoding == enc::YUYV) {return CV_8UC2;} if (encoding == enc::YUV422_YUY2) {return CV_8UC2;} + if (encoding == enc::NV21) {return CV_8UC2;} + if (encoding == enc::NV24) {return CV_8UC2;} // Check all the generic content encodings std::cmatch m; @@ -120,7 +124,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, UYVY, YUYV, BAYER_RGGB, BAYER_BGGR, BAYER_GBRG, BAYER_GRBG}; Encoding getEncoding(const std::string & encoding) @@ -130,8 +134,8 @@ Encoding getEncoding(const std::string & encoding) if ((encoding == enc::RGB8) || (encoding == enc::RGB16)) {return RGB;} if ((encoding == enc::BGRA8) || (encoding == enc::BGRA16)) {return BGRA;} 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::UYVY) || (encoding == enc::YUV422)){return UYVY;} + if ((encoding == enc::YUYV) || (encoding == enc::YUV422_YUY2)) {return YUYV;} if ((encoding == enc::BAYER_RGGB8) || (encoding == enc::BAYER_RGGB16)) {return BAYER_RGGB;} if ((encoding == enc::BAYER_BGGR8) || (encoding == enc::BAYER_BGGR16)) {return BAYER_BGGR;} @@ -152,62 +156,62 @@ std::map, std::vector> getConversionCodes() { std::map, std::vector> res; for (int i = 0; i <= 5; ++i) { - res[std::pair(Encoding(i), Encoding(i))].push_back(SAME_FORMAT); + res[{Encoding(i), Encoding(i)}].push_back(SAME_FORMAT); } - res[std::make_pair(GRAY, RGB)].push_back(cv::COLOR_GRAY2RGB); - res[std::make_pair(GRAY, BGR)].push_back(cv::COLOR_GRAY2BGR); - res[std::make_pair(GRAY, RGBA)].push_back(cv::COLOR_GRAY2RGBA); - res[std::make_pair(GRAY, BGRA)].push_back(cv::COLOR_GRAY2BGRA); - - res[std::make_pair(RGB, GRAY)].push_back(cv::COLOR_RGB2GRAY); - res[std::make_pair(RGB, BGR)].push_back(cv::COLOR_RGB2BGR); - res[std::make_pair(RGB, RGBA)].push_back(cv::COLOR_RGB2RGBA); - res[std::make_pair(RGB, BGRA)].push_back(cv::COLOR_RGB2BGRA); - - res[std::make_pair(BGR, GRAY)].push_back(cv::COLOR_BGR2GRAY); - res[std::make_pair(BGR, RGB)].push_back(cv::COLOR_BGR2RGB); - res[std::make_pair(BGR, RGBA)].push_back(cv::COLOR_BGR2RGBA); - res[std::make_pair(BGR, BGRA)].push_back(cv::COLOR_BGR2BGRA); - - res[std::make_pair(RGBA, GRAY)].push_back(cv::COLOR_RGBA2GRAY); - res[std::make_pair(RGBA, RGB)].push_back(cv::COLOR_RGBA2RGB); - res[std::make_pair(RGBA, BGR)].push_back(cv::COLOR_RGBA2BGR); - res[std::make_pair(RGBA, BGRA)].push_back(cv::COLOR_RGBA2BGRA); - - res[std::make_pair(BGRA, GRAY)].push_back(cv::COLOR_BGRA2GRAY); - res[std::make_pair(BGRA, RGB)].push_back(cv::COLOR_BGRA2RGB); - res[std::make_pair(BGRA, BGR)].push_back(cv::COLOR_BGRA2BGR); - res[std::make_pair(BGRA, RGBA)].push_back(cv::COLOR_BGRA2RGBA); - - res[std::make_pair(YUV422, GRAY)].push_back(cv::COLOR_YUV2GRAY_UYVY); - res[std::make_pair(YUV422, RGB)].push_back(cv::COLOR_YUV2RGB_UYVY); - res[std::make_pair(YUV422, BGR)].push_back(cv::COLOR_YUV2BGR_UYVY); - res[std::make_pair(YUV422, RGBA)].push_back(cv::COLOR_YUV2RGBA_UYVY); - res[std::make_pair(YUV422, BGRA)].push_back(cv::COLOR_YUV2BGRA_UYVY); - - res[std::make_pair(YUV422_YUY2, GRAY)].push_back(cv::COLOR_YUV2GRAY_YUY2); - res[std::make_pair(YUV422_YUY2, RGB)].push_back(cv::COLOR_YUV2RGB_YUY2); - res[std::make_pair(YUV422_YUY2, BGR)].push_back(cv::COLOR_YUV2BGR_YUY2); - 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[{GRAY, RGB}].push_back(cv::COLOR_GRAY2RGB); + res[{GRAY, BGR}].push_back(cv::COLOR_GRAY2BGR); + res[{GRAY, RGBA}].push_back(cv::COLOR_GRAY2RGBA); + res[{GRAY, BGRA}].push_back(cv::COLOR_GRAY2BGRA); + + res[{RGB, GRAY}].push_back(cv::COLOR_RGB2GRAY); + res[{RGB, BGR}].push_back(cv::COLOR_RGB2BGR); + res[{RGB, RGBA}].push_back(cv::COLOR_RGB2RGBA); + res[{RGB, BGRA}].push_back(cv::COLOR_RGB2BGRA); + + res[{BGR, GRAY}].push_back(cv::COLOR_BGR2GRAY); + res[{BGR, RGB}].push_back(cv::COLOR_BGR2RGB); + res[{BGR, RGBA}].push_back(cv::COLOR_BGR2RGBA); + res[{BGR, BGRA}].push_back(cv::COLOR_BGR2BGRA); + + res[{RGBA, GRAY}].push_back(cv::COLOR_RGBA2GRAY); + res[{RGBA, RGB}].push_back(cv::COLOR_RGBA2RGB); + res[{RGBA, BGR}].push_back(cv::COLOR_RGBA2BGR); + res[{RGBA, BGRA}].push_back(cv::COLOR_RGBA2BGRA); + + res[{BGRA, GRAY}].push_back(cv::COLOR_BGRA2GRAY); + res[{BGRA, RGB}].push_back(cv::COLOR_BGRA2RGB); + res[{BGRA, BGR}].push_back(cv::COLOR_BGRA2BGR); + res[{BGRA, RGBA}].push_back(cv::COLOR_BGRA2RGBA); + + res[{UYVY, GRAY}].push_back(cv::COLOR_YUV2GRAY_UYVY); + res[{UYVY, RGB}].push_back(cv::COLOR_YUV2RGB_UYVY); + res[{UYVY, BGR}].push_back(cv::COLOR_YUV2BGR_UYVY); + res[{UYVY, RGBA}].push_back(cv::COLOR_YUV2RGBA_UYVY); + res[{UYVY, BGRA}].push_back(cv::COLOR_YUV2BGRA_UYVY); + + res[{YUYV, GRAY}].push_back(cv::COLOR_YUV2GRAY_YUY2); + res[{YUYV, RGB}].push_back(cv::COLOR_YUV2RGB_YUY2); + res[{YUYV, BGR}].push_back(cv::COLOR_YUV2BGR_YUY2); + res[{YUYV, RGBA}].push_back(cv::COLOR_YUV2RGBA_YUY2); + res[{YUYV, BGRA}].push_back(cv::COLOR_YUV2BGRA_YUY2); // 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); - res[std::make_pair(BAYER_RGGB, BGR)].push_back(cv::COLOR_BayerBG2BGR); + res[{BAYER_RGGB, GRAY}].push_back(cv::COLOR_BayerBG2GRAY); + res[{BAYER_RGGB, RGB}].push_back(cv::COLOR_BayerBG2RGB); + res[{BAYER_RGGB, BGR}].push_back(cv::COLOR_BayerBG2BGR); - res[std::make_pair(BAYER_BGGR, GRAY)].push_back(cv::COLOR_BayerRG2GRAY); - res[std::make_pair(BAYER_BGGR, RGB)].push_back(cv::COLOR_BayerRG2RGB); - res[std::make_pair(BAYER_BGGR, BGR)].push_back(cv::COLOR_BayerRG2BGR); + res[{BAYER_BGGR, GRAY}].push_back(cv::COLOR_BayerRG2GRAY); + res[{BAYER_BGGR, RGB}].push_back(cv::COLOR_BayerRG2RGB); + res[{BAYER_BGGR, BGR}].push_back(cv::COLOR_BayerRG2BGR); - res[std::make_pair(BAYER_GBRG, GRAY)].push_back(cv::COLOR_BayerGR2GRAY); - res[std::make_pair(BAYER_GBRG, RGB)].push_back(cv::COLOR_BayerGR2RGB); - res[std::make_pair(BAYER_GBRG, BGR)].push_back(cv::COLOR_BayerGR2BGR); + res[{BAYER_GBRG, GRAY}].push_back(cv::COLOR_BayerGR2GRAY); + res[{BAYER_GBRG, RGB}].push_back(cv::COLOR_BayerGR2RGB); + res[{BAYER_GBRG, BGR}].push_back(cv::COLOR_BayerGR2BGR); - res[std::make_pair(BAYER_GRBG, GRAY)].push_back(cv::COLOR_BayerGB2GRAY); - res[std::make_pair(BAYER_GRBG, RGB)].push_back(cv::COLOR_BayerGB2RGB); - res[std::make_pair(BAYER_GRBG, BGR)].push_back(cv::COLOR_BayerGB2BGR); + res[{BAYER_GRBG, GRAY}].push_back(cv::COLOR_BayerGB2GRAY); + res[{BAYER_GRBG, RGB}].push_back(cv::COLOR_BayerGB2RGB); + res[{BAYER_GRBG, BGR}].push_back(cv::COLOR_BayerGB2BGR); return res; } @@ -217,9 +221,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); 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); bool is_num_channels_the_same = (enc::numChannels(src_encoding) == enc::numChannels(dst_encoding)); @@ -599,7 +603,7 @@ CvImageConstPtr cvtColorForDisplay( } else { // We choose BGR by default here as we assume people will use OpenCV if ((enc::bitDepth(source->encoding) == 8) || - (enc::bitDepth(source->encoding) == 16) || + (enc::bitDepth(source->encoding) == 16) || (enc::bitDepth(source->encoding) == 32)) { encoding = enc::BGR8; diff --git a/cv_bridge/test/utest2.cpp b/cv_bridge/test/utest2.cpp index 7121b04b4..61d2cc729 100644 --- a/cv_bridge/test/utest2.cpp +++ b/cv_bridge/test/utest2.cpp @@ -35,7 +35,7 @@ #include #include -#include +#include "sensor_msgs/image_encodings.hpp" #include #include @@ -45,59 +45,114 @@ using namespace sensor_msgs::image_encodings; -bool isUnsigned(const std::string & encoding) -{ - return encoding == RGB8 || encoding == RGBA8 || encoding == RGB16 || encoding == RGBA16 || - encoding == BGR8 || encoding == BGRA8 || encoding == BGR16 || encoding == BGRA16 || - encoding == MONO8 || encoding == MONO16 || - encoding == MONO8 || encoding == MONO16 || encoding == TYPE_8UC1 || - encoding == TYPE_8UC2 || encoding == TYPE_8UC3 || encoding == TYPE_8UC4 || - encoding == TYPE_16UC1 || encoding == TYPE_16UC2 || encoding == TYPE_16UC3 || - encoding == TYPE_16UC4; - // BAYER_RGGB8, BAYER_BGGR8, BAYER_GBRG8, BAYER_GRBG8, BAYER_RGGB16, - // BAYER_BGGR16, BAYER_GBRG16, BAYER_GRBG16, YUV422 -} -std::vector -getEncodings() + +struct EncodingProperty { -// TODO(N/A) for Groovy, the following types should be uncommented - std::string encodings[] = {RGB8, RGBA8, RGB16, RGBA16, BGR8, BGRA8, BGR16, BGRA16, MONO8, MONO16, - TYPE_8UC1, /*TYPE_8UC2,*/ TYPE_8UC3, TYPE_8UC4, - TYPE_8SC1, /*TYPE_8SC2,*/ TYPE_8SC3, TYPE_8SC4, - TYPE_16UC1, /*TYPE_16UC2,*/ TYPE_16UC3, TYPE_16UC4, - TYPE_16SC1, /*TYPE_16SC2,*/ TYPE_16SC3, TYPE_16SC4, - TYPE_32SC1, /*TYPE_32SC2,*/ TYPE_32SC3, TYPE_32SC4, - TYPE_32FC1, /*TYPE_32FC2,*/ TYPE_32FC3, TYPE_32FC4, - 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); -} + EncodingProperty(int num_channels, bool is_yuv, bool has_channel_info) + : num_channels(num_channels), is_yuv(is_yuv), has_channel_info(has_channel_info) + { + } + + int num_channels; + bool is_yuv; + + // Whether the channels can be assumed in the encoding. We cannot assume channels in opencv encodings. + // Think about it this way: it does not make sense to convert 8UC3 to RGB as you don't know what + // you store in your 3 channels (could be BGR, YUV whatever). Same here, maybe your 1 channel is + // grayscale, maybe intensity, maybe it's not even color, it's a depth image. + bool has_channel_info; +}; + +typedef std::map EncodingProperties; +static EncodingProperties encoding_properties = { + {RGB8, {3, false, true}}, + {RGBA8, {4, false, true}}, + {RGB16, {3, false, true}}, + {RGBA16, {4, false, true}}, + {BGR8, {3, false, true}}, + {BGRA8, {4, false, true}}, + {BGR16, {3, false, true}}, + {BGRA16, {4, false, true}}, + {MONO8, {1, false, true}}, + {MONO16, {1, false, true}}, + + // OpenCV CvMat types + {TYPE_8UC1, {1, false, false}}, + {TYPE_8UC2, {2, false, false}}, + {TYPE_8UC3, {3, false, false}}, + {TYPE_8UC4, {4, false, false}}, + {TYPE_8SC1, {1, false, false}}, + {TYPE_8SC2, {2, false, false}}, + {TYPE_8SC3, {3, false, false}}, + {TYPE_8SC4, {4, false, false}}, + {TYPE_16UC1, {1, false, false}}, + {TYPE_16UC2, {2, false, false}}, + {TYPE_16UC3, {3, false, false}}, + {TYPE_16UC4, {4, false, false}}, + {TYPE_16SC1, {1, false, false}}, + {TYPE_16SC2, {2, false, false}}, + {TYPE_16SC3, {3, false, false}}, + {TYPE_16SC4, {4, false, false}}, + {TYPE_32SC1, {1, false, false}}, + {TYPE_32SC2, {2, false, false}}, + {TYPE_32SC3, {3, false, false}}, + {TYPE_32SC4, {4, false, false}}, + {TYPE_32FC1, {1, false, false}}, + {TYPE_32FC2, {2, false, false}}, + {TYPE_32FC3, {3, false, false}}, + {TYPE_32FC4, {4, false, false}}, + {TYPE_64FC1, {1, false, false}}, + {TYPE_64FC2, {2, false, false}}, + {TYPE_64FC3, {3, false, false}}, + {TYPE_64FC4, {4, false, false}}, + + // // Bayer encodings + // {BAYER_RGGB8, }, + // {BAYER_BGGR8, }, + // {BAYER_GBRG8, }, + // {BAYER_GRBG8, }, + // {BAYER_RGGB16, }, + // {BAYER_BGGR16, }, + // {BAYER_GBRG16, }, + // {BAYER_GRBG16, }, + + // YUV formats + // YUV 4:2:2 encodings with an 8-bit depth + // https://www.kernel.org/doc/html/latest/userspace-api/media/v4l/pixfmt-packed-yuv.html#id1 + // fourcc: UYVY + {UYVY, {2, true, true}}, + {YUV422, {2, true, true}}, // Deprecated + // fourcc: YUYV + {YUYV, {2, true, true}}, + {YUV422_YUY2, {2, true, true}}, // Deprecated + + // YUV 4:2:0 encodings with an 8-bit depth + // NV21: https://www.kernel.org/doc/html/latest/userspace-api/media/v4l/pixfmt-yuv-planar.html#nv12-nv21-nv12m-and-nv21m + {NV21, {2, true, true}}, + + // YUV 4:4:4 encodings with 8-bit depth + // NV24: https://www.kernel.org/doc/html/latest/userspace-api/media/v4l/pixfmt-yuv-planar.html#nv24-and-nv42 + {NV24, {2, true, true}}, +}; + TEST(OpencvTests, testCase_encode_decode) { - std::vector encodings = getEncodings(); - for (size_t i = 0; i < encodings.size(); ++i) { - std::string src_encoding = encodings[i]; + for (const auto & [src_encoding, src_encoding_property] : encoding_properties) { 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); + src_encoding_property.is_yuv; cv::Mat image_original(cv::Size(400, 400), cv_bridge::getCvType(src_encoding)); cv::RNG r(77); r.fill(image_original, cv::RNG::UNIFORM, 0, 127); - sensor_msgs::msg::Image image_message; cv_bridge::CvImage image_bridge(std_msgs::msg::Header(), src_encoding, image_original); // Convert to a sensor_msgs::Image sensor_msgs::msg::Image::SharedPtr image_msg = image_bridge.toImageMsg(); - for (size_t j = 0; j < encodings.size(); ++j) { - std::string dst_encoding = encodings[j]; + for (const auto & [dst_encoding, dst_encoding_property] : encoding_properties) { 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_property.is_yuv; bool is_num_channels_the_same = (numChannels(src_encoding) == numChannels(dst_encoding)); cv_bridge::CvImageConstPtr cv_image; @@ -122,23 +177,26 @@ TEST(OpencvTests, testCase_encode_decode) EXPECT_THROW(cv_bridge::toCvShare(image_msg, dst_encoding), cv_bridge::Exception); continue; } + // We do not support conversion from Bayer for now + if (isBayer(dst_encoding) && src_encoding != dst_encoding) { + EXPECT_THROW(cv_bridge::toCvShare(image_msg, src_encoding), cv_bridge::Exception); + continue; + } cv_image = cv_bridge::toCvShare(image_msg, dst_encoding); // We cannot convert from non-color to color 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 - if (((dst_encoding == YUV422) && (src_encoding != YUV422)) || - ((dst_encoding == YUV422_YUY2) && (src_encoding != YUV422_YUY2))) { + // We do not support conversion to YUV for now + if (dst_encoding_property.is_yuv && src_encoding != dst_encoding) { 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 - if (((src_encoding == YUV422) && (dst_encoding != YUV422)) || - ((src_encoding == YUV422_YUY2) && (dst_encoding != YUV422_YUY2))){ + // We do not support conversion from YUV for now + if (src_encoding_property.is_yuv && src_encoding != dst_encoding) { EXPECT_THROW((void)cvtColor(cv_image, src_encoding)->image, cv_bridge::Exception); continue; } @@ -153,20 +211,94 @@ TEST(OpencvTests, testCase_encode_decode) } if (bitDepth(src_encoding) >= 32) { // In the case where the input has floats, we will lose precision but no more than 1 - EXPECT_LT(cv::norm(image_original, image_back, cv::NORM_INF), + EXPECT_LT( + cv::norm(image_original, image_back, cv::NORM_INF), 1) << "problem converting from " << src_encoding << " to " << dst_encoding << " and back."; } else if ((bitDepth(src_encoding) == 16) && (bitDepth(dst_encoding) == 8)) { // In the case where the input has floats, we // will lose precision but no more than 1 * max(127) - EXPECT_LT(cv::norm(image_original, image_back, cv::NORM_INF), + EXPECT_LT( + cv::norm(image_original, image_back, cv::NORM_INF), 128) << "problem converting from " << src_encoding << " to " << dst_encoding << " and back."; } else { - EXPECT_EQ(cv::norm(image_original, image_back, cv::NORM_INF), + EXPECT_EQ( + cv::norm(image_original, image_back, cv::NORM_INF), 0) << "problem converting from " << src_encoding << " to " << dst_encoding << " and back."; } } } } + +static bool conversionPossible(const std::string & src_encoding, const std::string & dst_encoding) +{ + return true; +} + +class TestCVConversion : public testing::TestWithParam> +{ +public: + struct PrintToStringParamName + { + template + std::string operator()(const testing::TestParamInfo & info) const + { + const auto & [src_property, dst_property] = info.param; + const auto & [src_encoding, _1] = src_property; + const auto & [dst_encoding, _2] = dst_property; + std::string name = src_encoding + "_to_" + dst_encoding; + return name; + } + }; +}; + +TEST_P(TestCVConversion, ) +{ + const auto & [src_property, dst_property] = GetParam(); + const auto & [src_encoding, src_encoding_property] = src_property; + const auto & [dst_encoding, dst_encoding_property] = dst_property; + + // Generate a random image + cv::Mat image_original(cv::Size(400, 400), cv_bridge::getCvType(src_encoding)); + cv::RNG r(77); + r.fill(image_original, cv::RNG::UNIFORM, 0, 127); + + sensor_msgs::msg::Image image_message; + cv_bridge::CvImage image_bridge(std_msgs::msg::Header(), src_encoding, image_original); + + // Convert to a sensor_msgs::Image + sensor_msgs::msg::Image::SharedPtr image_msg = image_bridge.toImageMsg(); + + if (conversionPossible(src_encoding, dst_encoding)) { + cv_bridge::CvImageConstPtr cv_image; + EXPECT_NO_THROW( + auto cv_image = cv_bridge::toCvShare(image_msg, dst_encoding) + ) << "problem converting from " << src_encoding << " to " << dst_encoding; + } else { + EXPECT_THROW(cv_bridge::toCvShare(image_msg, dst_encoding), cv_bridge::Exception); + } +} + +static std::string generateName(const testing::TestParamInfo& info) +{ + const auto & [src_property, dst_property] = info.param; + const auto & [src_encoding, _1] = src_property; + const auto & [dst_encoding, _2] = dst_property; + return src_encoding + "_to_" + dst_encoding; +} + +INSTANTIATE_TEST_SUITE_P( + , + TestCVConversion, testing::Combine( + testing::ValuesIn(encoding_properties), + testing::ValuesIn(encoding_properties)), + generateName); + +// TIP: To test conversion of one cv type to another, run the following. +// +// ./build/cv_bridge/test/cv_bridge-utest --gtest_filter=TestCVConversion.mono8_to_mono16 +// +// Replace mono8 with your source encoding, and mono16 with your target encoding