diff --git a/cv_bridge/include/cv_bridge/cv_bridge.h b/cv_bridge/include/cv_bridge/cv_bridge.h index 60f8bfabb..5290c4619 100644 --- a/cv_bridge/include/cv_bridge/cv_bridge.h +++ b/cv_bridge/include/cv_bridge/cv_bridge.h @@ -37,297 +37,10 @@ #ifndef CV_BRIDGE__CV_BRIDGE_H_ #define CV_BRIDGE__CV_BRIDGE_H_ -#include -#include -#include -#include -#include -#include -#include +// This file is deprecated as of I-turtle and can be completely removed in J-turtle. -#include -#include -#include -#include +#warning This header is obsolete, please include cv_bridge/cv_bridge.hpp instead -namespace cv_bridge -{ - -class CV_BRIDGE_EXPORT Exception : public std::runtime_error -{ -public: - explicit Exception(const std::string & description) - : std::runtime_error(description) {} -}; - -class CvImage; - -typedef std::shared_ptr CvImagePtr; -typedef std::shared_ptr 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, DIB, - 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 CV_BRIDGE_EXPORT CvImage -{ -public: - std_msgs::msg::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::msg::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::msg::Image message. - * - * The returned sensor_msgs::msg::Image message contains a copy of the image data. - */ - sensor_msgs::msg::Image::SharedPtr 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::msg::CompressedImage::SharedPtr toCompressedImageMsg( - const Format dst_format = - JPG) const; - - /** - * \brief Copy the message data to a ROS sensor_msgs::msg::Image message. - * - * This overload is intended mainly for aggregate messages such as stereo_msgs::DisparityImage, - * which contains a sensor_msgs::msg::Image as a data member. - */ - void toImageMsg(sensor_msgs::msg::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::msg::CompressedImage & ros_image, - const Format dst_format = JPG) const; - - - typedef std::shared_ptr Ptr; - typedef std::shared_ptr ConstPtr; - -protected: - std::shared_ptr tracked_object_; // for sharing ownership - - /// @cond DOXYGEN_IGNORE - friend - CV_BRIDGE_EXPORT CvImageConstPtr toCvShare( - const sensor_msgs::msg::Image & source, - const std::shared_ptr & tracked_object, - const std::string & encoding); - /// @endcond -}; - - -/** - * \brief Convert a sensor_msgs::msg::Image message to an OpenCV-compatible CvImage, copying the - * image data. - * - * \param source A shared_ptr to a sensor_msgs::msg::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. - */ -CV_BRIDGE_EXPORT CvImagePtr toCvCopy( - const sensor_msgs::msg::Image::ConstSharedPtr & source, - const std::string & encoding = std::string()); - -CV_BRIDGE_EXPORT CvImagePtr toCvCopy( - const sensor_msgs::msg::CompressedImage::ConstSharedPtr & source, - const std::string & encoding = std::string()); - -/** - * \brief Convert a sensor_msgs::msg::Image message to an OpenCV-compatible CvImage, copying the - * image data. - * - * \param source A sensor_msgs::msg::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 - */ -CV_BRIDGE_EXPORT CvImagePtr toCvCopy( - const sensor_msgs::msg::Image & source, - const std::string & encoding = std::string()); - -CV_BRIDGE_EXPORT CvImagePtr toCvCopy( - const sensor_msgs::msg::CompressedImage & source, - const std::string & encoding = std::string()); - -/** - * \brief Convert an immutable sensor_msgs::msg::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::msg::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. - */ -CV_BRIDGE_EXPORT CvImageConstPtr toCvShare( - const sensor_msgs::msg::Image::ConstSharedPtr & source, - const std::string & encoding = std::string()); - -/** - * \brief Convert an immutable sensor_msgs::msg::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::msg::Image, and wish to share ownership with the containing message. - * - * \param source The sensor_msgs::msg::Image message - * \param tracked_object A shared_ptr to an object owning the sensor_msgs::msg::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. - */ -CV_BRIDGE_EXPORT CvImageConstPtr toCvShare( - const sensor_msgs::msg::Image & source, - const std::shared_ptr & tracked_object, - const std::string & encoding = std::string()); - -/** - * \brief Convert a CvImage to another encoding using the same rules as toCvCopy - */ -CV_BRIDGE_EXPORT CvImagePtr cvtColor( - const CvImageConstPtr & source, - const std::string & encoding); - -struct CvtColorForDisplayOptions -{ - CvtColorForDisplayOptions() - : do_dynamic_scaling(false), - min_image_value(0.0), - max_image_value(0.0), - colormap(-1), - bg_label(-1) {} - bool do_dynamic_scaling; - double min_image_value; - double max_image_value; - int colormap; - int bg_label; -}; - - -/** - * \brief Converts an immutable sensor_msgs::msg::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::msg::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. it tries to convert to BGR no matter what - * encoding image is passed. - * - if the output encoding is not empty, it must have sensor_msgs::msg::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. - * - if the input encoding is 32SC1, this estimate that image as label image and will convert it as - * bgr image with different colors for each label. - * - * \param source A shared_ptr to a sensor_msgs::msg::Image message - * \param encoding Either an encoding string that returns true in sensor_msgs::msg::image_encodings::isColor - * isMono or the empty string as explained above. - * \param options (cv_bridge::CvtColorForDisplayOptions) Options to convert the source image with. - * - do_dynamic_scaling If true, the image is dynamically scaled between its minimum and maximum value - * before being converted to its final encoding. - * - 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. - * - max_image_value Maximum image value - * - colormap Colormap which the source image converted with. - */ -CV_BRIDGE_EXPORT CvImageConstPtr cvtColorForDisplay( - const CvImageConstPtr & source, - const std::string & encoding = std::string(), - const CvtColorForDisplayOptions options = CvtColorForDisplayOptions()); - -/** - * \brief Get the OpenCV type enum corresponding to the encoding. - * - * For example, "bgr8" -> CV_8UC3, "32FC1" -> CV_32FC1, and "32FC10" -> CV_32FC10. - */ -CV_BRIDGE_EXPORT int getCvType(const std::string & encoding); - -} // namespace cv_bridge +#include #endif // CV_BRIDGE__CV_BRIDGE_H_ diff --git a/cv_bridge/include/cv_bridge/cv_bridge.hpp b/cv_bridge/include/cv_bridge/cv_bridge.hpp new file mode 100644 index 000000000..e3c58d773 --- /dev/null +++ b/cv_bridge/include/cv_bridge/cv_bridge.hpp @@ -0,0 +1,333 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2011, Willow Garage, Inc, +* Copyright (c) 2015, Tal Regev. +* Copyright (c) 2018 Intel Corporation. +* 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_HPP_ +#define CV_BRIDGE__CV_BRIDGE_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace cv_bridge +{ + +class CV_BRIDGE_EXPORT Exception : public std::runtime_error +{ +public: + explicit Exception(const std::string & description) + : std::runtime_error(description) {} +}; + +class CvImage; + +typedef std::shared_ptr CvImagePtr; +typedef std::shared_ptr 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, DIB, + 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 CV_BRIDGE_EXPORT CvImage +{ +public: + std_msgs::msg::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::msg::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::msg::Image message. + * + * The returned sensor_msgs::msg::Image message contains a copy of the image data. + */ + sensor_msgs::msg::Image::SharedPtr 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::msg::CompressedImage::SharedPtr toCompressedImageMsg( + const Format dst_format = + JPG) const; + + /** + * \brief Copy the message data to a ROS sensor_msgs::msg::Image message. + * + * This overload is intended mainly for aggregate messages such as stereo_msgs::DisparityImage, + * which contains a sensor_msgs::msg::Image as a data member. + */ + void toImageMsg(sensor_msgs::msg::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::msg::CompressedImage & ros_image, + const Format dst_format = JPG) const; + + + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; + +protected: + std::shared_ptr tracked_object_; // for sharing ownership + + /// @cond DOXYGEN_IGNORE + friend + CV_BRIDGE_EXPORT CvImageConstPtr toCvShare( + const sensor_msgs::msg::Image & source, + const std::shared_ptr & tracked_object, + const std::string & encoding); + /// @endcond +}; + + +/** + * \brief Convert a sensor_msgs::msg::Image message to an OpenCV-compatible CvImage, copying the + * image data. + * + * \param source A shared_ptr to a sensor_msgs::msg::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. + */ +CV_BRIDGE_EXPORT CvImagePtr toCvCopy( + const sensor_msgs::msg::Image::ConstSharedPtr & source, + const std::string & encoding = std::string()); + +CV_BRIDGE_EXPORT CvImagePtr toCvCopy( + const sensor_msgs::msg::CompressedImage::ConstSharedPtr & source, + const std::string & encoding = std::string()); + +/** + * \brief Convert a sensor_msgs::msg::Image message to an OpenCV-compatible CvImage, copying the + * image data. + * + * \param source A sensor_msgs::msg::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 + */ +CV_BRIDGE_EXPORT CvImagePtr toCvCopy( + const sensor_msgs::msg::Image & source, + const std::string & encoding = std::string()); + +CV_BRIDGE_EXPORT CvImagePtr toCvCopy( + const sensor_msgs::msg::CompressedImage & source, + const std::string & encoding = std::string()); + +/** + * \brief Convert an immutable sensor_msgs::msg::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::msg::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. + */ +CV_BRIDGE_EXPORT CvImageConstPtr toCvShare( + const sensor_msgs::msg::Image::ConstSharedPtr & source, + const std::string & encoding = std::string()); + +/** + * \brief Convert an immutable sensor_msgs::msg::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::msg::Image, and wish to share ownership with the containing message. + * + * \param source The sensor_msgs::msg::Image message + * \param tracked_object A shared_ptr to an object owning the sensor_msgs::msg::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. + */ +CV_BRIDGE_EXPORT CvImageConstPtr toCvShare( + const sensor_msgs::msg::Image & source, + const std::shared_ptr & tracked_object, + const std::string & encoding = std::string()); + +/** + * \brief Convert a CvImage to another encoding using the same rules as toCvCopy + */ +CV_BRIDGE_EXPORT CvImagePtr cvtColor( + const CvImageConstPtr & source, + const std::string & encoding); + +struct CvtColorForDisplayOptions +{ + CvtColorForDisplayOptions() + : do_dynamic_scaling(false), + min_image_value(0.0), + max_image_value(0.0), + colormap(-1), + bg_label(-1) {} + bool do_dynamic_scaling; + double min_image_value; + double max_image_value; + int colormap; + int bg_label; +}; + + +/** + * \brief Converts an immutable sensor_msgs::msg::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::msg::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. it tries to convert to BGR no matter what + * encoding image is passed. + * - if the output encoding is not empty, it must have sensor_msgs::msg::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. + * - if the input encoding is 32SC1, this estimate that image as label image and will convert it as + * bgr image with different colors for each label. + * + * \param source A shared_ptr to a sensor_msgs::msg::Image message + * \param encoding Either an encoding string that returns true in sensor_msgs::msg::image_encodings::isColor + * isMono or the empty string as explained above. + * \param options (cv_bridge::CvtColorForDisplayOptions) Options to convert the source image with. + * - do_dynamic_scaling If true, the image is dynamically scaled between its minimum and maximum value + * before being converted to its final encoding. + * - 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. + * - max_image_value Maximum image value + * - colormap Colormap which the source image converted with. + */ +CV_BRIDGE_EXPORT CvImageConstPtr cvtColorForDisplay( + const CvImageConstPtr & source, + const std::string & encoding = std::string(), + const CvtColorForDisplayOptions options = CvtColorForDisplayOptions()); + +/** + * \brief Get the OpenCV type enum corresponding to the encoding. + * + * For example, "bgr8" -> CV_8UC3, "32FC1" -> CV_32FC1, and "32FC10" -> CV_32FC10. + */ +CV_BRIDGE_EXPORT int getCvType(const std::string & encoding); + +} // namespace cv_bridge + +#endif // CV_BRIDGE__CV_BRIDGE_HPP_ diff --git a/cv_bridge/include/cv_bridge/rgb_colors.h b/cv_bridge/include/cv_bridge/rgb_colors.h index 3441a6ede..dd5b47a20 100644 --- a/cv_bridge/include/cv_bridge/rgb_colors.h +++ b/cv_bridge/include/cv_bridge/rgb_colors.h @@ -36,178 +36,10 @@ #ifndef CV_BRIDGE__RGB_COLORS_H_ #define CV_BRIDGE__RGB_COLORS_H_ -#include -#include +// This file is deprecated as of I-turtle and can be completely removed in J-turtle. +#warning This header is obsolete, please include cv_bridge/rgb_colors.hpp instead -namespace cv_bridge -{ - -namespace rgb_colors -{ - -/** - * @brief - * 146 rgb colors - */ -enum Colors -{ - ALICEBLUE, - ANTIQUEWHITE, - AQUA, - AQUAMARINE, - AZURE, - BEIGE, - BISQUE, - BLACK, - BLANCHEDALMOND, - BLUE, - BLUEVIOLET, - BROWN, - BURLYWOOD, - CADETBLUE, - CHARTREUSE, - CHOCOLATE, - CORAL, - CORNFLOWERBLUE, - CORNSILK, - CRIMSON, - CYAN, - DARKBLUE, - DARKCYAN, - DARKGOLDENROD, - DARKGRAY, - DARKGREEN, - DARKGREY, - DARKKHAKI, - DARKMAGENTA, - DARKOLIVEGREEN, - DARKORANGE, - DARKORCHID, - DARKRED, - DARKSALMON, - DARKSEAGREEN, - DARKSLATEBLUE, - DARKSLATEGRAY, - DARKSLATEGREY, - DARKTURQUOISE, - DARKVIOLET, - DEEPPINK, - DEEPSKYBLUE, - DIMGRAY, - DIMGREY, - DODGERBLUE, - FIREBRICK, - FLORALWHITE, - FORESTGREEN, - FUCHSIA, - GAINSBORO, - GHOSTWHITE, - GOLD, - GOLDENROD, - GRAY, - GREEN, - GREENYELLOW, - GREY, - HONEYDEW, - HOTPINK, - INDIANRED, - INDIGO, - IVORY, - KHAKI, - LAVENDER, - LAVENDERBLUSH, - LAWNGREEN, - LEMONCHIFFON, - LIGHTBLUE, - LIGHTCORAL, - LIGHTCYAN, - LIGHTGOLDENRODYELLOW, - LIGHTGRAY, - LIGHTGREEN, - LIGHTGREY, - LIGHTPINK, - LIGHTSALMON, - LIGHTSEAGREEN, - LIGHTSKYBLUE, - LIGHTSLATEGRAY, - LIGHTSLATEGREY, - LIGHTSTEELBLUE, - LIGHTYELLOW, - LIME, - LIMEGREEN, - LINEN, - MAGENTA, - MAROON, - MEDIUMAQUAMARINE, - MEDIUMBLUE, - MEDIUMORCHID, - MEDIUMPURPLE, - MEDIUMSEAGREEN, - MEDIUMSLATEBLUE, - MEDIUMSPRINGGREEN, - MEDIUMTURQUOISE, - MEDIUMVIOLETRED, - MIDNIGHTBLUE, - MINTCREAM, - MISTYROSE, - MOCCASIN, - NAVAJOWHITE, - NAVY, - OLDLACE, - OLIVE, - OLIVEDRAB, - ORANGE, - ORANGERED, - ORCHID, - PALEGOLDENROD, - PALEGREEN, - PALEVIOLETRED, - PAPAYAWHIP, - PEACHPUFF, - PERU, - PINK, - PLUM, - POWDERBLUE, - PURPLE, - RED, - ROSYBROWN, - ROYALBLUE, - SADDLEBROWN, - SALMON, - SANDYBROWN, - SEAGREEN, - SEASHELL, - SIENNA, - SILVER, - SKYBLUE, - SLATEBLUE, - SLATEGRAY, - SLATEGREY, - SNOW, - SPRINGGREEN, - STEELBLUE, - TAN, - TEAL, - THISTLE, - TOMATO, - TURQUOISE, - VIOLET, - WHEAT, - WHITE, - WHITESMOKE, - YELLOW, - YELLOWGREEN, -}; - -/** - * @brief - * get rgb color with enum. - */ -CV_BRIDGE_EXPORT cv::Vec3d getRGBColor(const int color); - -} // namespace rgb_colors - -} // namespace cv_bridge +#include #endif // CV_BRIDGE__RGB_COLORS_H_ diff --git a/cv_bridge/include/cv_bridge/rgb_colors.hpp b/cv_bridge/include/cv_bridge/rgb_colors.hpp new file mode 100644 index 000000000..b403de97c --- /dev/null +++ b/cv_bridge/include/cv_bridge/rgb_colors.hpp @@ -0,0 +1,213 @@ +/********************************************************************* + * Original color definition is at scikit-image distributed with + * following license disclaimer: + * + * Copyright (C) 2011, the scikit-image team + * Copyright (c) 2018 Intel Corporation. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name of skimage 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 AUTHOR ``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 AUTHOR 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__RGB_COLORS_HPP_ +#define CV_BRIDGE__RGB_COLORS_HPP_ + +#include +#include + + +namespace cv_bridge +{ + +namespace rgb_colors +{ + +/** + * @brief + * 146 rgb colors + */ +enum Colors +{ + ALICEBLUE, + ANTIQUEWHITE, + AQUA, + AQUAMARINE, + AZURE, + BEIGE, + BISQUE, + BLACK, + BLANCHEDALMOND, + BLUE, + BLUEVIOLET, + BROWN, + BURLYWOOD, + CADETBLUE, + CHARTREUSE, + CHOCOLATE, + CORAL, + CORNFLOWERBLUE, + CORNSILK, + CRIMSON, + CYAN, + DARKBLUE, + DARKCYAN, + DARKGOLDENROD, + DARKGRAY, + DARKGREEN, + DARKGREY, + DARKKHAKI, + DARKMAGENTA, + DARKOLIVEGREEN, + DARKORANGE, + DARKORCHID, + DARKRED, + DARKSALMON, + DARKSEAGREEN, + DARKSLATEBLUE, + DARKSLATEGRAY, + DARKSLATEGREY, + DARKTURQUOISE, + DARKVIOLET, + DEEPPINK, + DEEPSKYBLUE, + DIMGRAY, + DIMGREY, + DODGERBLUE, + FIREBRICK, + FLORALWHITE, + FORESTGREEN, + FUCHSIA, + GAINSBORO, + GHOSTWHITE, + GOLD, + GOLDENROD, + GRAY, + GREEN, + GREENYELLOW, + GREY, + HONEYDEW, + HOTPINK, + INDIANRED, + INDIGO, + IVORY, + KHAKI, + LAVENDER, + LAVENDERBLUSH, + LAWNGREEN, + LEMONCHIFFON, + LIGHTBLUE, + LIGHTCORAL, + LIGHTCYAN, + LIGHTGOLDENRODYELLOW, + LIGHTGRAY, + LIGHTGREEN, + LIGHTGREY, + LIGHTPINK, + LIGHTSALMON, + LIGHTSEAGREEN, + LIGHTSKYBLUE, + LIGHTSLATEGRAY, + LIGHTSLATEGREY, + LIGHTSTEELBLUE, + LIGHTYELLOW, + LIME, + LIMEGREEN, + LINEN, + MAGENTA, + MAROON, + MEDIUMAQUAMARINE, + MEDIUMBLUE, + MEDIUMORCHID, + MEDIUMPURPLE, + MEDIUMSEAGREEN, + MEDIUMSLATEBLUE, + MEDIUMSPRINGGREEN, + MEDIUMTURQUOISE, + MEDIUMVIOLETRED, + MIDNIGHTBLUE, + MINTCREAM, + MISTYROSE, + MOCCASIN, + NAVAJOWHITE, + NAVY, + OLDLACE, + OLIVE, + OLIVEDRAB, + ORANGE, + ORANGERED, + ORCHID, + PALEGOLDENROD, + PALEGREEN, + PALEVIOLETRED, + PAPAYAWHIP, + PEACHPUFF, + PERU, + PINK, + PLUM, + POWDERBLUE, + PURPLE, + RED, + ROSYBROWN, + ROYALBLUE, + SADDLEBROWN, + SALMON, + SANDYBROWN, + SEAGREEN, + SEASHELL, + SIENNA, + SILVER, + SKYBLUE, + SLATEBLUE, + SLATEGRAY, + SLATEGREY, + SNOW, + SPRINGGREEN, + STEELBLUE, + TAN, + TEAL, + THISTLE, + TOMATO, + TURQUOISE, + VIOLET, + WHEAT, + WHITE, + WHITESMOKE, + YELLOW, + YELLOWGREEN, +}; + +/** + * @brief + * get rgb color with enum. + */ +CV_BRIDGE_EXPORT cv::Vec3d getRGBColor(const int color); + +} // namespace rgb_colors + +} // namespace cv_bridge + +#endif // CV_BRIDGE__RGB_COLORS_HPP_ diff --git a/cv_bridge/src/cv_bridge.cpp b/cv_bridge/src/cv_bridge.cpp index 0a2e4f577..669b2f5f3 100644 --- a/cv_bridge/src/cv_bridge.cpp +++ b/cv_bridge/src/cv_bridge.cpp @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include +#include +#include #include #include #include diff --git a/cv_bridge/src/module.hpp b/cv_bridge/src/module.hpp index 9d90467ef..3da45432c 100644 --- a/cv_bridge/src/module.hpp +++ b/cv_bridge/src/module.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION diff --git a/cv_bridge/src/rgb_colors.cpp b/cv_bridge/src/rgb_colors.cpp index c0e74e0f9..dd5ba84a3 100644 --- a/cv_bridge/src/rgb_colors.cpp +++ b/cv_bridge/src/rgb_colors.cpp @@ -33,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include "cv_bridge/rgb_colors.h" +#include "cv_bridge/rgb_colors.hpp" #include diff --git a/cv_bridge/test/test_compression.cpp b/cv_bridge/test/test_compression.cpp index 962f9fd46..f299468f4 100644 --- a/cv_bridge/test/test_compression.cpp +++ b/cv_bridge/test/test_compression.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include diff --git a/cv_bridge/test/test_endian.cpp b/cv_bridge/test/test_endian.cpp index 270105413..088091822 100644 --- a/cv_bridge/test/test_endian.cpp +++ b/cv_bridge/test/test_endian.cpp @@ -1,5 +1,5 @@ #include -#include +#include #include #include diff --git a/cv_bridge/test/test_rgb_colors.cpp b/cv_bridge/test/test_rgb_colors.cpp index aa7bdfbe4..e8de3dfae 100644 --- a/cv_bridge/test/test_rgb_colors.cpp +++ b/cv_bridge/test/test_rgb_colors.cpp @@ -1,7 +1,7 @@ #include #include -#include "cv_bridge/rgb_colors.h" +#include "cv_bridge/rgb_colors.hpp" TEST(RGBColors, testGetRGBColor) { diff --git a/cv_bridge/test/utest.cpp b/cv_bridge/test/utest.cpp index 3d2a35a1a..4ef3a091d 100644 --- a/cv_bridge/test/utest.cpp +++ b/cv_bridge/test/utest.cpp @@ -1,7 +1,7 @@ #include #include -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" // Tests conversion of non-continuous cv::Mat. #5206 TEST(CvBridgeTest, NonContinuous) diff --git a/cv_bridge/test/utest2.cpp b/cv_bridge/test/utest2.cpp index 6fdac3556..7121b04b4 100644 --- a/cv_bridge/test/utest2.cpp +++ b/cv_bridge/test/utest2.cpp @@ -40,7 +40,7 @@ #include #include -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" #include "opencv2/core/core.hpp" using namespace sensor_msgs::image_encodings; diff --git a/image_geometry/include/image_geometry/pinhole_camera_model.h b/image_geometry/include/image_geometry/pinhole_camera_model.h index fdbecd7d5..0bc882e76 100644 --- a/image_geometry/include/image_geometry/pinhole_camera_model.h +++ b/image_geometry/include/image_geometry/pinhole_camera_model.h @@ -1,409 +1,10 @@ #ifndef IMAGE_GEOMETRY__PINHOLE_CAMERA_MODEL_H #define IMAGE_GEOMETRY__PINHOLE_CAMERA_MODEL_H -#include "image_geometry/visibility_control.hpp" +// This file is deprecated as of I-turtle and can be completely removed in J-turtle. -#include -#include -#include -#include -#include -#include +#warning This header is obsolete, please include image_geometry/pinhole_camera_model.hpp instead -namespace image_geometry { - -class Exception : public std::runtime_error -{ -public: - Exception(const std::string& description) : std::runtime_error(description) {} -}; - -/** - * \brief Simplifies interpreting images geometrically using the parameters from - * sensor_msgs/CameraInfo. - */ -class PinholeCameraModel -{ -public: - IMAGE_GEOMETRY_PUBLIC - PinholeCameraModel(); - - IMAGE_GEOMETRY_PUBLIC - PinholeCameraModel(const PinholeCameraModel& other); - - IMAGE_GEOMETRY_PUBLIC - PinholeCameraModel& operator=(const PinholeCameraModel& other); - - /** - * \brief Set the camera parameters from the sensor_msgs/CameraInfo message. - */ - IMAGE_GEOMETRY_PUBLIC - bool fromCameraInfo(const sensor_msgs::msg::CameraInfo& msg); - - /** - * \brief Set the camera parameters from the sensor_msgs/CameraInfo message. - */ - IMAGE_GEOMETRY_PUBLIC - bool fromCameraInfo(const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg); - - /** - * \brief Get the name of the camera coordinate frame in tf. - */ - IMAGE_GEOMETRY_PUBLIC - std::string tfFrame() const; - - /** - * \brief Get the time stamp associated with this camera model. - */ - IMAGE_GEOMETRY_PUBLIC - builtin_interfaces::msg::Time stamp() const; - - /** - * \brief The resolution at which the camera was calibrated. - * - * The maximum resolution at which the camera can be used with the current - * calibration; normally this is the same as the imager resolution. - */ - IMAGE_GEOMETRY_PUBLIC - cv::Size fullResolution() const; - - /** - * \brief The resolution of the current rectified image. - * - * The size of the rectified image associated with the latest CameraInfo, as - * reduced by binning/ROI and affected by distortion. If binning and ROI are - * not in use, this is the same as fullResolution(). - */ - IMAGE_GEOMETRY_PUBLIC - cv::Size reducedResolution() const; - - IMAGE_GEOMETRY_PUBLIC - cv::Point2d toFullResolution(const cv::Point2d& uv_reduced) const; - - IMAGE_GEOMETRY_PUBLIC - cv::Rect toFullResolution(const cv::Rect& roi_reduced) const; - - IMAGE_GEOMETRY_PUBLIC - cv::Point2d toReducedResolution(const cv::Point2d& uv_full) const; - - IMAGE_GEOMETRY_PUBLIC - cv::Rect toReducedResolution(const cv::Rect& roi_full) const; - - /** - * \brief The current raw ROI, as used for capture by the camera driver. - */ - IMAGE_GEOMETRY_PUBLIC - cv::Rect rawRoi() const; - - /** - * \brief The current rectified ROI, which best fits the raw ROI. - */ - IMAGE_GEOMETRY_PUBLIC - cv::Rect rectifiedRoi() const; - - /** - * \brief Project a 3d point to rectified pixel coordinates. - * - * This is the inverse of projectPixelTo3dRay(). - * - * \param xyz 3d point in the camera coordinate frame - * \return (u,v) in rectified pixel coordinates - */ - IMAGE_GEOMETRY_PUBLIC - cv::Point2d project3dToPixel(const cv::Point3d& xyz) const; - - /** - * \brief Project a rectified pixel to a 3d ray. - * - * Returns the unit vector in the camera coordinate frame in the direction of rectified - * pixel (u,v) in the image plane. This is the inverse of project3dToPixel(). - * - * In 1.4.x, the vector has z = 1.0. Previously, this function returned a unit vector. - * - * \param uv_rect Rectified pixel coordinates - * \return 3d ray passing through (u,v) - */ - IMAGE_GEOMETRY_PUBLIC - cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect) const; - cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect, const cv::Matx34d& P) const; - - /** - * \brief Rectify a raw camera image. - */ - IMAGE_GEOMETRY_PUBLIC - void rectifyImage(const cv::Mat& raw, cv::Mat& rectified, - int interpolation = cv::INTER_LINEAR) const; - - /** - * \brief Apply camera distortion to a rectified image. - */ - IMAGE_GEOMETRY_PUBLIC - void unrectifyImage(const cv::Mat& rectified, cv::Mat& raw, - int interpolation = cv::INTER_LINEAR) const; - - /** - * \brief Compute the rectified image coordinates of a pixel in the raw image. - */ - IMAGE_GEOMETRY_PUBLIC - cv::Point2d rectifyPoint(const cv::Point2d& uv_raw) const; - cv::Point2d rectifyPoint(const cv::Point2d& uv_raw, const cv::Matx33d& K, const cv::Matx34d& P) const; - - /** - * \brief Compute the raw image coordinates of a pixel in the rectified image. - */ - IMAGE_GEOMETRY_PUBLIC - cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect) const; - cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect, const cv::Matx33d& K, const cv::Matx34d& P) const; - - /** - * \brief Compute the rectified ROI best fitting a raw ROI. - */ - IMAGE_GEOMETRY_PUBLIC - cv::Rect rectifyRoi(const cv::Rect& roi_raw) const; - - /** - * \brief Compute the raw ROI best fitting a rectified ROI. - */ - IMAGE_GEOMETRY_PUBLIC - cv::Rect unrectifyRoi(const cv::Rect& roi_rect) const; - - /** - * \brief Returns the camera info message held internally - */ - IMAGE_GEOMETRY_PUBLIC - const sensor_msgs::msg::CameraInfo& cameraInfo() const; - - /** - * \brief Returns the original camera matrix. - */ - IMAGE_GEOMETRY_PUBLIC - const cv::Matx33d& intrinsicMatrix() const; - - /** - * \brief Returns the distortion coefficients. - */ - IMAGE_GEOMETRY_PUBLIC - const cv::Mat_& distortionCoeffs() const; - - /** - * \brief Returns the rotation matrix. - */ - IMAGE_GEOMETRY_PUBLIC - const cv::Matx33d& rotationMatrix() const; - - /** - * \brief Returns the projection matrix. - */ - IMAGE_GEOMETRY_PUBLIC - const cv::Matx34d& projectionMatrix() const; - - /** - * \brief Returns the original camera matrix for full resolution. - */ - IMAGE_GEOMETRY_PUBLIC - const cv::Matx33d& fullIntrinsicMatrix() const; - - /** - * \brief Returns the projection matrix for full resolution. - */ - IMAGE_GEOMETRY_PUBLIC - const cv::Matx34d& fullProjectionMatrix() const; - - /** - * \brief Returns the focal length (pixels) in x direction of the rectified image. - */ - IMAGE_GEOMETRY_PUBLIC - double fx() const; - - /** - * \brief Returns the focal length (pixels) in y direction of the rectified image. - */ - IMAGE_GEOMETRY_PUBLIC - double fy() const; - - /** - * \brief Returns the x coordinate of the optical center. - */ - IMAGE_GEOMETRY_PUBLIC - double cx() const; - - /** - * \brief Returns the y coordinate of the optical center. - */ - IMAGE_GEOMETRY_PUBLIC - double cy() const; - - /** - * \brief Returns the x-translation term of the projection matrix. - */ - IMAGE_GEOMETRY_PUBLIC - double Tx() const; - - /** - * \brief Returns the y-translation term of the projection matrix. - */ - IMAGE_GEOMETRY_PUBLIC - double Ty() const; - - /** - * \brief Returns the number of columns in each bin. - */ - IMAGE_GEOMETRY_PUBLIC - uint32_t binningX() const; - - /** - * \brief Returns the number of rows in each bin. - */ - IMAGE_GEOMETRY_PUBLIC - uint32_t binningY() const; - - /** - * \brief Compute delta u, given Z and delta X in Cartesian space. - * - * For given Z, this is the inverse of getDeltaX(). - * - * \param deltaX Delta X, in Cartesian space - * \param Z Z (depth), in Cartesian space - */ - IMAGE_GEOMETRY_PUBLIC - double getDeltaU(double deltaX, double Z) const; - - /** - * \brief Compute delta v, given Z and delta Y in Cartesian space. - * - * For given Z, this is the inverse of getDeltaY(). - * - * \param deltaY Delta Y, in Cartesian space - * \param Z Z (depth), in Cartesian space - */ - IMAGE_GEOMETRY_PUBLIC - double getDeltaV(double deltaY, double Z) const; - - /** - * \brief Compute delta X, given Z in Cartesian space and delta u in pixels. - * - * For given Z, this is the inverse of getDeltaU(). - * - * \param deltaU Delta u, in pixels - * \param Z Z (depth), in Cartesian space - */ - IMAGE_GEOMETRY_PUBLIC - double getDeltaX(double deltaU, double Z) const; - - /** - * \brief Compute delta Y, given Z in Cartesian space and delta v in pixels. - * - * For given Z, this is the inverse of getDeltaV(). - * - * \param deltaV Delta v, in pixels - * \param Z Z (depth), in Cartesian space - */ - IMAGE_GEOMETRY_PUBLIC - double getDeltaY(double deltaV, double Z) const; - - /** - * \brief Returns true if the camera has been initialized - */ - IMAGE_GEOMETRY_PUBLIC - bool initialized() const { return (bool)cache_; } - -protected: - sensor_msgs::msg::CameraInfo cam_info_; - cv::Mat_ D_; // Unaffected by binning, ROI - cv::Matx33d R_; // Unaffected by binning, ROI - cv::Matx33d K_; // Describe current image (includes binning, ROI) - cv::Matx34d P_; // Describe current image (includes binning, ROI) - cv::Matx33d K_full_; // Describe full-res image, needed for full maps - cv::Matx34d P_full_; // Describe full-res image, needed for full maps - - // Use PIMPL here so we can change internals in patch updates if needed - struct Cache; - std::shared_ptr cache_; // Holds cached data for internal use - - IMAGE_GEOMETRY_PUBLIC - void initRectificationMaps() const; - void initUnrectificationMaps() const; - - friend class StereoCameraModel; -}; - - -/* Trivial inline functions */ -IMAGE_GEOMETRY_PUBLIC -inline std::string PinholeCameraModel::tfFrame() const -{ - assert( initialized() ); - return cam_info_.header.frame_id; -} - -IMAGE_GEOMETRY_PUBLIC -inline builtin_interfaces::msg::Time PinholeCameraModel::stamp() const -{ - assert( initialized() ); - return cam_info_.header.stamp; -} - -IMAGE_GEOMETRY_PUBLIC -inline const sensor_msgs::msg::CameraInfo& PinholeCameraModel::cameraInfo() const { return cam_info_; } -IMAGE_GEOMETRY_PUBLIC -inline const cv::Matx33d& PinholeCameraModel::intrinsicMatrix() const { return K_; } -IMAGE_GEOMETRY_PUBLIC -inline const cv::Mat_& PinholeCameraModel::distortionCoeffs() const { return D_; } -IMAGE_GEOMETRY_PUBLIC -inline const cv::Matx33d& PinholeCameraModel::rotationMatrix() const { return R_; } -IMAGE_GEOMETRY_PUBLIC -inline const cv::Matx34d& PinholeCameraModel::projectionMatrix() const { return P_; } -IMAGE_GEOMETRY_PUBLIC -inline const cv::Matx33d& PinholeCameraModel::fullIntrinsicMatrix() const { return K_full_; } -IMAGE_GEOMETRY_PUBLIC -inline const cv::Matx34d& PinholeCameraModel::fullProjectionMatrix() const { return P_full_; } - -IMAGE_GEOMETRY_PUBLIC -inline double PinholeCameraModel::fx() const { return P_(0,0); } -IMAGE_GEOMETRY_PUBLIC -inline double PinholeCameraModel::fy() const { return P_(1,1); } -IMAGE_GEOMETRY_PUBLIC -inline double PinholeCameraModel::cx() const { return P_(0,2); } -IMAGE_GEOMETRY_PUBLIC -inline double PinholeCameraModel::cy() const { return P_(1,2); } -IMAGE_GEOMETRY_PUBLIC -inline double PinholeCameraModel::Tx() const { return P_(0,3); } -IMAGE_GEOMETRY_PUBLIC -inline double PinholeCameraModel::Ty() const { return P_(1,3); } - -IMAGE_GEOMETRY_PUBLIC -inline uint32_t PinholeCameraModel::binningX() const { return cam_info_.binning_x; } -IMAGE_GEOMETRY_PUBLIC -inline uint32_t PinholeCameraModel::binningY() const { return cam_info_.binning_y; } - -IMAGE_GEOMETRY_PUBLIC -inline double PinholeCameraModel::getDeltaU(double deltaX, double Z) const -{ - assert( initialized() ); - return fx() * deltaX / Z; -} - -IMAGE_GEOMETRY_PUBLIC -inline double PinholeCameraModel::getDeltaV(double deltaY, double Z) const -{ - assert( initialized() ); - return fy() * deltaY / Z; -} - -IMAGE_GEOMETRY_PUBLIC -inline double PinholeCameraModel::getDeltaX(double deltaU, double Z) const -{ - assert( initialized() ); - return Z * deltaU / fx(); -} - -IMAGE_GEOMETRY_PUBLIC -inline double PinholeCameraModel::getDeltaY(double deltaV, double Z) const -{ - assert( initialized() ); - return Z * deltaV / fy(); -} - -} // namespace image_geometry +#include #endif diff --git a/image_geometry/include/image_geometry/pinhole_camera_model.hpp b/image_geometry/include/image_geometry/pinhole_camera_model.hpp new file mode 100644 index 000000000..3b3601969 --- /dev/null +++ b/image_geometry/include/image_geometry/pinhole_camera_model.hpp @@ -0,0 +1,409 @@ +#ifndef IMAGE_GEOMETRY__PINHOLE_CAMERA_MODEL_HPP_ +#define IMAGE_GEOMETRY__PINHOLE_CAMERA_MODEL_HPP_ + +#include "image_geometry/visibility_control.hpp" + +#include +#include +#include +#include +#include +#include + +namespace image_geometry { + +class Exception : public std::runtime_error +{ +public: + Exception(const std::string& description) : std::runtime_error(description) {} +}; + +/** + * \brief Simplifies interpreting images geometrically using the parameters from + * sensor_msgs/CameraInfo. + */ +class PinholeCameraModel +{ +public: + IMAGE_GEOMETRY_PUBLIC + PinholeCameraModel(); + + IMAGE_GEOMETRY_PUBLIC + PinholeCameraModel(const PinholeCameraModel& other); + + IMAGE_GEOMETRY_PUBLIC + PinholeCameraModel& operator=(const PinholeCameraModel& other); + + /** + * \brief Set the camera parameters from the sensor_msgs/CameraInfo message. + */ + IMAGE_GEOMETRY_PUBLIC + bool fromCameraInfo(const sensor_msgs::msg::CameraInfo& msg); + + /** + * \brief Set the camera parameters from the sensor_msgs/CameraInfo message. + */ + IMAGE_GEOMETRY_PUBLIC + bool fromCameraInfo(const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg); + + /** + * \brief Get the name of the camera coordinate frame in tf. + */ + IMAGE_GEOMETRY_PUBLIC + std::string tfFrame() const; + + /** + * \brief Get the time stamp associated with this camera model. + */ + IMAGE_GEOMETRY_PUBLIC + builtin_interfaces::msg::Time stamp() const; + + /** + * \brief The resolution at which the camera was calibrated. + * + * The maximum resolution at which the camera can be used with the current + * calibration; normally this is the same as the imager resolution. + */ + IMAGE_GEOMETRY_PUBLIC + cv::Size fullResolution() const; + + /** + * \brief The resolution of the current rectified image. + * + * The size of the rectified image associated with the latest CameraInfo, as + * reduced by binning/ROI and affected by distortion. If binning and ROI are + * not in use, this is the same as fullResolution(). + */ + IMAGE_GEOMETRY_PUBLIC + cv::Size reducedResolution() const; + + IMAGE_GEOMETRY_PUBLIC + cv::Point2d toFullResolution(const cv::Point2d& uv_reduced) const; + + IMAGE_GEOMETRY_PUBLIC + cv::Rect toFullResolution(const cv::Rect& roi_reduced) const; + + IMAGE_GEOMETRY_PUBLIC + cv::Point2d toReducedResolution(const cv::Point2d& uv_full) const; + + IMAGE_GEOMETRY_PUBLIC + cv::Rect toReducedResolution(const cv::Rect& roi_full) const; + + /** + * \brief The current raw ROI, as used for capture by the camera driver. + */ + IMAGE_GEOMETRY_PUBLIC + cv::Rect rawRoi() const; + + /** + * \brief The current rectified ROI, which best fits the raw ROI. + */ + IMAGE_GEOMETRY_PUBLIC + cv::Rect rectifiedRoi() const; + + /** + * \brief Project a 3d point to rectified pixel coordinates. + * + * This is the inverse of projectPixelTo3dRay(). + * + * \param xyz 3d point in the camera coordinate frame + * \return (u,v) in rectified pixel coordinates + */ + IMAGE_GEOMETRY_PUBLIC + cv::Point2d project3dToPixel(const cv::Point3d& xyz) const; + + /** + * \brief Project a rectified pixel to a 3d ray. + * + * Returns the unit vector in the camera coordinate frame in the direction of rectified + * pixel (u,v) in the image plane. This is the inverse of project3dToPixel(). + * + * In 1.4.x, the vector has z = 1.0. Previously, this function returned a unit vector. + * + * \param uv_rect Rectified pixel coordinates + * \return 3d ray passing through (u,v) + */ + IMAGE_GEOMETRY_PUBLIC + cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect) const; + cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect, const cv::Matx34d& P) const; + + /** + * \brief Rectify a raw camera image. + */ + IMAGE_GEOMETRY_PUBLIC + void rectifyImage(const cv::Mat& raw, cv::Mat& rectified, + int interpolation = cv::INTER_LINEAR) const; + + /** + * \brief Apply camera distortion to a rectified image. + */ + IMAGE_GEOMETRY_PUBLIC + void unrectifyImage(const cv::Mat& rectified, cv::Mat& raw, + int interpolation = cv::INTER_LINEAR) const; + + /** + * \brief Compute the rectified image coordinates of a pixel in the raw image. + */ + IMAGE_GEOMETRY_PUBLIC + cv::Point2d rectifyPoint(const cv::Point2d& uv_raw) const; + cv::Point2d rectifyPoint(const cv::Point2d& uv_raw, const cv::Matx33d& K, const cv::Matx34d& P) const; + + /** + * \brief Compute the raw image coordinates of a pixel in the rectified image. + */ + IMAGE_GEOMETRY_PUBLIC + cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect) const; + cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect, const cv::Matx33d& K, const cv::Matx34d& P) const; + + /** + * \brief Compute the rectified ROI best fitting a raw ROI. + */ + IMAGE_GEOMETRY_PUBLIC + cv::Rect rectifyRoi(const cv::Rect& roi_raw) const; + + /** + * \brief Compute the raw ROI best fitting a rectified ROI. + */ + IMAGE_GEOMETRY_PUBLIC + cv::Rect unrectifyRoi(const cv::Rect& roi_rect) const; + + /** + * \brief Returns the camera info message held internally + */ + IMAGE_GEOMETRY_PUBLIC + const sensor_msgs::msg::CameraInfo& cameraInfo() const; + + /** + * \brief Returns the original camera matrix. + */ + IMAGE_GEOMETRY_PUBLIC + const cv::Matx33d& intrinsicMatrix() const; + + /** + * \brief Returns the distortion coefficients. + */ + IMAGE_GEOMETRY_PUBLIC + const cv::Mat_& distortionCoeffs() const; + + /** + * \brief Returns the rotation matrix. + */ + IMAGE_GEOMETRY_PUBLIC + const cv::Matx33d& rotationMatrix() const; + + /** + * \brief Returns the projection matrix. + */ + IMAGE_GEOMETRY_PUBLIC + const cv::Matx34d& projectionMatrix() const; + + /** + * \brief Returns the original camera matrix for full resolution. + */ + IMAGE_GEOMETRY_PUBLIC + const cv::Matx33d& fullIntrinsicMatrix() const; + + /** + * \brief Returns the projection matrix for full resolution. + */ + IMAGE_GEOMETRY_PUBLIC + const cv::Matx34d& fullProjectionMatrix() const; + + /** + * \brief Returns the focal length (pixels) in x direction of the rectified image. + */ + IMAGE_GEOMETRY_PUBLIC + double fx() const; + + /** + * \brief Returns the focal length (pixels) in y direction of the rectified image. + */ + IMAGE_GEOMETRY_PUBLIC + double fy() const; + + /** + * \brief Returns the x coordinate of the optical center. + */ + IMAGE_GEOMETRY_PUBLIC + double cx() const; + + /** + * \brief Returns the y coordinate of the optical center. + */ + IMAGE_GEOMETRY_PUBLIC + double cy() const; + + /** + * \brief Returns the x-translation term of the projection matrix. + */ + IMAGE_GEOMETRY_PUBLIC + double Tx() const; + + /** + * \brief Returns the y-translation term of the projection matrix. + */ + IMAGE_GEOMETRY_PUBLIC + double Ty() const; + + /** + * \brief Returns the number of columns in each bin. + */ + IMAGE_GEOMETRY_PUBLIC + uint32_t binningX() const; + + /** + * \brief Returns the number of rows in each bin. + */ + IMAGE_GEOMETRY_PUBLIC + uint32_t binningY() const; + + /** + * \brief Compute delta u, given Z and delta X in Cartesian space. + * + * For given Z, this is the inverse of getDeltaX(). + * + * \param deltaX Delta X, in Cartesian space + * \param Z Z (depth), in Cartesian space + */ + IMAGE_GEOMETRY_PUBLIC + double getDeltaU(double deltaX, double Z) const; + + /** + * \brief Compute delta v, given Z and delta Y in Cartesian space. + * + * For given Z, this is the inverse of getDeltaY(). + * + * \param deltaY Delta Y, in Cartesian space + * \param Z Z (depth), in Cartesian space + */ + IMAGE_GEOMETRY_PUBLIC + double getDeltaV(double deltaY, double Z) const; + + /** + * \brief Compute delta X, given Z in Cartesian space and delta u in pixels. + * + * For given Z, this is the inverse of getDeltaU(). + * + * \param deltaU Delta u, in pixels + * \param Z Z (depth), in Cartesian space + */ + IMAGE_GEOMETRY_PUBLIC + double getDeltaX(double deltaU, double Z) const; + + /** + * \brief Compute delta Y, given Z in Cartesian space and delta v in pixels. + * + * For given Z, this is the inverse of getDeltaV(). + * + * \param deltaV Delta v, in pixels + * \param Z Z (depth), in Cartesian space + */ + IMAGE_GEOMETRY_PUBLIC + double getDeltaY(double deltaV, double Z) const; + + /** + * \brief Returns true if the camera has been initialized + */ + IMAGE_GEOMETRY_PUBLIC + bool initialized() const { return (bool)cache_; } + +protected: + sensor_msgs::msg::CameraInfo cam_info_; + cv::Mat_ D_; // Unaffected by binning, ROI + cv::Matx33d R_; // Unaffected by binning, ROI + cv::Matx33d K_; // Describe current image (includes binning, ROI) + cv::Matx34d P_; // Describe current image (includes binning, ROI) + cv::Matx33d K_full_; // Describe full-res image, needed for full maps + cv::Matx34d P_full_; // Describe full-res image, needed for full maps + + // Use PIMPL here so we can change internals in patch updates if needed + struct Cache; + std::shared_ptr cache_; // Holds cached data for internal use + + IMAGE_GEOMETRY_PUBLIC + void initRectificationMaps() const; + void initUnrectificationMaps() const; + + friend class StereoCameraModel; +}; + + +/* Trivial inline functions */ +IMAGE_GEOMETRY_PUBLIC +inline std::string PinholeCameraModel::tfFrame() const +{ + assert( initialized() ); + return cam_info_.header.frame_id; +} + +IMAGE_GEOMETRY_PUBLIC +inline builtin_interfaces::msg::Time PinholeCameraModel::stamp() const +{ + assert( initialized() ); + return cam_info_.header.stamp; +} + +IMAGE_GEOMETRY_PUBLIC +inline const sensor_msgs::msg::CameraInfo& PinholeCameraModel::cameraInfo() const { return cam_info_; } +IMAGE_GEOMETRY_PUBLIC +inline const cv::Matx33d& PinholeCameraModel::intrinsicMatrix() const { return K_; } +IMAGE_GEOMETRY_PUBLIC +inline const cv::Mat_& PinholeCameraModel::distortionCoeffs() const { return D_; } +IMAGE_GEOMETRY_PUBLIC +inline const cv::Matx33d& PinholeCameraModel::rotationMatrix() const { return R_; } +IMAGE_GEOMETRY_PUBLIC +inline const cv::Matx34d& PinholeCameraModel::projectionMatrix() const { return P_; } +IMAGE_GEOMETRY_PUBLIC +inline const cv::Matx33d& PinholeCameraModel::fullIntrinsicMatrix() const { return K_full_; } +IMAGE_GEOMETRY_PUBLIC +inline const cv::Matx34d& PinholeCameraModel::fullProjectionMatrix() const { return P_full_; } + +IMAGE_GEOMETRY_PUBLIC +inline double PinholeCameraModel::fx() const { return P_(0,0); } +IMAGE_GEOMETRY_PUBLIC +inline double PinholeCameraModel::fy() const { return P_(1,1); } +IMAGE_GEOMETRY_PUBLIC +inline double PinholeCameraModel::cx() const { return P_(0,2); } +IMAGE_GEOMETRY_PUBLIC +inline double PinholeCameraModel::cy() const { return P_(1,2); } +IMAGE_GEOMETRY_PUBLIC +inline double PinholeCameraModel::Tx() const { return P_(0,3); } +IMAGE_GEOMETRY_PUBLIC +inline double PinholeCameraModel::Ty() const { return P_(1,3); } + +IMAGE_GEOMETRY_PUBLIC +inline uint32_t PinholeCameraModel::binningX() const { return cam_info_.binning_x; } +IMAGE_GEOMETRY_PUBLIC +inline uint32_t PinholeCameraModel::binningY() const { return cam_info_.binning_y; } + +IMAGE_GEOMETRY_PUBLIC +inline double PinholeCameraModel::getDeltaU(double deltaX, double Z) const +{ + assert( initialized() ); + return fx() * deltaX / Z; +} + +IMAGE_GEOMETRY_PUBLIC +inline double PinholeCameraModel::getDeltaV(double deltaY, double Z) const +{ + assert( initialized() ); + return fy() * deltaY / Z; +} + +IMAGE_GEOMETRY_PUBLIC +inline double PinholeCameraModel::getDeltaX(double deltaU, double Z) const +{ + assert( initialized() ); + return Z * deltaU / fx(); +} + +IMAGE_GEOMETRY_PUBLIC +inline double PinholeCameraModel::getDeltaY(double deltaV, double Z) const +{ + assert( initialized() ); + return Z * deltaV / fy(); +} + +} // namespace image_geometry + +#endif // IMAGE_GEOMETRY__PINHOLE_CAMERA_MODEL_HPP_ diff --git a/image_geometry/include/image_geometry/stereo_camera_model.h b/image_geometry/include/image_geometry/stereo_camera_model.h index 83a081fcf..419945c4b 100644 --- a/image_geometry/include/image_geometry/stereo_camera_model.h +++ b/image_geometry/include/image_geometry/stereo_camera_model.h @@ -1,155 +1,10 @@ #ifndef IMAGE_GEOMETRY__STEREO_CAMERA_MODEL_H #define IMAGE_GEOMETRY__STEREO_CAMERA_MODEL_H -#include "image_geometry/pinhole_camera_model.h" -#include "image_geometry/visibility_control.hpp" +// This file is deprecated as of I-turtle and can be completely removed in J-turtle. -namespace image_geometry { +#warning This header is obsolete, please include image_geometry/stereo_camera_model.hpp instead -/** - * \brief Simplifies interpreting stereo image pairs geometrically using the - * parameters from the left and right sensor_msgs/CameraInfo. - */ -class StereoCameraModel -{ -public: - IMAGE_GEOMETRY_PUBLIC - StereoCameraModel(); - - IMAGE_GEOMETRY_PUBLIC - StereoCameraModel(const StereoCameraModel& other); - - IMAGE_GEOMETRY_PUBLIC - StereoCameraModel& operator=(const StereoCameraModel& other); - - /** - * \brief Set the camera parameters from the sensor_msgs/CameraInfo messages. - */ - IMAGE_GEOMETRY_PUBLIC - bool fromCameraInfo(const sensor_msgs::msg::CameraInfo& left, - const sensor_msgs::msg::CameraInfo& right); - - /** - * \brief Set the camera parameters from the sensor_msgs/CameraInfo messages. - */ - IMAGE_GEOMETRY_PUBLIC - bool fromCameraInfo(const sensor_msgs::msg::CameraInfo::ConstSharedPtr& left, - const sensor_msgs::msg::CameraInfo::ConstSharedPtr& right); - - /** - * \brief Get the left monocular camera model. - */ - IMAGE_GEOMETRY_PUBLIC - const PinholeCameraModel& left() const; - - /** - * \brief Get the right monocular camera model. - */ - IMAGE_GEOMETRY_PUBLIC - const PinholeCameraModel& right() const; - - /** - * \brief Get the name of the camera coordinate frame in tf. - * - * For stereo cameras, both the left and right CameraInfo should be in the left - * optical frame. - */ - IMAGE_GEOMETRY_PUBLIC - std::string tfFrame() const; - - /** - * \brief Project a rectified pixel with disparity to a 3d point. - */ - IMAGE_GEOMETRY_PUBLIC - void projectDisparityTo3d(const cv::Point2d& left_uv_rect, float disparity, cv::Point3d& xyz) const; - - /** - * \brief Project a disparity image to a 3d point cloud. - * - * If handleMissingValues = true, all points with minimal disparity (outliers) have - * Z set to MISSING_Z (currently 10000.0). - */ - IMAGE_GEOMETRY_PUBLIC - void projectDisparityImageTo3d(const cv::Mat& disparity, cv::Mat& point_cloud, - bool handleMissingValues = false) const; - IMAGE_GEOMETRY_PUBLIC - static const double MISSING_Z; - - /** - * \brief Returns the disparity reprojection matrix. - */ - IMAGE_GEOMETRY_PUBLIC - const cv::Matx44d& reprojectionMatrix() const; - - /** - * \brief Returns the horizontal baseline in world coordinates. - */ - IMAGE_GEOMETRY_PUBLIC - double baseline() const; - - /** - * \brief Returns the depth at which a point is observed with a given disparity. - * - * This is the inverse of getDisparity(). - */ - IMAGE_GEOMETRY_PUBLIC - double getZ(double disparity) const; - - /** - * \brief Returns the disparity observed for a point at depth Z. - * - * This is the inverse of getZ(). - */ - IMAGE_GEOMETRY_PUBLIC - double getDisparity(double Z) const; - - /** - * \brief Returns true if the camera has been initialized - */ - IMAGE_GEOMETRY_PUBLIC - bool initialized() const { return left_.initialized() && right_.initialized(); } -protected: - PinholeCameraModel left_, right_; - cv::Matx44d Q_; - - IMAGE_GEOMETRY_PUBLIC - void updateQ(); -}; - - -/* Trivial inline functions */ -IMAGE_GEOMETRY_PUBLIC -inline const PinholeCameraModel& StereoCameraModel::left() const { return left_; } -IMAGE_GEOMETRY_PUBLIC -inline const PinholeCameraModel& StereoCameraModel::right() const { return right_; } - -IMAGE_GEOMETRY_PUBLIC -inline std::string StereoCameraModel::tfFrame() const { return left_.tfFrame(); } - -IMAGE_GEOMETRY_PUBLIC -inline const cv::Matx44d& StereoCameraModel::reprojectionMatrix() const { return Q_; } - -IMAGE_GEOMETRY_PUBLIC -inline double StereoCameraModel::baseline() const -{ - /// @todo Currently assuming horizontal baseline - return -right_.Tx() / right_.fx(); -} - -IMAGE_GEOMETRY_PUBLIC -inline double StereoCameraModel::getZ(double disparity) const -{ - assert( initialized() ); - return -right_.Tx() / (disparity - (left().cx() - right().cx())); -} - -IMAGE_GEOMETRY_PUBLIC -inline double StereoCameraModel::getDisparity(double Z) const -{ - assert( initialized() ); - return -right_.Tx() / Z + (left().cx() - right().cx()); ; -} - -} // namespace image_geometry +#include #endif diff --git a/image_geometry/include/image_geometry/stereo_camera_model.hpp b/image_geometry/include/image_geometry/stereo_camera_model.hpp new file mode 100644 index 000000000..34ac23076 --- /dev/null +++ b/image_geometry/include/image_geometry/stereo_camera_model.hpp @@ -0,0 +1,155 @@ +#ifndef IMAGE_GEOMETRY__STEREO_CAMERA_MODEL_HPP_ +#define IMAGE_GEOMETRY__STEREO_CAMERA_MODEL_HPP_ + +#include "image_geometry/pinhole_camera_model.hpp" +#include "image_geometry/visibility_control.hpp" + +namespace image_geometry { + +/** + * \brief Simplifies interpreting stereo image pairs geometrically using the + * parameters from the left and right sensor_msgs/CameraInfo. + */ +class StereoCameraModel +{ +public: + IMAGE_GEOMETRY_PUBLIC + StereoCameraModel(); + + IMAGE_GEOMETRY_PUBLIC + StereoCameraModel(const StereoCameraModel& other); + + IMAGE_GEOMETRY_PUBLIC + StereoCameraModel& operator=(const StereoCameraModel& other); + + /** + * \brief Set the camera parameters from the sensor_msgs/CameraInfo messages. + */ + IMAGE_GEOMETRY_PUBLIC + bool fromCameraInfo(const sensor_msgs::msg::CameraInfo& left, + const sensor_msgs::msg::CameraInfo& right); + + /** + * \brief Set the camera parameters from the sensor_msgs/CameraInfo messages. + */ + IMAGE_GEOMETRY_PUBLIC + bool fromCameraInfo(const sensor_msgs::msg::CameraInfo::ConstSharedPtr& left, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& right); + + /** + * \brief Get the left monocular camera model. + */ + IMAGE_GEOMETRY_PUBLIC + const PinholeCameraModel& left() const; + + /** + * \brief Get the right monocular camera model. + */ + IMAGE_GEOMETRY_PUBLIC + const PinholeCameraModel& right() const; + + /** + * \brief Get the name of the camera coordinate frame in tf. + * + * For stereo cameras, both the left and right CameraInfo should be in the left + * optical frame. + */ + IMAGE_GEOMETRY_PUBLIC + std::string tfFrame() const; + + /** + * \brief Project a rectified pixel with disparity to a 3d point. + */ + IMAGE_GEOMETRY_PUBLIC + void projectDisparityTo3d(const cv::Point2d& left_uv_rect, float disparity, cv::Point3d& xyz) const; + + /** + * \brief Project a disparity image to a 3d point cloud. + * + * If handleMissingValues = true, all points with minimal disparity (outliers) have + * Z set to MISSING_Z (currently 10000.0). + */ + IMAGE_GEOMETRY_PUBLIC + void projectDisparityImageTo3d(const cv::Mat& disparity, cv::Mat& point_cloud, + bool handleMissingValues = false) const; + IMAGE_GEOMETRY_PUBLIC + static const double MISSING_Z; + + /** + * \brief Returns the disparity reprojection matrix. + */ + IMAGE_GEOMETRY_PUBLIC + const cv::Matx44d& reprojectionMatrix() const; + + /** + * \brief Returns the horizontal baseline in world coordinates. + */ + IMAGE_GEOMETRY_PUBLIC + double baseline() const; + + /** + * \brief Returns the depth at which a point is observed with a given disparity. + * + * This is the inverse of getDisparity(). + */ + IMAGE_GEOMETRY_PUBLIC + double getZ(double disparity) const; + + /** + * \brief Returns the disparity observed for a point at depth Z. + * + * This is the inverse of getZ(). + */ + IMAGE_GEOMETRY_PUBLIC + double getDisparity(double Z) const; + + /** + * \brief Returns true if the camera has been initialized + */ + IMAGE_GEOMETRY_PUBLIC + bool initialized() const { return left_.initialized() && right_.initialized(); } +protected: + PinholeCameraModel left_, right_; + cv::Matx44d Q_; + + IMAGE_GEOMETRY_PUBLIC + void updateQ(); +}; + + +/* Trivial inline functions */ +IMAGE_GEOMETRY_PUBLIC +inline const PinholeCameraModel& StereoCameraModel::left() const { return left_; } +IMAGE_GEOMETRY_PUBLIC +inline const PinholeCameraModel& StereoCameraModel::right() const { return right_; } + +IMAGE_GEOMETRY_PUBLIC +inline std::string StereoCameraModel::tfFrame() const { return left_.tfFrame(); } + +IMAGE_GEOMETRY_PUBLIC +inline const cv::Matx44d& StereoCameraModel::reprojectionMatrix() const { return Q_; } + +IMAGE_GEOMETRY_PUBLIC +inline double StereoCameraModel::baseline() const +{ + /// @todo Currently assuming horizontal baseline + return -right_.Tx() / right_.fx(); +} + +IMAGE_GEOMETRY_PUBLIC +inline double StereoCameraModel::getZ(double disparity) const +{ + assert( initialized() ); + return -right_.Tx() / (disparity - (left().cx() - right().cx())); +} + +IMAGE_GEOMETRY_PUBLIC +inline double StereoCameraModel::getDisparity(double Z) const +{ + assert( initialized() ); + return -right_.Tx() / Z + (left().cx() - right().cx()); ; +} + +} // namespace image_geometry + +#endif // IMAGE_GEOMETRY__STEREO_CAMERA_MODEL_HPP_ diff --git a/image_geometry/src/pinhole_camera_model.cpp b/image_geometry/src/pinhole_camera_model.cpp index c6c5fbaf3..b3c92214b 100644 --- a/image_geometry/src/pinhole_camera_model.cpp +++ b/image_geometry/src/pinhole_camera_model.cpp @@ -1,4 +1,4 @@ -#include "image_geometry/pinhole_camera_model.h" +#include "image_geometry/pinhole_camera_model.hpp" #include namespace image_geometry { diff --git a/image_geometry/src/stereo_camera_model.cpp b/image_geometry/src/stereo_camera_model.cpp index b8f9999f7..14ea37c8c 100644 --- a/image_geometry/src/stereo_camera_model.cpp +++ b/image_geometry/src/stereo_camera_model.cpp @@ -1,4 +1,4 @@ -#include "image_geometry/stereo_camera_model.h" +#include "image_geometry/stereo_camera_model.hpp" namespace image_geometry { diff --git a/image_geometry/test/utest.cpp b/image_geometry/test/utest.cpp index 8d022077a..5e958bb66 100644 --- a/image_geometry/test/utest.cpp +++ b/image_geometry/test/utest.cpp @@ -1,4 +1,4 @@ -#include "image_geometry/pinhole_camera_model.h" +#include "image_geometry/pinhole_camera_model.hpp" #include #include diff --git a/image_geometry/test/utest_equi.cpp b/image_geometry/test/utest_equi.cpp index a2afc7423..423dbb0d0 100644 --- a/image_geometry/test/utest_equi.cpp +++ b/image_geometry/test/utest_equi.cpp @@ -1,4 +1,4 @@ -#include "image_geometry/pinhole_camera_model.h" +#include "image_geometry/pinhole_camera_model.hpp" #include #include