tracking/Utilities.h

Go to the documentation of this file.

/*
 * Copyright (c) Meta Platforms, Inc. and affiliates.
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */
 
#ifndef META_OCEAN_TRACKING_UTILITIES_H
#define META_OCEAN_TRACKING_UTILITIES_H
 
#include "ocean/tracking/Tracking.h"
 
#include "ocean/base/Frame.h"
#include "ocean/base/Maintenance.h"
#include "ocean/base/Thread.h"
#include "ocean/base/Worker.h"
 
#include "ocean/cv/PixelBoundingBox.h"
#include "ocean/cv/Canvas.h"
 
#include "ocean/io/Bitstream.h"
 
#include "ocean/math/AnyCamera.h"
#include "ocean/math/Box3.h"
#include "ocean/math/Cone3.h"
#include "ocean/math/Cylinder3.h"
#include "ocean/math/HomogenousMatrix4.h"
#include "ocean/math/PinholeCamera.h"
#include "ocean/math/Triangle3.h"
#include "ocean/math/Vector2.h"
#include "ocean/math/Vector3.h"
 
#include "ocean/tracking/Database.h"
 
namespace Ocean
{
 
namespace Tracking
{
 
/**
 * This class implements utility functions for convenient visualization of tracking data.
 * @ingroup tracking
 */
class OCEAN_TRACKING_EXPORT Utilities
{
    public:
 
        /**
         * Blends two given frames with the same frame type.
         * The resulting frame contains pixel intensity values blended at fifty percent from each frame.
         * @param frame0 The first frame to be blended, must be valid
         * @param frame1 The second frame to be blended, with same frame type as the first frame, must be valid
         * @param worker Optional worker object to distribute the computation
         * @return The resulting blended frame
         */
        static Frame blendFrames(const Frame& frame0, const Frame& frame1, Worker* worker = nullptr);
 
        /**
         * Blends two given frames with the same pixel origin.
         * If the image resolutions differ, both images will be aligned at the image center and the borders will be padded as needed.<br>
         * The resulting frame contains pixel intensity values blended at fifty percent from each frame.
         * @param frame0 The first frame to be blended, must be valid
         * @param frame1 The second frame to be blended, with same frame type as the first frame, must be valid
         * @param offset0 The location of the top-left corner of the first frame within the blended frame, with range [0, 0]x(infinity, infinity)
         * @param offset1 The location of the top-left corner of the second frame within the blended frame, with range [0, 0]x(infinity, infinity)
         * @param pixelFormat The pixel format of the resulting blended image, FORMAT_UNDEFINED to use the pixel format of the given images (which must be identical in that case)
         * @param worker Optional worker object to distribute the computation
         * @return The resulting blended frame
         */
        static Frame blendFrames(const Frame& frame0, const Frame& frame1, Vector2& offset0, Vector2& offset1, const FrameType::PixelFormat pixelFormat = FrameType::FORMAT_UNDEFINED, Worker* worker = nullptr);
 
        /**
         * Paints a line into a given frame.
         * @param frame The frame in which the line will be drawn
         * @param startPosition The start position of the line
         * @param stopPosition The stop position of the line
         * @param color The color to be used for painting, one value per frame channel, nullptr to use black
         * @param subPixel True to paint the line with sub-pixel accuracy; False to paint with pixel accuracy
         */
        static inline void paintLine(Frame& frame, const Vector2& startPosition, const Vector2& stopPosition, const uint8_t* color = nullptr, const bool subPixel = true);
 
        /**
         * Paints a set of lines into a given frame.
         * @param frame The frame in which the lines will be drawn
         * @param startPositions The start positions of the lines
         * @param stopPositions The stop positions of the lines, each stop position must have a corresponding start position
         * @param numberLines The number of lines, with range [0, infinity)
         * @param color The color to be used for painting, one value per frame channel, nullptr to use black
         * @param worker Optional worker object to distribute the computation
         * @param subPixel True to paint the lines with sub-pixel accuracy; False to paint with pixel accuracy
         * @param offsetStartPositions The offset to be added to each start position before painting the line, with range (-infinity, infinity)x(-infinity, infinity)
         * @param offsetStopPositions The offset to be added to each stop position before painting the line, with range (-infinity, infinity)x(-infinity, infinity)
         */
        static inline void paintLines(Frame& frame, const Vector2* startPositions, const Vector2* stopPositions, const size_t numberLines, const uint8_t* color = nullptr, Worker* worker = nullptr, const bool subPixel = true, const Vector2& offsetStartPositions = Vector2(0, 0), const Vector2& offsetStopPositions = Vector2(0, 0));
 
        /**
         * Paints a set of lines into a given frame with sub-pixel accuracy.
         * @param frame The frame in which the lines will be drawn, must be valid
         * @param startPositions The start positions of the lines
         * @param stopPositions The stop positions of the lines, each stop position must have a corresponding start position
         * @param numberLines The number of lines, with range [0, infinity)
         * @param color The color to be used for painting, one value per frame channel, nullptr to use black
         * @param worker Optional worker object to distribute the computation
         * @tparam tSize The thickness of the lines in pixels, must be odd with range [1, infinity)
         */
        template <unsigned int tSize>
        static inline void paintLines(Frame& frame, const Vector2* startPositions, const Vector2* stopPositions, const size_t numberLines, const uint8_t* color = nullptr, Worker* worker = nullptr);
 
        /**
         * Paints a set of lines with foreground and background colors into a given frame with sub-pixel accuracy.
         * @param frame The frame in which the lines will be drawn, must be valid
         * @param startPositions The start positions of the lines
         * @param stopPositions The stop positions of the lines, each stop position must have a corresponding start position
         * @param numberLines The number of lines, with range [0, infinity)
         * @param colorForeground The foreground color to be used for painting, one value per frame channel, nullptr to use black
         * @param colorBackground The background color to be used for painting, one value per frame channel, nullptr to use black
         * @param worker Optional worker object to distribute the computation
         * @tparam tSizeForeground The thickness of the foreground lines in pixels, must be odd with range [1, infinity)
         * @tparam tSizeBackground The thickness of the background lines in pixels, must be odd with range (tSizeForeground, infinity)
         */
        template <unsigned int tSizeForeground, unsigned int tSizeBackground>
        static inline void paintLines(Frame& frame, const Vector2* startPositions, const Vector2* stopPositions, const size_t numberLines, const uint8_t* colorForeground = nullptr, const uint8_t* colorBackground = nullptr, Worker* worker = nullptr);
 
        /**
         * Paints several paths into a given frame with sub-pixel accuracy.
         * A path is a chain of connected image points with arbitrary length (start and end points are not connected).
         * @param frame The frame in which the paths will be painted
         * @param paths The individual paths to be painted
         * @param size The number of paths, with range [0, infinity)
         * @param color The color to be used for painting, one value per frame channel, nullptr to use black
         * @param worker Optional worker object to distribute the computation
         * @tparam tSize The thickness of the paths in pixels, must be odd with range [1, infinity)
         */
        template <unsigned int tSize>
        static inline void paintPaths(Frame& frame, const Vectors2* paths, const size_t size, const uint8_t* color = nullptr, Worker* worker = nullptr);
 
        /**
         * Paints several paths into a given frame with sub-pixel accuracy.
         * A path is a chain of connected image points with arbitrary length (start and end points are not connected).<br>
         * The colors of the paths are determined by interpolation between two color values.
         * @param frame The frame in which the paths will be painted
         * @param paths The individual paths to be painted
         * @param size The number of paths, with range [0, infinity)
         * @param color0 The first color value, one value per frame channel
         * @param color1 The second color value, one value per frame channel
         * @param factors The interpolation factors, one factor per path, with range [0, 1]
         * @param worker Optional worker object to distribute the computation
         * @tparam tSize The thickness of the paths in pixels, must be odd with range [1, infinity)
         */
        template <unsigned int tSize>
        static inline void paintPaths(Frame& frame, const Vectors2* paths, const size_t size, const uint8_t* color0, const uint8_t* color1, const Scalar* factors, Worker* worker = nullptr);
 
        /**
         * Paints a 2D triangle into a given frame with sub-pixel accuracy.
         * @param frame The frame receiving the triangle
         * @param triangle The triangle to be painted, must be valid
         * @param color The color to be used for painting the triangle edges, one value per frame channel, nullptr to use black
         * @tparam tSize The thickness of the triangle edges in pixels, must be odd with range [1, infinity)
         */
        template <unsigned int tSize = 1u>
        static void paintTriangle(Frame& frame, const Triangle2& triangle, const uint8_t* color = nullptr);
 
        /**
         * Paints a set of 2D triangles into a given frame with sub-pixel accuracy.
         * @param frame The frame receiving the triangles
         * @param triangles The triangles to be painted
         * @param color The color to be used for painting the triangle edges, one value per frame channel, nullptr to use black
         * @param worker Optional worker to distribute the computation
         * @tparam tSize The thickness of the triangle edges in pixels, must be odd with range [1, infinity)
         */
        template <unsigned int tSize = 1u>
        static void paintTriangles(Frame& frame, const Triangles2& triangles, const uint8_t* color = nullptr, Worker* worker = nullptr);
 
        /**
         * Paints a set of 2D image points into a given frame with sub-pixel accuracy.
         * @param frame The frame in which the image points will be painted
         * @param imagePoints The image points to be painted
         * @param size The number of 2D points, with range [0, infinity)
         * @param color The color for the image points
         * @param worker Optional worker object to distribute the computation
         * @tparam tPointSize The radius of the image points, must be odd with range [1, infinity)
         */
        template <unsigned int tPointSize>
        static inline void paintImagePoints(Frame& frame, const Vector2* imagePoints, const size_t size, const uint8_t* color, Worker* worker = nullptr);
 
        /**
         * Paints a set of projected 3D object points into a given frame with sub-pixel accuracy.
         * @param frame The frame in which the projected object points will be painted
         * @param anyCamera The camera profile defining the projection from 3D object points to the camera plane
         * @param world_T_camera The camera pose transforming camera to world, with default viewing direction towards the negative z-space and y-axis as up vector, must be valid
         * @param objectPoints The object points to be projected into the camera frame, defined in world coordinates
         * @param size The number of 3D points, with range [0, infinity)
         * @param color The color for the object points
         * @param worker Optional worker object to distribute the computation
         * @tparam tPointSize The radius of the object points, must be odd with range [1, infinity)
         */
        template <unsigned int tPointSize>
        static inline void paintObjectPoints(Frame& frame, const AnyCamera& anyCamera, const HomogenousMatrix4& world_T_camera, const Vector3* objectPoints, const size_t size, const uint8_t* color, Worker* worker = nullptr);
 
        /**
         * Paints a feature point with a radius (scale) and orientation.
         * @param frame The frame in which the feature point will be painted, must be valid
         * @param position The position of the feature point in the pixel domain of the frame, with range (-infinity, infinity)x(-infinity, infinity)
         * @param radius The radius (scale) of the feature point in pixels, with range (0, infinity)
         * @param orientation The orientation of the feature point as CCW angle in radians, with range [0, 2PI)
         * @param color The color for the feature point, one value per frame channel
         * @param shadowColor The shadow color for the feature point, one value per frame channel, or nullptr to skip painting the shadow
         */
        static void paintFeaturePoint(Frame& frame, const Vector2& position, const Scalar radius, const Scalar orientation, const uint8_t* color, const uint8_t* shadowColor);
 
        /**
         * Paints a feature point with a radius (scale) and orientation.
         * @param frame The frame in which the feature point will be painted, must be valid
         * @param width The width of the frame in pixels, with range (0, infinity)
         * @param height The height of the frame in pixels, with range (0, infinity)
         * @param position The position of the feature point in the pixel domain of the frame, with range (-infinity, infinity)x(-infinity, infinity)
         * @param radius The radius (scale) of the feature point in pixels, with range (0, infinity)
         * @param orientation The orientation of the feature point as CCW angle in radians, with range [0, 2PI)
         * @param color The color for the feature point, one value per frame channel
         * @param shadowColor The shadow color for the feature point, one value per frame channel, or nullptr to skip painting the shadow
         * @param framePaddingElements The number of padding elements at the end of each frame row, in elements, with range [0, infinity)
         * @tparam tChannels The number of data channels, with range [1, infinity)
         */
        template <unsigned int tChannels>
        static void paintFeaturePoint8BitPerChannel(uint8_t* frame, const unsigned int width, const unsigned int height, const Vector2& position, const Scalar radius, const Scalar orientation, const uint8_t* color, const uint8_t* shadowColor, const unsigned int framePaddingElements = 0u);
 
        /**
         * Paints feature points with radius (scale) and orientation.
         * @param frame The frame in which the feature points will be painted, must be valid
         * @param positions The positions of the feature points in the pixel domain of the frame, with range (-infinity, infinity)x(-infinity, infinity), can be nullptr if size is 0
         * @param radii The radii (scale) of the feature points in pixels, with range (0, infinity), one per position, can be nullptr if size is 0
         * @param orientations The orientations of the feature points as CCW angles in radians, with range [0, 2PI), one per position, can be nullptr if size is 0
         * @param size The number of feature points to be painted, with range [0, infinity)
         * @param color The color for the feature points, one value per frame channel
         * @param shadowColor The shadow color for the feature points, one value per frame channel, or nullptr to skip painting the shadow
         * @param explicitOffset Optional explicit offset to be added to every feature point location before painting, with range (-infinity, infinity)x(-infinity, infinity)
         * @param worker Optional worker object to distribute the computation
         */
        static void paintFeaturePoints(Frame& frame, const Vector2* positions, const Scalar* radii, const Scalar* orientations, const size_t size, const uint8_t* color, const uint8_t* shadowColor, const Vector2& explicitOffset = Vector2(0, 0), Worker* worker = nullptr);
 
        /**
         * Paints feature points with radius (scale) and orientation.
         * @param frame The frame in which the feature points will be painted, must be valid
         * @param width The width of the frame in pixels, with range (0, infinity)
         * @param height The height of the frame in pixels, with range (0, infinity)
         * @param positions The positions of the feature points in the pixel domain of the frame, with range (-infinity, infinity)x(-infinity, infinity), can be nullptr if size is 0
         * @param radii The radii (scale) of the feature points in pixels, with range (0, infinity), one per position, can be nullptr if size is 0
         * @param orientations The orientations of the feature points as CCW angles in radians, with range [0, 2PI), one per position, can be nullptr if size is 0
         * @param size The number of feature points to be painted, with range [0, infinity)
         * @param color The color for the feature points, one value per frame channel
         * @param shadowColor The shadow color for the feature points, one value per frame channel, or nullptr to skip painting the shadow
         * @param explicitOffset Optional explicit offset to be added to every feature point location before painting, with range (-infinity, infinity)x(-infinity, infinity)
         * @param framePaddingElements The number of padding elements at the end of each frame row, in elements, with range [0, infinity)
         * @param worker Optional worker object to distribute the computation
         * @tparam tChannels The number of data channels, with range [1, infinity)
         */
        template <unsigned int tChannels>
        static inline void paintFeaturePoints8BitPerChannel(uint8_t* frame, const unsigned int width, const unsigned int height, const Vector2* positions, const Scalar* radii, const Scalar* orientations, const size_t size, const uint8_t* color, const uint8_t* shadowColor, const Vector2& explicitOffset, const unsigned int framePaddingElements, Worker* worker = nullptr);
 
        /**
         * Paints a set of correspondences between 2D image points and 3D object points (or their projected 2D counterparts) into a given frame with sub-pixel accuracy.
         * The projected object points will be painted first, followed by their corresponding image points.<br>
         * Additionally, a connection between the projected object points and the corresponding image points can be painted, which can be helpful if the given pose is not accurate.
         * @param frame The frame in which the projected object points and image points will be painted
         * @param camera The camera profile defining the projection between 3D object points and camera plane
         * @param model_T_camera The camera pose transforming the camera coordinate system to the coordinate system of the object points (with viewing direction along the negative z-axis and y-axis as up vector), must be valid
         * @param objectPoints The object points to be projected into the camera frame
         * @param imagePoints The image points corresponding to the given object points, the smaller the distance between image points and projected object points the more accurate the camera pose
         * @param correspondences The number of given 2D/3D point correspondences, with range [0, infinity)
         * @param maxSqrError The maximal square pixel error between a projected object point and the corresponding image point to count as valid, with range [0, infinity)
         * @param colorValidObjectPoints The color for valid object points
         * @param colorValidImagePoints The color for valid object points
         * @param colorInvalidObjectPoints The color for invalid object points
         * @param colorInvalidImagePoints The color for invalid image points
         * @param drawObjectPoints True, to draw the object points
         * @param drawImagePoints True, to draw the image points
         * @param drawConnections True, to draw the connections between projected object points and the corresponding image points
         * @param worker Optional worker object to distribute the computation
         * @tparam tObjectPointSize The radius of the object points, must be odd with range [tImagePointSize, infinity)
         * @tparam tImagePointSize The radius of the image points, must be odd, with range [1, infinity)
         */
        template <unsigned int tObjectPointSize, unsigned int tImagePointSize>
        static inline void paintCorrespondences(Frame& frame, const AnyCamera& camera, const HomogenousMatrix4& model_T_camera, const Vector3* objectPoints, const Vector2* imagePoints, const size_t correspondences, const Scalar maxSqrError, const uint8_t* colorValidObjectPoints, const uint8_t* colorValidImagePoints, const uint8_t* colorInvalidObjectPoints, const uint8_t* colorInvalidImagePoints, const bool drawObjectPoints = true, const bool drawImagePoints = true, const bool drawConnections = true, Worker* worker = nullptr);
 
        /**
         * Blends two corresponding frames each with a ratio of fifty percent and paints a set of given feature correspondences.
         * Both frames must have the same pixel origin, while internally both images will be converted to FORMAT_RGB24 images.
         * @param frame0 The first frame, must be valid
         * @param frame1 The second frame, with same frame type as the first frame
         * @param points0 The positions of the feature correspondences in the first frame
         * @param points1 The positions of the feature correspondences in the second frame, each point as a corresponding point in the first frame
         * @param numberPoints The number of given feature correspondences, with range [0, infinity)
         * @param color The color to be used to paint the points, one value per frame channel (order: R, G, B), otherwise nullptr to use black
         * @param worker Optional worker object to distribute the computation
         * @return Returns the resulting blend frame showing the feature correspondences, with pixel format FORMAT_RGB24
         */
        static Frame paintCorrespondencesBlend(const Frame& frame0, const Frame& frame1, const Vector2* points0, const Vector2* points1, const size_t numberPoints, const uint8_t* color = nullptr, Worker* worker = nullptr);
 
        /**
         * Joins two corresponding frames horizontally and paints a set of given feature correspondences.
         * Both frames can have individual frame dimensions (as long as the pixel origin is identical).<br>
         * Internally both images will be converted to FORMAT_RGB24 images.
         * @param frame0 The first frame, must be valid
         * @param frame1 The second frame, with same pixel origin as the first frame, must be valid
         * @param points0 The positions of the feature correspondences in the first frame
         * @param points1 The positions of the feature correspondences in the second frame, each point as a corresponding point in the first frame
         * @param numberPoints The number of given feature correspondences, with range [0, infinity)
         * @param color The color to be used to paint the points, one value per frame channel (order: R, G, B), otherwise nullptr to use black
         * @param worker Optional worker object to distribute the computation
         * @return Returns the resulting joined frame showing the feature correspondences, with pixel format FORMAT_RGB24
         */
        static Frame paintCorrespondencesHorizontal(const Frame& frame0, const Frame& frame1, const Vector2* points0, const Vector2* points1, const size_t numberPoints, const uint8_t* color = nullptr, Worker* worker = nullptr);
 
        /**
         * Joins two corresponding frames vertically and paints a set of given feature correspondences.
         * Both frames can have individual frame dimensions, while internally both images will be converted to FORMAT_RGB24 images.
         * @param frame0 The first frame, must be valid
         * @param frame1 The second frame, with same pixel origin as the first frame, must be valid
         * @param points0 The positions of the feature correspondences in the first frame
         * @param points1 The positions of the feature correspondences in the second frame, each point as a corresponding point in the first frame
         * @param numberPoints The number of given feature correspondences, with range [0, infinity)
         * @param color The color to be used to paint the points, one value per frame channel (order: R, G, B), otherwise nullptr to use black
         * @param worker Optional worker object to distribute the computation
         * @return Returns the resulting joined frame showing the feature correspondences, with pixel format FORMAT_RGB24
         */
        static Frame paintCorrespondencesVertical(const Frame& frame0, const Frame& frame1, const Vector2* points0, const Vector2* points1, const size_t numberPoints, const uint8_t* color = nullptr, Worker* worker = nullptr);
 
        /**
         * Joins two corresponding frames by application of a homography and paints a set of given feature correspondences.
         * The second frame will be aligned so that it matches the first frame (by application of the homography).<br>
         * Both frames can have individual frame dimensions.
         * @param frame0 The first frame, must be valid
         * @param frame1 The second frame, with pixel origin as the first frame, must be valid
         * @param points1_H_points0 The homography transforming points defined in the first frame to points defined in the second frame, must be valid
         * @param points0 The positions of the feature correspondences in the first frame, with range [0, frame0.width())x[0, frame0.height())
         * @param points1 The positions of the feature correspondences in the second frame, each point as a corresponding point in the first frame, with range [0, frame1.width())x[0, frame1.height())
         * @param numberPoints The number of given feature correspondences, with range [0, infinity)
         * @param fullCoverage True, to create a frame fully covering both frames; False, to create a frame covering the first frame only
         * @param result Returns the resulting joined frame showing the feature correspondences
         * @param foregroundColor The foreground color to be used to paint the lines, one value per frame channel, nullptr to use black
         * @param backgroundColor Optional background color to be used for each line, nullptr to avoid the usage of a background color
         * @param startColor Optional color for the start points of each line, nullptr to avoid the usage of a color for the start point
         * @param worker Optional worker object to distribute the computation
         * @return True, if succeeded
         */
        static bool paintCorrespondencesHomography(const Frame& frame0, const Frame& frame1, const SquareMatrix3& points1_H_points0, const Vector2* points0, const Vector2* points1, const size_t numberPoints, const bool fullCoverage, Frame& result, const uint8_t* foregroundColor = nullptr, const uint8_t* backgroundColor = nullptr, const uint8_t* startColor = nullptr, Worker* worker = nullptr);
 
        /**
         * Joins two corresponding frames by application of their orientations and paints a set of given feature correspondences.
         * @param pinholeCamera The pinhole camera profile defining the project, must be valid
         * @param frame0 The first frame with frame dimension as the camera dimension, must be valid
         * @param frame1 The second frame, with frame type as the first frame, must be valid
         * @param orientation0 The orientation of the first frame, must be valid
         * @param orientation1 The orientation of the second frame, must be valid
         * @param points0 The positions of the feature correspondences in the first frame, with range [0, frame0.width())x[0, frame0.height())
         * @param points1 The positions of the feature correspondences in the second frame, each point as a corresponding point in the first frame, with range [0, frame1.width())x[0, frame1.height())
         * @param numberPoints The number of given feature correspondences, with range [0, infinity)
         * @param result Returns the resulting joined frame showing the feature correspondences
         * @param foregroundColor The foreground color to be used to paint the lines, one value per frame channel, nullptr to use black
         * @param backgroundColor Optional background color to be used for each line, nullptr to avoid the usage of a background color
         * @param startColor Optional color for the start points of each line, nullptr to avoid the usage of a color for the start point
         * @param worker Optional worker object to distribute the computation
         * @return True, if succeeded
         */
        static bool paintCorrespondencesOrientations(const PinholeCamera& pinholeCamera, const Frame& frame0, const Frame& frame1, const SquareMatrix3& orientation0, const SquareMatrix3& orientation1, const Vector2* points0, const Vector2* points1, const size_t numberPoints, Frame& result, const uint8_t* foregroundColor = nullptr, const uint8_t* backgroundColor = nullptr, const uint8_t* startColor = nullptr, Worker* worker = nullptr);
 
        /**
         * Paints a set of image points into a given frame.
         * @param frame The frame receiving the points
         * @param imagePoints Image points to be painted
         * @param number The number of provided image points, with range [0, infinity)
         * @param radius The radius of the paintings in pixels, with range [0, infinity)
         * @param colorInner Inner color to be used, one value per frame channel, otherwise white is used
         * @param colorOuter Outer color to be used, one value per frame channel, otherwise black is used
         */
        static void paintPoints(Frame& frame, const Vector2* imagePoints, const size_t number, const unsigned int radius, const uint8_t* colorInner = nullptr, const uint8_t* colorOuter = nullptr);
 
        /**
         * Paints (projected) object points and image points into a given frame.
         * @param frame The frame receiving the points
         * @param world_T_camera The transformations between camera and the world, must be valid
         * @param pinholeCamera The pinhole camera profile defining the projection, with dimension equal to the frame dimension
         * @param objectPoints The object points to be painted, defined in world
         * @param numberObjectPoints Number of provided object points, with range [0, infinity)
         * @param imagePoints Image points to be painted
         * @param numberImagePoints Number of provided image points, with range [0, infinity)
         * @param distortProjectedObjectPoints True, to apply the distortion parameters of the camera to the projected object points
         * @param radiusObjectPoints Radius of the object point paintings in pixels
         * @param radiusImagePoints Radius of the image point paintings in pixels
         * @param colorObjectPoints Color to be used to paint the object points, one value per frame channel, otherwise white is used
         * @param colorImagePoints Color to be used to paint the image points, one value per frame channel, otherwise white is used
         */
        static inline void paintPoints(Frame& frame, const HomogenousMatrix4& world_T_camera, const PinholeCamera& pinholeCamera, const Vector3* objectPoints, const size_t numberObjectPoints, const Vector2* imagePoints, const size_t numberImagePoints, const bool distortProjectedObjectPoints, const unsigned int radiusObjectPoints, const unsigned int radiusImagePoints, const uint8_t* colorObjectPoints = nullptr, const uint8_t* colorImagePoints = nullptr);
 
        /**
         * Paints (projected) object points and image points into a given frame.
         * @param frame The frame receiving the points, must be valid
         * @param flippedCamera_T_world The transformation between the world and the flipped camera, must be valid
         * @param pinholeCamera The pinhole camera profile defining the projection, with dimension equal to the frame dimension
         * @param objectPoints The object points to be painted, defined in world
         * @param numberObjectPoints Number of provided object points, with range [0, infinity)
         * @param imagePoints Image points to be painted
         * @param numberImagePoints Number of provided image points, with range [0, infinity)
         * @param distortProjectedObjectPoints True, to apply the distortion parameters of the camera to the projected object points
         * @param radiusObjectPoints Radius of the object point paintings in pixels
         * @param radiusImagePoints Radius of the image point paintings in pixels
         * @param colorObjectPoints Color to be used to paint the object points, one value per frame channel, otherwise white is used
         * @param colorImagePoints Color to be used to paint the image points, one value per frame channel, otherwise white is used
         */
        static void paintPointsIF(Frame& frame, const HomogenousMatrix4& flippedCamera_T_world, const PinholeCamera& pinholeCamera, const Vector3* objectPoints, const size_t numberObjectPoints, const Vector2* imagePoints, const size_t numberImagePoints, const bool distortProjectedObjectPoints, const unsigned int radiusObjectPoints, const unsigned int radiusImagePoints, const uint8_t* colorObjectPoints = nullptr, const uint8_t* colorImagePoints = nullptr);
 
        /**
         * Paints a (projected) 3D axis aligned bounding box into a given frame.
         * @param frame The frame receiving the points, must be valid
         * @param flippedCamera_T_world The transformation between world and the flipped camera, must be valid
         * @param anyCamera The camera profile defining the projection, with dimension equal to the frame dimension
         * @param boundingBox The bounding box to be painted, defined in world
         * @param foregroundColor The foreground color of the plane, nullptr to skip the painting with the foreground color
         * @param backgroundColor The background color of the plane, nullptr to skip the painting with the background color
         * @param edgesOnly True, to paint the edges of the bounding box only; False, to paint also the diagonal connections
         */
        static void paintBoundingBoxIF(Frame& frame, const HomogenousMatrix4& flippedCamera_T_world, const AnyCamera& anyCamera, const Box3& boundingBox, const uint8_t* foregroundColor, const uint8_t* backgroundColor, const bool edgesOnly = true);
 
        /**
         * Paints a (projected) wire-frame cone into a given frame.
         * @param frame The frame being drawn to, must be valid
         * @param flippedCamera_T_cone Inverted and flipped pose of the camera w.r.t the cone
         * @param pinholeCamera The pinhole camera profile defining the projection, with dimension equal to the frame dimension
         * @param cone The cone to be painted
         * @param distortProjectedObjectPoints True, to apply the distortion parameters of the camera to the projected object points
         * @param worker Optional worker to distribute the computation
         * @param color The color for the drawn lines
         * @param numCircles Number of axis-slicing circles to draw along the cone's vertical span, with range [2, infinity)
         * @param numVerticalLines Number axis-parallel lines to draw around the cone, with range [0, infinity)
         * @param numSamples Number of lines to draw to approximate the projection of each circle, with range [3, infinity)
         */
        static void paintWireframeConeIF(Frame& frame, const HomogenousMatrix4& flippedCamera_T_cone, const PinholeCamera& pinholeCamera, const Cone3& cone, const bool distortProjectedObjectPoints = true, Worker* worker = nullptr, const uint8_t* color = CV::Canvas::yellow(), const unsigned int numCircles = 6u, const unsigned int numVerticalLines = 4u, const unsigned int numSamples = 72u);
 
        /**
         * Paints a (projected) wire-frame cylinder into a given frame.
         * @param frame The frame being drawn to, must be valid
         * @param flippedCamera_T_cylinder Inverted and flipped pose of the camera w.r.t the cylinder
         * @param pinholeCamera The pinhole camera profile defining the projection, with dimension equal to the frame dimension
         * @param cylinder The cylinder to be painted
         * @param distortProjectedObjectPoints True, to apply the distortion parameters of the camera to the projected object points
         * @param worker Optional worker to distribute the computation
         * @param color The color for the drawn lines
         * @param numCircles Number of axis-slicing circles to draw along the cylinder's vertical span, with range [2, infinity)
         * @param numVerticalLines Number axis-parallel lines to draw around the cylinder, with range [0, infinity)
         * @param numSamples Number of lines to draw to approximate the projection of each circle, with range [3, infinity)
         */
        static void paintWireframeCylinderIF(Frame& frame, const HomogenousMatrix4& flippedCamera_T_cylinder, const PinholeCamera& pinholeCamera, const Cylinder3& cylinder, const bool distortProjectedObjectPoints = true, Worker* worker = nullptr, const uint8_t* color = CV::Canvas::yellow(), const unsigned int numCircles = 6u, const unsigned int numVerticalLines = 4u, const unsigned int numSamples = 72u);
 
        /**
         * Paints a (projected) 3D triangle into a given frame.
         * @param frame The frame in which the triangle will be painted, must be valid
         * @param flippedCamera_T_world The transformation transforming world to the flipped camera, the flipped camera points towards the positive z-space with y-axis down, must be valid
         * @param anyCamera The camera profile defining the projection, must be valid
         * @param triangle The triangle to be painted
         * @param color The color to be used to paint the triangle edges, nullptr to use black
         * @tparam tSize The thickness of the triangle outline in pixels, must be odd with range [1, infinity)
         */
        template <unsigned int tSize = 1u>
        static inline void paintTriangleIF(Frame& frame, const HomogenousMatrix4& flippedCamera_T_world, const AnyCamera& anyCamera, const Triangle3& triangle, const uint8_t* color = nullptr);
 
        /**
         * Paints (projected) 3D triangles into a given frame.
         * @param frame The frame in which the triangles will be painted, must be valid
         * @param flippedCamera_T_world The transformation transforming world to the flipped camera, the flipped camera points towards the positive z-space with y-axis down, must be valid
         * @param anyCamera The camera profile defining the projection, must be valid
         * @param triangles The 3D triangles to be painted, defined in world, can be nullptr if 'numberTriangles == 0'
         * @param numberTriangles Number of triangles to be painted, with range [0, infinity)
         * @param color The color to be used to paint the triangle edges, nullptr to use black
         * @param worker Optional worker to distribute the computation
         * @tparam tSize The thickness of the triangle outline in pixels, must be odd with range [1, infinity)
         */
        template <unsigned int tSize = 1u>
        static void paintTrianglesIF(Frame& frame, const HomogenousMatrix4& flippedCamera_T_world, const AnyCamera& anyCamera, const Triangle3* triangles, const size_t numberTriangles, const uint8_t* color = nullptr, Worker* worker = nullptr);
 
        /**
         * Paints a projected 3D triangle into a given frame.
         * This function is using a triangle clipper allowing to avoid re-projection issues when drawing triangles with camera profiles with strong distortions.
         * @param frame The frame in which the triangles will be painted, must be valid
         * @param flippedCamera_T_world The transformation transforming world to the flipped camera, the flipped camera points towards the positive z-space with y-axis down, must be valid
         * @param cameraClipper The camera clipper to be used to project the 3D triangle, must be valid
         * @param triangle The 3D triangle to point, must be valid
         * @param segments The number of segments each triangle edge will be composed of, with range [1, infinity)
         * @param color The color to be used to paint the triangle edges, nullptr to use black
         * @tparam tSize The thickness of the triangle outline in pixels, must be odd with range [1, infinity)
         */
        template <unsigned int tSize = 1u>
        static void paintTriangleIF(Frame& frame, const HomogenousMatrix4& flippedCamera_T_world, const AnyCameraClipper& cameraClipper, const Triangle3& triangle, const size_t segments = 3, const uint8_t* color = nullptr);
 
        /**
         * Paints a (projected) 3D line into a given frame.
         * @param frame The frame in which the line will be painted, must be valid
         * @param flippedCamera_T_world The transformation transforming world to the flipped camera, the flipped camera points towards the positive z-space with y-axis down, must be valid
         * @param anyCamera The camera profile defining the projection, must be valid
         * @param objectPoint0 The start 3D object point of the 3D line, defined in world
         * @param objectPoint1 The end 3D object point of the 3D line, defined in world
         * @param segments The number of segments in which the line will be separated, with range [1, infinity), the more segments the better the adjustment to the camera distortion (if any)
         * @param foregroundColor The foreground color of the plane, nullptr to skip the painting with the foreground color
         * @param backgroundColor The background color of the plane, nullptr to skip the painting with the background color
         */
        static void paintLineIF(Frame& frame, const HomogenousMatrix4& flippedCamera_T_world, const AnyCamera& anyCamera, const Vector3& objectPoint0, const Vector3& objectPoint1, const unsigned int segments, const uint8_t* foregroundColor, const uint8_t* backgroundColor);
 
        /**
         * Paints a (projected) 3D line into a given frame.
         * @param frame The frame in which the line will be painted, must be valid
         * @param cameraClipper The camera clipper to be used when painting the line, must be valid
         * @param flippedCamera_T_world The transformation transforming world to the flipped camera, the flipped camera points towards the positive z-space with y-axis down, must be valid
         * @param objectPoint0 The start 3D object point of the 3D line, defined in world
         * @param objectPoint1 The end 3D object point of the 3D line, defined in world
         * @param segments The number of segments in which the line will be separated, with range [1, infinity), the more segments the better the adjustment to the camera distortion (if any)
         * @param foregroundColor The foreground color of the plane, nullptr to skip the painting with the foreground color
         * @param backgroundColor The background color of the plane, nullptr to skip the painting with the background color
         */
        static void paintLineIF(Frame& frame, const AnyCameraClipper& cameraClipper, const HomogenousMatrix4& flippedCamera_T_world, const Vector3& objectPoint0, const Vector3& objectPoint1, const unsigned int segments, const uint8_t* foregroundColor, const uint8_t* backgroundColor);
 
        /**
         * Paints a 3D coordinate system (projected) into a frame.
         * In case the frame has pixel format FORMAT_RGB24, the axis are painted in red (x), green (y), and blue (z).
         * @param frame The frame in which the coordinate system is painted, must be valid
         * @param flippedCamera_T_world The camera posed converting world to the flipped camera coordinate system (a camera coordinate system pointing towards the positive z-space), must be valid
         * @param anyCamera The camera profile that is used to render the coordinate system
         * @param world_T_coordinateSystem The transformation of the coordinate system transforming points defined in the local coordinate system (to be rendered) into points defined in the world coordinate system, must be valid
         * @param length The length of the three axis of the coordinate system, defined in the units of the local coordinate system
         */
        static void paintCoordinateSystemIF(Frame& frame, const HomogenousMatrix4& flippedCamera_T_world, const AnyCamera& anyCamera, const HomogenousMatrix4& world_T_coordinateSystem, const Scalar length);
 
        /**
         * Paints a 3D coordinate system (projected) into a frame.
         * In case the frame has pixel format FORMAT_RGB24, the axis are painted in red (x), green (y), and blue (z).
         * @param frame The frame in which the coordinate system is painted, must be valid
         * @param cameraClipper The camera clipper defining the projection, must be valid
         * @param flippedCamera_T_world The camera posed converting world to the flipped camera coordinate system (a camera coordinate system pointing towards the positive z-space), must be valid
         * @param world_T_coordinateSystem The transformation of the coordinate system transforming points defined in the local coordinate system (to be rendered) into points defined in the world coordinate system, must be valid
         * @param length The length of the three axis of the coordinate system, defined in the units of the local coordinate system
         * @param segments The number of segments to be used for each axis, with range [1, infinity)
         */
        static void paintCoordinateSystemIF(Frame& frame, const AnyCameraClipper& cameraClipper, const HomogenousMatrix4& flippedCamera_T_world, const HomogenousMatrix4& world_T_coordinateSystem, const Scalar length, const unsigned int segments = 10u);
 
        /**
         * Paints a 3D plane into the frame, further the origin of the plane is painted.
         * This function determines a worthwhile expansion of the plane best matching with the scene.
         * @param frame The frame in which the plane is painted
         * @param world_T_camera The camera pose from which the 3D object points are observed with default viewing direction towards the negative z-space and y-axis as up vector, transforming camera to world, must be valid
         * @param camera The camera profile defining the project, must be valid
         * @param planeTransformation The transformation having the origin on the plane and the z-axis parallel to the plane's normal
         * @param bins The number of bins in both directions to be painted, with range [1, infinity)
         * @param foregroundColor The foreground color of the plane, nullptr to skip the painting with the foreground color
         * @param backgroundColor The background color of the plane, nullptr to skip the painting with the background color
         * @param expansion Optional resulting plane's expansion in the scene in x-axis and y-axis defined in world coordinates, with range (0, infinity)
         * @return True, if the plane is perpendicular to the given camera frame
         */
        static bool paintPlane(Frame& frame, const HomogenousMatrix4& world_T_camera, const AnyCamera& camera, const HomogenousMatrix4& planeTransformation, const unsigned int bins, const uint8_t* foregroundColor, const uint8_t* backgroundColor, Scalar* expansion = nullptr);
 
        /**
         * Paints a 3D plane into the frame, further the origin of the plane is painted.
         * @param frame The frame in which the plane is painted
         * @param flippedCamera_T_world The camera posed converting world to the flipped camera coordinate system (a camera coordinate system pointing towards the positive z-space), must be valid
         * @param camera The camera profile defining the project, must be valid
         * @param planeTransformation The transformation having the origin on the plane and the z-axis parallel to the plane's normal
         * @param expansion The plane's expansion for the scene in x-axis and y-axis defined in world coordinates, with range (0, infinity)
         * @param bins The number of bins in both directions to be painted, with range [1, infinity)
         * @param foregroundColor The foreground color of the plane, nullptr to skip the painting with the foreground color
         * @param backgroundColor The background color of the plane, nullptr to skip the painting with the background color
         * @return True, if the plane is perpendicular to the given camera frame
         */
        static bool paintPlaneIF(Frame& frame, const HomogenousMatrix4& flippedCamera_T_world, const AnyCamera& camera, const HomogenousMatrix4& planeTransformation, const Scalar expansion, const unsigned int bins, const uint8_t* foregroundColor, const uint8_t* backgroundColor);
 
        /**
         * Paints quads that are located on a 3D plane into a given frame.
         * @param frame The frame receiving the points
         * @param world_T_camera The camera pose from which the 3D object points are observed with default viewing direction towards the negative z-space and y-axis as up vector, transforming camera to world, must be valid
         * @param camera The camera profile defining the projection, with dimension equal to the frame dimension
         * @param quadOrigin Origin of the upper left quad position, defined in world
         * @param quadHorizontal Vector starting at the quad origin and defining the horizontal direction of the quads, defined in world
         * @param quadVertical Vector starting at the quad origin and defining the vertical direction of the quads, defined in world
         * @param horizontalBins Number of horizontal bins to be painted
         * @param verticalBins Number of vertical bins to be painted
         * @param color The color to be used to paint the points, one value per frame channel, otherwise black is used
         */
        static inline void paintQuads(Frame& frame, const HomogenousMatrix4& world_T_camera, const AnyCamera& camera, const Vector3& quadOrigin, const Vector3& quadHorizontal, const Vector3& quadVertical, const unsigned int horizontalBins, const unsigned int verticalBins, const uint8_t* color = nullptr);
 
        /**
         * Paints quads that are located on a 3D plane into a given frame.
         * @see paintQuads().
         */
        static void paintQuadsIF(Frame& frame, const HomogenousMatrix4& flippedCamera_T_world, const AnyCamera& camera, const Vector3& quadOrigin, const Vector3& quadHorizontal, const Vector3& quadVertical, const unsigned int horizontalBins, const unsigned int verticalBins, const uint8_t* color = nullptr);
 
        /**
         * Paints / blends a binary 8 bit mask into a given frame with identical frame dimension.
         * @param frame The frame in which the mask will be painted, must be valid
         * @param mask The binary mask frame, with frame dimension matching with the given frame, must be valid
         * @param maskValue The mask value defining pixels to be painted as masked, may be 0x00 or 0xFF
         * @param worker Optional worker object to distribute the computation
         * @return The resulting frame with blended mask
         */
        static Frame paintMask(const Frame& frame, const Frame& mask, const uint8_t maskValue = uint8_t(0xFFu), Worker* worker = nullptr);
 
        /**
         * Paints / blends a bounding box into a given frame.
         * @param frame The frame in which the bounding box will be painted, must be valid
         * @param boundingBox The bounding box, must be valid and must be defined inside the frame
         * @param worker Optional worker object to distribute the computation
         * @return The resulting frame with blended mask
         */
        static Frame paintBoundingBox(const Frame& frame, const CV::PixelBoundingBox& boundingBox, Worker* worker = nullptr);
 
        /**
         * Paints a gravity vector into a given frame.
         * @param camera The camera profile defining the projection, must be valid
         * @param frame The frame in which the gravity vector will be painted, must be valid
         * @param gravity The gravity unit vector, defined in the coordinate system of the camera, with default camera pointing towards the negative z-space and y-axis upwards, must be valid
         * @param thickness The thickness of the gravity vector, possible values are {1, 3, 5, 7}
         * @param color The color to be used to paint the gravity vector, nullptr to use black
         * @param segments The number of segments the gravity vector will be separated into, for cameras with distortion, with range [1, infinity)
         * @param position The starting position of the gravity vector, defined in the camera coordinate system
         * @param length The length of the gravity to be painted, defined in units of the camera coordinate system, with range (0, infinity)
         * @return True, if succeeded
         */
        static bool paintGravity(const AnyCamera& camera, Frame& frame, const Vector3& gravity, const unsigned int thickness = 3u, const uint8_t* color = nullptr, const unsigned int segments = 20u, const Vector3& position = Vector3(0, 0, -1), const Scalar length = Scalar(1));
 
        /**
         * Aligns two frames connected by a given homography into one frame while the resulting frame covers the area of one given frame only (the fixed frame).
         * @param fixedFrame The fixed frame which will be untouched while the dynamic frame will be transformed by application of the homography and aligned to this frame, must be valid
         * @param dynamicFrame The dynamic frame which will be transformed and then aligned to the fixed frame, with same pixel format and pixel orientation as the fixed frame
         * @param dynamic_H_fixed The homography transforming points defined in the fixed frame to points defined in the dynamic frame, must be valid
         * @param result The resulting frame with aligned frames, the frame type will be adjusted/set internally
         * @param blend True, to blend both frames; False, to overwrite each pixel for which a corresponding pixel in the dynamic frame exist
         * @param worker Optional worker object to distribute the computation
         * @return True, if succeeded
         */
        static bool alignFramesHomography(const Frame& fixedFrame, const Frame& dynamicFrame, const SquareMatrix3& dynamic_H_fixed, Frame& result, const bool blend, Worker* worker = nullptr);
 
        /**
         * Aligns two frames connected by a given homography into one frame entirely covering the frame content of both images.
         * @param fixedFrame The fixed frame which will be untouched while the dynamic frame will be transformed by application of the homography and aligned to this frame, must be valid
         * @param dynamicFrame The dynamic frame which will be transformed and then aligned to the fixed frame, with same pixel format and pixel orientation as the fixed frame
         * @param dynamic_H_fixed The homography transforming points defined in the fixed frame to points defined in the dynamic frame, must be valid
         * @param result The resulting frame with aligned frames converting the image content of both frames, the frame type will be adjusted/set internally
         * @param blend True, to blend both frames; False, to overwrite each pixel for which a corresponding pixel in the dynamic frame exist
         * @param worker Optional worker object to distribute the computation
         * @param maximalWidth The maximal width of the resulting frame to ensure that an extreme homography does not create an extremely large result, in pixels with range [1, infinity)
         * @param maximalHeight The maximal height of the resulting frame to ensure that an extreme homography does not create an extremely large result, in pixels with range [1, infinity)
         * @param fixedFrameLeft Optional resulting horizontal location of the top left pixel of the fixed frame in the resulting aligned frame, with range [0, result.width() - fixedFrame.width()]
         * @param fixedFrameTop Optional resulting vertical location of the top left pixel of the fixed frame in the resulting aligned frame, with range [0, result.height() - fixedFrame.height()]
         * @param dynamicFrameLeft Optional resulting horizontal location of the top left pixel of the dynamic frame in relation to 'fixedFrameLeft', with range (-infinity, infinity)
         * @param dynamicFrameTop Optional resulting horizontal location of the top left pixel of the dynamic frame in relation to 'fixedFrameTop', with range (-infinity, infinity)
         * @param fullFixedFrame Optional resulting frame covering the fixed frame only, but with same frame dimension as the resulting aligned frame
         * @param fullDynamicFrame Optional resulting frame covering the transformed dynamic frame only, but with same frame dimension as the resulting aligned frame
         * @return True, if succeeded
         */
        static bool alignFramesHomographyFullCoverage(const Frame& fixedFrame, const Frame& dynamicFrame, const SquareMatrix3& dynamic_H_fixed, Frame& result, const bool blend, Worker* worker = nullptr, unsigned int maximalWidth = 16384u, const unsigned int maximalHeight = 16384u, unsigned int* fixedFrameLeft = nullptr, unsigned int* fixedFrameTop = nullptr, Scalar* dynamicFrameLeft = nullptr, Scalar* dynamicFrameTop = nullptr, Frame* fullFixedFrame = nullptr, Frame* fullDynamicFrame = nullptr);
 
        /**
         * Visualizes image point information of a tracking database for a specific pose id.
         * @param database The database holding the tracking information to be visualized
         * @param poseId The id of the pose for which the visualization will be created
         * @param frame The frame receiving the visualization of the database
         * @param colorImagePoints The color for feature points, one value per frame channel
         * @param colorImagePointsInstable The color for instable paths, one value per frame channel
         * @param colorImagePointsStable The color for stable paths, one value per frame channel
         * @param maximalPathLength The maximal length (number of concurrent frames) of paths of feature points that will be visualized, with range [0, infinity)
         * @param stablePathLength The length (number of concurrent frames) of paths so that they count as stable, with range [1, infinity)
         * @param transformation The transformation matrix which will be applied to each feature point position before the position is visualized
         * @param worker Optional worker to distribute the computation
         */
        static void visualizeDatabase(const Database& database, const Index32 poseId, Frame& frame, const uint8_t* colorImagePoints, const uint8_t* colorImagePointsInstable, const uint8_t* colorImagePointsStable, const unsigned int maximalPathLength = 20u, const unsigned int stablePathLength = 100u, const SquareMatrix3& transformation = SquareMatrix3(true), Worker* worker = nullptr);
 
        /**
         * Writes a camera profile to a binary output stream.
         * @param pinholeCamera The pinhole camera profile to be written
         * @param outputStream The output stream receiving the information
         * @return True, if succeeded
         * @see readCamera().
         */
        static bool writeCamera(const PinholeCamera& pinholeCamera, IO::OutputBitstream& outputStream);
 
        /**
         * Reads a camera profile from a binary input stream.
         * @param inputStream The input stream providing the information
         * @param pinholeCamera The resulting pinhole camera profile
         * @see writeCamera().
         */
        static bool readCamera(IO::InputBitstream& inputStream, PinholeCamera& pinholeCamera);
 
        /**
         * Writes the information of a database to a given output stream as binary information.
         * @param database The database to be written
         * @param outputStream The output stream receiving the information
         * @return True, if succeeded
         * @see readDatabase(), writeCamera().
         */
        static bool writeDatabase(const Database& database, IO::OutputBitstream& outputStream);
 
        /**
         * Reads a database information from a binary input stream.
         * @param inputStream The input stream providing the information
         * @param database The database receiving the information, the given database will be cleared before the information is assigned
         * @return True, if succeeded
         * @see writeDatabase(), readCamera().
         */
        static bool readDatabase(IO::InputBitstream& inputStream, Database& database);
 
        /**
         * Encodes the tracking environment composed of a frame mesh (a frame with correspondences of 2D image points and 3D object points), a camera pose from which the frame has been captured, and an independent set of 3D object points.
         * @param frame The frame to encode
         * @param frameImagePoints The image points located in the given frame
         * @param frameObjectPoints The object points, one object point for each image point
         * @param framePose The camera pose to encode
         * @param objectPoints The independent object points to encode
         * @return The encoded buffer
         */
        static Maintenance::Buffer encodeEnvironment(const Frame& frame, const Vectors2& frameImagePoints, const Vectors3& frameObjectPoints, const HomogenousMatrix4& framePose, const Vectors3& objectPoints);
 
    protected:
 
        /**
         * Paints a subset of a set of lines into a given frame.
         * @param frame The frame in which the lines are drawn
         * @param startPositions Start positions of the lines
         * @param stopPositions Stop positions of the lines, each end position must have a corresponding start position
         * @param color The color to be used to paint the lines, one value per frame channel, nullptr to use black
         * @param subPixel True, to paint the lines with sub-pixel accuracy; False, to paint the lines with pixel accuracy
         * @param offsetStartPositions The offset which will be added to each start position before painting the line, with range (-infinity, infinity)x(-infinity, infinity)
         * @param offsetStopPositions The offset which will be added to each stop position before painting the line, with range (-infinity, infinity)x(-infinity, infinity)
         * @param firstLine First line to be handled
         * @param numberLines The number of lines to be handled
         */
        static inline void paintLinesSubset(Frame* frame, const Vector2* startPositions, const Vector2* stopPositions, const uint8_t* color, const bool subPixel, const Vector2* offsetStartPositions, const Vector2* offsetStopPositions, const unsigned int firstLine, const unsigned int numberLines);
 
        /**
         * Paints a subset of a set of lines into a given frame with sub-pixel accuracy.
         * @param frame The frame in which the lines are drawn, must be valid
         * @param startPositions Start positions of the lines
         * @param stopPositions Stop positions of the lines, each end position must have a corresponding start position
         * @param color The color to be used to paint the lines, one value per frame channel, nullptr to use black
         * @param firstLine First line to be handled
         * @param numberLines The number of lines to be handled
         * @tparam tSize The thickness of the lines in pixels, must be odd with range [1, infinity)
         */
        template <unsigned int tSize>
        static inline void paintLinesSubset(Frame* frame, const Vector2* startPositions, const Vector2* stopPositions, const uint8_t* color, const unsigned int firstLine, const unsigned int numberLines);
 
        /**
         * Paints a subset of a set of lines into a given frame with sub-pixel accuracy.
         * @param frame The frame in which the lines are drawn, must be valid
         * @param startPositions Start positions of the lines
         * @param stopPositions Stop positions of the lines, each end position must have a corresponding start position
         * @param colorForeground Foreground color to be used to paint the points, one value per frame channel, nullptr to use black
         * @param colorBackground Background color to be used to paint the points, one value per frame channel, nullptr to use black
         * @param firstLine First line to be handled
         * @param numberLines The number of lines to be handled
         * @tparam tSizeForeground The thickness of the foreground lines in pixels, must be odd with range [1, infinity)
         * @tparam tSizeBackground The thickness of the background lines in pixels, must be odd with range (tSizeForeground, infinity)
         */
        template <unsigned int tSizeForeground, unsigned int tSizeBackground>
        static inline void paintLinesSubset(Frame* frame, const Vector2* startPositions, const Vector2* stopPositions, const uint8_t* colorForeground, const uint8_t* colorBackground, const unsigned int firstLine, const unsigned int numberLines);
 
        /**
         * Paints a subset of several paths into a given frame with sub-pixel accuracy.
         * @param frame The frame in which the paths will be painted
         * @param paths The individual paths to be painted
         * @param color The color to be used to paint the points, one value per frame channel, nullptr to apply black
         * @param firstPath The first path to be handled
         * @param numberPaths The number of paths to handle
         * @tparam tSize The thickness of the paths in pixels, must be odd with range [1, infinity)
         * @see paintPaths().
         */
        template <unsigned int tSize>
        static inline void paintPathsSubset(Frame* frame, const Vectors2* paths, const uint8_t* color, const unsigned int firstPath, const unsigned int numberPaths);
 
        /**
         * Paints a subset of several paths into a given frame with sub-pixel accuracy.
         * The colors of the paths are determined by an interpolation between two separate color values.
         * @param frame The frame in which the paths will be painted
         * @param paths The individual paths to be painted
         * @param color0 The first color value, one value per frame channel
         * @param color1 The second color value, one value per frame channel
         * @param factors The interpolation factor, one factor for each path, with range [0, 1]
         * @param firstPath The first path to be handled
         * @param numberPaths The number of paths to handle
         * @tparam tSize The thickness of the paths in pixels, must be odd with range [1, infinity)
         * @see paintPaths().
         */
        template <unsigned int tSize>
        static inline void paintPathsAdvancedSubset(Frame* frame, const Vectors2* paths, const uint8_t* color0, const uint8_t* color1, const Scalar* factors, const unsigned int firstPath, const unsigned int numberPaths);
 
        /**
         * Paints a subset of 2D triangles into a given frame with sub-pixel accuracy.
         * @param frame The frame receiving the triangles
         * @param triangles The triangles to be painted
         * @param color The color to be used to paint the triangle points, one value per frame channel, otherwise black is used
         * @param firstTriangle The first triangle to be handled
         * @param numberTriangles The number of triangle to handle
         * @tparam tSize The thickness of the triangle edges in pixels, must be odd with range [1, infinity)
         */
        template <unsigned int tSize = 1u>
        static inline void paintTrianglesSubset(Frame* frame, const Triangle2* triangles, const uint8_t* color, const unsigned int firstTriangle, const unsigned int numberTriangles);
 
        /**
         * Projects and paints a subset of 3D triangles into a given frame with sub-pixel accuracy.
         * @param frame The frame in which the triangle will be painted, must be valid
         * @param flippedCamera_T_world The transformation transforming world to the flipped camera, the flipped camera points towards the positive z-space with y-axis down, must be valid
         * @param anyCamera The camera profile defining the projection, must be valid
         * @param triangles The triangles to be painted, defined in world, must be valid
         * @param color The color to be used to paint the triangle edges, nullptr to use black
         * @param firstTriangle The first triangle to be handled
         * @param numberTriangles The number of triangle to handle
         * @tparam tSize The thickness of the triangle edges in pixels, must be odd with range [1, infinity)
         */
        template <unsigned int tSize = 1u>
        static inline void paintTrianglesIFSubset(Frame* frame, const HomogenousMatrix4* flippedCamera_T_world, const AnyCamera* anyCamera, const Triangle3* triangles, const uint8_t* color, const unsigned int firstTriangle, const unsigned int numberTriangles);
 
        /**
         * Paints subset of a set of 2D image points into a given frame with sub-pixel accuracy.
         * @param frame The frame in which the image points will be painted
         * @param imagePoints The image points to be painted
         * @param color The color for the image points
         * @param firstImagePoint The first image point to handle
         * @param numberImagePoints The number of image points to handle
         * @tparam tPointSize The radius of the image points, must be odd with range [1, infinity)
         */
        template <unsigned int tPointSize>
        static inline void paintImagePointsSubset(Frame* frame, const Vector2* imagePoints, const uint8_t* color, const unsigned int firstImagePoint, const unsigned int numberImagePoints);
 
        /**
         * Paints a subset of a set of projected 3D object points into a given frame with sub-pixel accuracy.
         * @param frame The frame in which the projected object points will be painted
         * @param anyCamera The camera profile defining the projection between 3D object points and camera plane, must be valid
         * @param flippedCamera_T_world The inverted and flipped camera pose from which the 3D object points are observed, with default camera pointing towards the positive z-space with y-axis down, must be valid
         * @param objectPoints The object points to be projected into the camera frame
         * @param color The color for the object points
         * @param firstObjectPoint The first object point to handle
         * @param numberObjectPoints The number of object points to handle
         * @tparam tPointSize The radius of the object points, must be odd with range [1, infinity)
         */
        template <unsigned int tPointSize>
        static inline void paintObjectPointsSubset(Frame* frame, const AnyCamera* anyCamera, const HomogenousMatrix4* flippedCamera_T_world, const Vector3* objectPoints, const uint8_t* color, const unsigned int firstObjectPoint, const unsigned int numberObjectPoints);
 
        /**
         * Paints a subset of feature points with radius (scale) and orientation.
         * @param frame The frame in which the feature points will be painted, must be valid
         * @param width The width of the frame in pixels, with range (0, infinity)
         * @param height The height of the frame in pixels, with range (0, infinity)
         * @param positions The positions of the feature points in the pixel domain of the frame, with range (-infinity, infinity)x(-infinity, infinity)
         * @param radii The radii (scale) of the feature points in pixels, with range (0, infinity), one for each position
         * @param orientations The orientations of the feature points as CCW angle in radians, with range [0, 2PI), one for each position
         * @param color The color for the feature point, one value for each frame data channel
         * @param shadowColor The outer color for the object points, one value for each frame data channel or nullptr to skip painting the shadow
         * @param offsetX Explicit horizontal offset which will be added to every feature point location before the point is painted, with range (-infinity, infinity)
         * @param offsetY Explicit vertical offset which will be added to every feature point location before the point is painted, with range (-infinity, infinity)
         * @param framePaddingElements The number of padding elements at the end of each frame row, in elements, with range [0, infinity)
         * @param firstFeaturePoint The first feature point to be handled, with range [0, 'size')
         * @param numberFeaturePoints The number of feature points to be handled, with range [1, 'size']
         * @tparam tChannels The number of data channels, with range [1, infinity)
         */
        template <unsigned int tChannels>
        static inline void paintFeaturePoints8BitPerChannelSubset(uint8_t* frame, const unsigned int width, const unsigned int height, const Vector2* positions, const Scalar* radii, const Scalar* orientations, const uint8_t* color, const uint8_t* shadowColor, const Scalar offsetX, const Scalar offsetY, const unsigned int framePaddingElements, const unsigned int firstFeaturePoint, const unsigned int numberFeaturePoints);
 
        /**
         * Paints a subset of a set of correspondences between 2D image points and 3D object points (or their projected 2D counterparts) into a given frame with sub-pixel accuracy.
         * The projected object points will be painted first, followed by their corresponding image points.<br>
         * Additionally, a connection between the projected object points and the corresponding image points can be painted, which can be helpful if the given pose is not accurate.
         * @param frame The frame in which the projected object points and image points will be painted
         * @param camera The camera profile defining the projection between 3D object points and camera plane
         * @param flippedCamera_T_model The inverted and flipped camera pose transforming points located in coordinate system of the model to points located in the coordinate system of the flipped camera, must be valid
         * @param objectPoints The object points to be projected into the camera frame
         * @param imagePoints The image points corresponding to the given object points, the smaller the distance between image points and projected object points the more accurate the camera pose
         * @param maxSqrError The maximal square pixel error between a projected object point and the corresponding image point to count as valid, with range [0, infinity)
         * @param colorValidObjectPoints The color for valid object points
         * @param colorValidImagePoints The color for valid object points
         * @param colorInvalidObjectPoints The color for invalid object points
         * @param colorInvalidImagePoints The color for invalid image points
         * @param drawObjectPoints True, to draw the object points
         * @param drawImagePoints True, to draw the image points
         * @param drawConnections True, to draw the connections between projected object points and the corresponding image points
         * @param firstCorrespondence The first correspondence to handle
         * @param numberCorrespondences The number of correspondences to handle
         * @tparam tObjectPointSize The radius of the object points, must be odd with range [tImagePointSize, infinity)
         * @tparam tImagePointSize The radius of the image points, must be odd, with range [1, infinity)
         * @see paintCorrespondences().
         */
        template <unsigned int tObjectPointSize, unsigned int tImagePointSize>
        static void paintCorrespondencesSubset(Frame* frame, const AnyCamera* camera, const HomogenousMatrix4* flippedCamera_T_model, const Vector3* objectPoints, const Vector2* imagePoints, const Scalar maxSqrError, const uint8_t* colorValidObjectPoints, const uint8_t* colorValidImagePoints, const uint8_t* colorInvalidObjectPoints, const uint8_t* colorInvalidImagePoints, const bool drawObjectPoints, const bool drawImagePoints, const bool drawConnections, const unsigned int firstCorrespondence, const unsigned int numberCorrespondences);
 
        /**
         * Paints / blends a binary 8 bit mask pixel into a given pixel with 8 bit per channel.
         * @param pixel The pixel data which will receive the mask
         * @param blendFactor The blend factor for the given pixel
         * @tparam tBlendChannel The index of the data channel which will be blended, with range [0, tChannels)
         * @tparam tChannels The number of data channels of the given pixel
         */
        template <unsigned int tBlendChannel, unsigned int tChannels>
        static inline void blendPixel(uint8_t* pixel, const uint8_t blendFactor);
};
 
inline void Utilities::paintLine(Frame& frame, const Vector2& startPosition, const Vector2& stopPosition, const uint8_t* color, const bool subPixel)
{
    if (subPixel)
    {
        CV::Canvas::line<1u>(frame, startPosition.x(), startPosition.y(), stopPosition.x(), stopPosition.y(), color);
    }
    else
    {
        CV::Canvas::line(frame, Numeric::round32(startPosition.x()), Numeric::round32(startPosition.y()), Numeric::round32(stopPosition.x()), Numeric::round32(stopPosition.y()), color);
    }
}
 
inline void Utilities::paintLines(Frame& frame, const Vector2* startPositions, const Vector2* stopPositions, const size_t numberLines, const uint8_t* color, Worker* worker, const bool subPixel, const Vector2& offsetStartPositions, const Vector2& offsetStopPositions)
{
    if (numberLines == 0)
    {
        return;
    }
 
    if (worker)
    {
        worker->executeFunction(Worker::Function::createStatic(&paintLinesSubset, &frame, startPositions, stopPositions, color, subPixel, &offsetStartPositions, &offsetStopPositions, 0u, 0u), 0u, (unsigned int)(numberLines));
    }
    else
    {
        paintLinesSubset(&frame, startPositions, stopPositions, color, subPixel, &offsetStartPositions, &offsetStopPositions, 0u, (unsigned int)(numberLines));
    }
}
 
template <unsigned int tSize>
inline void Utilities::paintLines(Frame& frame, const Vector2* startPositions, const Vector2* stopPositions, const size_t numberLines, const uint8_t* color, Worker* worker)
{
    if (numberLines == 0)
    {
        return;
    }
 
    if (worker)
    {
        worker->executeFunction(Worker::Function::createStatic(&paintLinesSubset<tSize>, &frame, startPositions, stopPositions, color, 0u, 0u), 0u, (unsigned int)numberLines);
    }
    else
    {
        paintLinesSubset<tSize>(&frame, startPositions, stopPositions, color, 0u, (unsigned int)numberLines);
    }
}
 
template <unsigned int tSizeForeground, unsigned int tSizeBackground>
inline void Utilities::paintLines(Frame& frame, const Vector2* startPositions, const Vector2* stopPositions, const size_t numberLines, const uint8_t* foregroundColor, const uint8_t* backgroundColor, Worker* worker)
{
    if (numberLines == 0)
    {
        return;
    }
 
    if (worker)
    {
        worker->executeFunction(Worker::Function::createStatic(&paintLinesSubset<tSizeForeground, tSizeBackground>, &frame, startPositions, stopPositions, foregroundColor, backgroundColor, 0u, 0u), 0u, (unsigned int)numberLines);
    }
    else
    {
        paintLinesSubset<tSizeForeground, tSizeBackground>(&frame, startPositions, stopPositions, foregroundColor, backgroundColor, 0u, (unsigned int)numberLines);
    }
}
 
template <unsigned int tSize>
inline void Utilities::paintPaths(Frame& frame, const Vectors2* paths, const size_t size, const uint8_t* color, Worker* worker)
{
    if (size == 0)
    {
        return;
    }
 
    if (worker)
    {
        worker->executeFunction(Worker::Function::createStatic(&paintPathsSubset<tSize>, &frame, paths, color, 0u, 0u), 0u, (unsigned int)size);
    }
    else
    {
        paintPathsSubset<tSize>(&frame, paths, color, 0u, (unsigned int)size);
    }
}
 
template <unsigned int tSize>
inline void Utilities::paintPaths(Frame& frame, const Vectors2* paths, const size_t size, const uint8_t* color0, const uint8_t* color1, const Scalar* factors, Worker* worker)
{
    if (size == 0)
    {
        return;
    }
 
    if (worker)
    {
        worker->executeFunction(Worker::Function::createStatic(&paintPathsAdvancedSubset<tSize>, &frame, paths, color0, color1, factors, 0u, 0u), 0u, (unsigned int)size);
    }
    else
    {
        paintPathsAdvancedSubset<tSize>(&frame, paths, color0, color1, factors, 0u, (unsigned int)size);
    }
}
 
template <unsigned int tSize>
void Utilities::paintTriangle(Frame& frame, const Triangle2& triangle, const uint8_t* color)
{
    paintTrianglesSubset<tSize>(&frame, &triangle, color, 0u, 1u);
}
 
template <unsigned int tSize>
void Utilities::paintTriangles(Frame& frame, const Triangles2& triangles, const uint8_t* color, Worker* worker)
{
    if (triangles.empty())
    {
        return;
    }
 
    if (worker)
    {
        worker->executeFunction(Worker::Function::createStatic(&paintTrianglesSubset<tSize>, &frame, triangles.data(), color, 0u, 0u), 0u, (unsigned int)triangles.size());
    }
    else
    {
        paintTrianglesSubset<tSize>(&frame, triangles.data(), color, 0u, (unsigned int)triangles.size());
    }
}
 
template <unsigned int tPointSize>
inline void Utilities::paintImagePoints(Frame& frame, const Vector2* imagePoints, const size_t size, const uint8_t* color, Worker* worker)
{
    static_assert(tPointSize % 2u == 1u, "Invalid point size!");
 
    if (size == 0)
    {
        return;
    }
 
    if (worker)
    {
        worker->executeFunction(Worker::Function::createStatic(&paintImagePointsSubset<tPointSize>, &frame, imagePoints, color, 0u, 0u), 0u, (unsigned int)size);
    }
    else
    {
        paintImagePointsSubset<tPointSize>(&frame, imagePoints, color, 0u, (unsigned int)size);
    }
}
 
template <unsigned int tPointSize>
inline void Utilities::paintObjectPoints(Frame& frame, const AnyCamera& anyCamera, const HomogenousMatrix4& world_T_camera, const Vector3* objectPoints, const size_t size, const uint8_t* color, Worker* worker)
{
    static_assert(tPointSize % 2u == 1u, "Invalid point size!");
 
    ocean_assert(frame.isValid() && anyCamera.isValid());
    ocean_assert(frame.width() == anyCamera.width() && frame.height() == anyCamera.height());
 
    if (size == 0)
    {
        return;
    }
 
    const HomogenousMatrix4 flippedCamera_T_world(AnyCamera::standard2InvertedFlipped(world_T_camera));
 
    if (worker)
    {
        worker->executeFunction(Worker::Function::createStatic(&paintObjectPointsSubset<tPointSize>, &frame, &anyCamera, &flippedCamera_T_world, objectPoints, color, 0u, 0u), 0u, (unsigned int)(size));
    }
    else
    {
        paintObjectPointsSubset<tPointSize>(&frame, &anyCamera, &flippedCamera_T_world, objectPoints, color, 0u, (unsigned int)(size));
    }
}
 
template <unsigned int tChannels>
void Utilities::paintFeaturePoint8BitPerChannel(uint8_t* frame, const unsigned int width, const unsigned int height, const Vector2& position, const Scalar radius, const Scalar orientation, const uint8_t* color, const uint8_t* shadowColor, const unsigned int framePaddingElements)
{
    ocean_assert(frame && width != 0u && height != 0u && color);
    ocean_assert(radius >= Numeric::eps() && orientation >= 0 && orientation <= Numeric::pi2());
 
    const Quaternion rotation(Vector3(0, 0, 1), orientation);
 
    const Vector3 direction = rotation * Vector3(radius, 0, 0);
    const Vector3 leftTop = rotation * Vector3(-radius, -radius, 0);
    const Vector3 rightTop = rotation * Vector3(radius, -radius, 0);
    const Vector3 rightBottom = rotation * Vector3(radius, radius, 0);
    const Vector3 leftBottom = rotation * Vector3(-radius, radius, 0);
 
    if (shadowColor)
    {
        CV::Canvas::line8BitPerChannel<tChannels, 3u>(frame, width, height, position.x(), position.y(), position.x() + direction.x(), position.y() + direction.y(), shadowColor, framePaddingElements);
 
        CV::Canvas::line8BitPerChannel<tChannels, 3u>(frame, width, height, position.x() + leftTop.x(), position.y() + leftTop.y(), position.x() + leftBottom.x(), position.y() + leftBottom.y(), shadowColor, framePaddingElements);
        CV::Canvas::line8BitPerChannel<tChannels, 3u>(frame, width, height, position.x() + leftBottom.x(), position.y() + leftBottom.y(), position.x() + rightBottom.x(), position.y() + rightBottom.y(), shadowColor, framePaddingElements);
        CV::Canvas::line8BitPerChannel<tChannels, 3u>(frame, width, height, position.x() + rightBottom.x(), position.y() + rightBottom.y(), position.x() + rightTop.x(), position.y() + rightTop.y(), shadowColor, framePaddingElements);
        CV::Canvas::line8BitPerChannel<tChannels, 3u>(frame, width, height, position.x() + rightTop.x(), position.y() + rightTop.y(), position.x() + leftTop.x(), position.y() + leftTop.y(), shadowColor, framePaddingElements);
    }
 
    CV::Canvas::line8BitPerChannel<tChannels, 1u>(frame, width, height, position.x(), position.y(), position.x() + direction.x(), position.y() + direction.y(), color, framePaddingElements);
 
    CV::Canvas::line8BitPerChannel<tChannels, 1u>(frame, width, height, position.x() + leftTop.x(), position.y() + leftTop.y(), position.x() + leftBottom.x(), position.y() + leftBottom.y(), color, framePaddingElements);
    CV::Canvas::line8BitPerChannel<tChannels, 1u>(frame, width, height, position.x() + leftBottom.x(), position.y() + leftBottom.y(), position.x() + rightBottom.x(), position.y() + rightBottom.y(), color, framePaddingElements);
    CV::Canvas::line8BitPerChannel<tChannels, 1u>(frame, width, height, position.x() + rightBottom.x(), position.y() + rightBottom.y(), position.x() + rightTop.x(), position.y() + rightTop.y(), color, framePaddingElements);
    CV::Canvas::line8BitPerChannel<tChannels, 1u>(frame, width, height, position.x() + rightTop.x(), position.y() + rightTop.y(), position.x() + leftTop.x(), position.y() + leftTop.y(), color, framePaddingElements);
}
 
template <unsigned int tChannels>
inline void Utilities::paintFeaturePoints8BitPerChannel(uint8_t* frame, const unsigned int width, const unsigned int height, const Vector2* positions, const Scalar* radii, const Scalar* orientations, const size_t size, const uint8_t* color, const uint8_t* shadowColor, const Vector2& explicitOffset, const unsigned int framePaddingElements, Worker* worker)
{
    static_assert(tChannels != 0u, "Invalid channel number!");
 
    ocean_assert(frame && width != 0u && height != 0u);
 
    if (size == 0)
    {
        return;
    }
 
    if (worker)
    {
        worker->executeFunction(Worker::Function::createStatic(&paintFeaturePoints8BitPerChannelSubset<tChannels>, frame, width, height, positions, radii, orientations, color, shadowColor, explicitOffset.x(), explicitOffset.y(), framePaddingElements, 0u, 0u), 0u, (unsigned int)(size), 11u, 12u, 20u);
    }
    else
    {
        paintFeaturePoints8BitPerChannelSubset<tChannels>(frame, width, height, positions, radii, orientations, color, shadowColor, explicitOffset.x(), explicitOffset.y(), framePaddingElements, 0u, (unsigned int)(size));
    }
}
 
template <unsigned int tObjectPointSize, unsigned int tImagePointSize>
inline void Utilities::paintCorrespondences(Frame& frame, const AnyCamera& camera, const HomogenousMatrix4& model_T_camera, const Vector3* objectPoints, const Vector2* imagePoints, const size_t correspondences, const Scalar maxSqrError, const uint8_t* colorValidObjectPoints, const uint8_t* colorValidImagePoints, const uint8_t* colorInvalidObjectPoints, const uint8_t* colorInvalidImagePoints, const bool drawObjectPoints, const bool drawImagePoints, const bool drawConnections, Worker* worker)
{
    static_assert(tObjectPointSize >= tImagePointSize, "Invalid point size!");
    static_assert(tObjectPointSize % 2u == 1u, "Invalid point size!");
    static_assert(tImagePointSize % 2u == 1u, "Invalid point size!");
 
    if (correspondences == 0)
    {
        return;
    }
 
    const HomogenousMatrix4 flippedCamera_T_model(AnyCamera::standard2InvertedFlipped(model_T_camera));
 
    if (worker)
    {
        worker->executeFunction(Worker::Function::createStatic(&paintCorrespondencesSubset<tObjectPointSize, tImagePointSize>, &frame, &camera, &flippedCamera_T_model, objectPoints, imagePoints, maxSqrError, colorValidObjectPoints, colorValidImagePoints, colorInvalidObjectPoints, colorInvalidImagePoints, drawObjectPoints, drawImagePoints, drawConnections, 0u, 0u), 0u, (unsigned int)correspondences);
    }
    else
    {
        paintCorrespondencesSubset<tObjectPointSize, tImagePointSize>(&frame, &camera, &flippedCamera_T_model, objectPoints, imagePoints, maxSqrError, colorValidObjectPoints, colorValidImagePoints, colorInvalidObjectPoints, colorInvalidImagePoints, drawObjectPoints, drawImagePoints, drawConnections, 0u, (unsigned int)correspondences);
    }
}
 
inline void Utilities::paintPoints(Frame& frame, const HomogenousMatrix4& world_T_camera, const PinholeCamera& pinholeCamera, const Vector3* objectPoints, const size_t numberObjectPoints, const Vector2* imagePoints, const size_t numberImagePoints, const bool distortProjectedObjectPoints, const unsigned int radiusObjectPoints, const unsigned int radiusImagePoints, const uint8_t* colorObjectPoints, const uint8_t* colorImagePoints)
{
    paintPointsIF(frame, PinholeCamera::standard2InvertedFlipped(world_T_camera), pinholeCamera, objectPoints, numberObjectPoints, imagePoints, numberImagePoints, distortProjectedObjectPoints, radiusObjectPoints, radiusImagePoints, colorObjectPoints, colorImagePoints);
}
 
inline void Utilities::paintQuads(Frame& frame, const HomogenousMatrix4& world_T_camera, const AnyCamera& camera, const Vector3& quadOrigin, const Vector3& quadHorizontal, const Vector3& quadVertical, const unsigned int horizontalBins, const unsigned int verticalBins, const uint8_t* color)
{
    paintQuadsIF(frame, AnyCamera::standard2InvertedFlipped(world_T_camera), camera, quadOrigin, quadHorizontal, quadVertical, horizontalBins, verticalBins, color);
}
 
template <unsigned int tSize>
void inline Utilities::paintTriangleIF(Frame& frame, const HomogenousMatrix4& flippedCamera_T_world, const AnyCamera& anyCamera, const Triangle3& triangle, const uint8_t* color)
{
    paintTrianglesIFSubset<tSize>(&frame, &flippedCamera_T_world, &anyCamera, &triangle, color, 0u, 1u);
}
 
template <unsigned int tSize>
void Utilities::paintTrianglesIF(Frame& frame, const HomogenousMatrix4& flippedCamera_T_world, const AnyCamera& anyCamera, const Triangle3* triangles, const size_t numberTriangles, const uint8_t* color, Worker* worker)
{
    if (numberTriangles == 0)
    {
        return;
    }
 
    if (worker)
    {
        worker->executeFunction(Worker::Function::createStatic(&paintTrianglesIFSubset<tSize>, &frame, &flippedCamera_T_world, &anyCamera, triangles, color, 0u, 0u), 0u, (unsigned int)(numberTriangles));
    }
    else
    {
        paintTrianglesIFSubset<tSize>(&frame, &flippedCamera_T_world, &anyCamera, triangles, color, 0u, (unsigned int)(numberTriangles));
    }
}
 
template <unsigned int tSize>
void Utilities::paintTriangleIF(Frame& frame, const HomogenousMatrix4& flippedCamera_T_world, const AnyCameraClipper& cameraClipper, const Triangle3& triangle, const size_t segments, const uint8_t* color)
{
    ocean_assert(frame.isValid());
    ocean_assert(flippedCamera_T_world.isValid());
    ocean_assert(cameraClipper.isValid());
    ocean_assert(triangle.isValid());
    ocean_assert(segments >= 1);
 
    for (unsigned int nSide = 0u; nSide < 3u; ++nSide)
    {
        const Vector3& point0 = triangle[nSide];
        const Vector3& point1 = triangle[(nSide + 1u) % 3u];
 
        Vector2 previousImagePoint = Vector2::minValue();
 
        if (!cameraClipper.projectToImageIF(flippedCamera_T_world, point0, &previousImagePoint))
        {
            ocean_assert(previousImagePoint == Vector2::minValue());
        }
 
        for (size_t nSegment = 0; nSegment < segments; ++nSegment)
        {
            const Scalar factor = Scalar(nSegment + 1) / Scalar(segments);
 
            const Vector3 currentPoint = point0 * (Scalar(1) - factor) + point1 * factor;
 
            Vector2 currentImagePoint = Vector2::minValue();
            if (cameraClipper.projectToImageIF(flippedCamera_T_world, currentPoint, &currentImagePoint))
            {
                if (previousImagePoint != Vector2::minValue())
                {
                    CV::Canvas::line<tSize>(frame, previousImagePoint, currentImagePoint, color);
                }
            }
 
            previousImagePoint = currentImagePoint;
        }
    }
}
 
inline void Utilities::paintLinesSubset(Frame* frame, const Vector2* startPositions, const Vector2* stopPositions, const uint8_t* color, const bool subPixel, const Vector2* offsetStartPositions, const Vector2* offsetStopPositions, const unsigned int firstLine, const unsigned int numberLines)
{
    ocean_assert(frame != nullptr);
    ocean_assert(startPositions != nullptr && stopPositions != nullptr);
    ocean_assert(offsetStartPositions != nullptr && offsetStopPositions != nullptr);
 
    for (unsigned int n = firstLine; n < firstLine + numberLines; ++n)
    {
        paintLine(*frame, startPositions[n] + *offsetStartPositions, stopPositions[n] + *offsetStopPositions, color, subPixel);
    }
}
 
template <unsigned int tSize>
inline void Utilities::paintLinesSubset(Frame* frame, const Vector2* startPositions, const Vector2* stopPositions, const uint8_t* color, const unsigned int firstLine, const unsigned int numberLines)
{
    ocean_assert(frame && startPositions && stopPositions);
 
    for (unsigned int n = firstLine; n < firstLine + numberLines; ++n)
    {
        CV::Canvas::line<tSize>(*frame, startPositions[n].x(), startPositions[n].y(), stopPositions[n].x(), stopPositions[n].y(), color);
    }
}
 
template <unsigned int tSizeForeground, unsigned int tSizeBackground>
inline void Utilities::paintLinesSubset(Frame* frame, const Vector2* startPositions, const Vector2* stopPositions, const uint8_t* colorForeground, const uint8_t* colorBackground, const unsigned int firstLine, const unsigned int numberLines)
{
    static_assert(tSizeForeground < tSizeBackground, "Invalid line size");
    ocean_assert(frame && startPositions && stopPositions);
 
    for (unsigned int n = firstLine; n < firstLine + numberLines; ++n)
    {
        CV::Canvas::line<tSizeBackground>(*frame, startPositions[n].x(), startPositions[n].y(), stopPositions[n].x(), stopPositions[n].y(), colorBackground);
        CV::Canvas::line<tSizeForeground>(*frame, startPositions[n].x(), startPositions[n].y(), stopPositions[n].x(), stopPositions[n].y(), colorForeground);
    }
}
 
template <unsigned int tSize>
inline void Utilities::paintPathsSubset(Frame* frame, const Vectors2* paths, const uint8_t* color, const unsigned int firstPath, const unsigned int numberPaths)
{
    static_assert(tSize % 2u == 1u, "Invalid size parameter.");
 
    ocean_assert(frame != nullptr);
 
    for (unsigned int n = firstPath; n < firstPath + numberPaths; ++n)
    {
        const Vectors2& path = paths[n];
 
        for (size_t i = 1; i < path.size(); ++i)
        {
            CV::Canvas::line<tSize>(*frame, path[i - 1], path[i], color);
        }
    }
}
 
template <unsigned int tSize>
inline void Utilities::paintPathsAdvancedSubset(Frame* frame, const Vectors2* paths, const uint8_t* color0, const uint8_t* color1, const Scalar* factors, const unsigned int firstPath, const unsigned int numberPaths)
{
    static_assert(tSize % 2u == 1u, "Invalid size parameter.");
 
    ocean_assert(frame != nullptr);
 
    std::vector<uint8_t> color(frame->channels());
 
    for (unsigned int n = firstPath; n < firstPath + numberPaths; ++n)
    {
        ocean_assert(Numeric::isInsideRange(0, factors[n], 1));
 
        for (unsigned int c = 0u; c < frame->channels(); ++c)
        {
            color[c] = uint8_t(Scalar(color0[c]) * (1 - factors[n]) + Scalar(color1[c]) * factors[n]);
        }
 
        const Vectors2& path = paths[n];
 
        for (size_t i = 1; i < path.size(); ++i)
        {
            CV::Canvas::line<tSize>(*frame, path[i - 1], path[i], color.data());
        }
    }
}
 
template <unsigned int tSize>
inline void Utilities::paintTrianglesSubset(Frame* frame, const Triangle2* triangles, const uint8_t* color, const unsigned int firstTriangle, const unsigned int numberTriangles)
{
    static_assert(tSize % 2u == 1u, "Invalid line width!");
 
    ocean_assert(frame && triangles);
 
    for (unsigned int n = firstTriangle; n < firstTriangle + numberTriangles; ++n)
    {
        const Triangle2& triangle = triangles[n];
        ocean_assert(triangle.isValid());
 
        CV::Canvas::line<tSize>(*frame, triangle.point0().x(), triangle.point0().y(), triangle.point1().x(), triangle.point1().y(), color);
        CV::Canvas::line<tSize>(*frame, triangle.point1().x(), triangle.point1().y(), triangle.point2().x(), triangle.point2().y(), color);
        CV::Canvas::line<tSize>(*frame, triangle.point2().x(), triangle.point2().y(), triangle.point0().x(), triangle.point0().y(), color);
    }
}
 
template <unsigned int tSize>
inline void Utilities::paintTrianglesIFSubset(Frame* frame, const HomogenousMatrix4* flippedCamera_T_world, const AnyCamera* anyCamera, const Triangle3* triangles, const uint8_t* color, const unsigned int firstTriangle, const unsigned int numberTriangles)
{
    static_assert(tSize % 2u == 1u, "Invalid line width!");
 
    ocean_assert(frame != nullptr && flippedCamera_T_world != nullptr && anyCamera != nullptr && triangles != nullptr);
 
    for (unsigned int n = firstTriangle; n < firstTriangle + numberTriangles; ++n)
    {
        if (AnyCamera::isObjectPointInFrontIF(*flippedCamera_T_world, triangles[n].point0()) && AnyCamera::isObjectPointInFrontIF(*flippedCamera_T_world, triangles[n].point1()) && AnyCamera::isObjectPointInFrontIF(*flippedCamera_T_world, triangles[n].point2()))
        {
            const Vector2 imagePoint0 = anyCamera->projectToImageIF(*flippedCamera_T_world, triangles[n][0]);
            const Vector2 imagePoint1 = anyCamera->projectToImageIF(*flippedCamera_T_world, triangles[n][1]);
            const Vector2 imagePoint2 = anyCamera->projectToImageIF(*flippedCamera_T_world, triangles[n][2]);
 
            CV::Canvas::line<tSize>(*frame, imagePoint0.x(), imagePoint0.y(), imagePoint1.x(), imagePoint1.y(), color);
            CV::Canvas::line<tSize>(*frame, imagePoint1.x(), imagePoint1.y(), imagePoint2.x(), imagePoint2.y(), color);
            CV::Canvas::line<tSize>(*frame, imagePoint2.x(), imagePoint2.y(), imagePoint0.x(), imagePoint0.y(), color);
        }
    }
}
 
template <unsigned int tPointSize>
inline void Utilities::paintImagePointsSubset(Frame* frame, const Vector2* imagePoints, const uint8_t* color, const unsigned int firstImagePoint, const unsigned int numberImagePoints)
{
    static_assert(tPointSize % 2u == 1u, "Invalid point size!");
 
    ocean_assert(frame != nullptr && imagePoints != nullptr);
 
    for (unsigned int n = firstImagePoint; n < firstImagePoint + numberImagePoints; ++n)
    {
        CV::Canvas::point<tPointSize>(*frame, imagePoints[n], color);
    }
}
 
template <unsigned int tPointSize>
inline void Utilities::paintObjectPointsSubset(Frame* frame, const AnyCamera* anyCamera, const HomogenousMatrix4* flippedCamera_T_world, const Vector3* objectPoints, const uint8_t* color, const unsigned int firstObjectPoint, const unsigned int numberObjectPoints)
{
    static_assert(tPointSize % 2u == 1u, "Invalid point size!");
 
    ocean_assert(frame != nullptr && anyCamera != nullptr && flippedCamera_T_world != nullptr && objectPoints != nullptr);
 
    for (unsigned int n = firstObjectPoint; n < firstObjectPoint + numberObjectPoints; ++n)
    {
        if (AnyCamera::isObjectPointInFrontIF(*flippedCamera_T_world, objectPoints[n]))
        {
            const Vector2 projectedObjectPoint(anyCamera->projectToImageIF(*flippedCamera_T_world, objectPoints[n]));
 
            CV::Canvas::point<tPointSize>(*frame, projectedObjectPoint, color);
        }
    }
}
 
template <unsigned int tChannels>
inline void Utilities::paintFeaturePoints8BitPerChannelSubset(uint8_t* frame, const unsigned int width, const unsigned int height, const Vector2* positions, const Scalar* radii, const Scalar* orientations, const uint8_t* color, const uint8_t* shadowColor, const Scalar offsetX, const Scalar offsetY, const unsigned int framePaddingElements, const unsigned int firstFeaturePoint, const unsigned int numberFeaturePoints)
{
    ocean_assert(frame && width != 0u && height != 0u);
    ocean_assert(positions && radii && orientations && color);
 
    for (unsigned int n = firstFeaturePoint; n < firstFeaturePoint + numberFeaturePoints; ++n)
    {
        paintFeaturePoint8BitPerChannel<tChannels>(frame, width, height, positions[n] + Vector2(offsetX, offsetY), radii[n], orientations[n], color, shadowColor, framePaddingElements);
    }
}
 
template <unsigned int tObjectPointSize, unsigned int tImagePointSize>
void Utilities::paintCorrespondencesSubset(Frame* frame, const AnyCamera* camera, const HomogenousMatrix4* flippedCamera_T_model, const Vector3* objectPoints, const Vector2* imagePoints, const Scalar maxSqrError, const uint8_t* colorValidObjectPoints, const uint8_t* colorValidImagePoints, const uint8_t* colorInvalidObjectPoints, const uint8_t* colorInvalidImagePoints, const bool drawObjectPoints, const bool drawImagePoints, const bool drawConnections, const unsigned int firstCorrespondence, const unsigned int numberCorrespondences)
{
    static_assert(tObjectPointSize >= tImagePointSize, "Invalid point size!");
    static_assert(tObjectPointSize % 2u == 1u, "Invalid point size!");
    static_assert(tImagePointSize % 2u == 1u, "Invalid point size!");
 
    ocean_assert(frame != nullptr && camera != nullptr  && flippedCamera_T_model != nullptr  && objectPoints != nullptr  && imagePoints != nullptr );
 
    for (unsigned int n = firstCorrespondence; n < firstCorrespondence + numberCorrespondences; ++n)
    {
        const Vector2 projectedObjectPoint(camera->projectToImageIF(*flippedCamera_T_model, objectPoints[n]));
 
        const Scalar sqrDistance = imagePoints[n].sqrDistance(projectedObjectPoint);
 
        const uint8_t* colorObjectPoint = (sqrDistance <= maxSqrError) ? colorValidObjectPoints : colorInvalidObjectPoints;
        const uint8_t* colorImagePoint = (sqrDistance <= maxSqrError) ? colorValidImagePoints : colorInvalidImagePoints;
 
        if (drawObjectPoints)
        {
            CV::Canvas::point<tObjectPointSize>(*frame, projectedObjectPoint, colorObjectPoint);
        }
 
        if (drawConnections && sqrDistance > 2 * 2)
        {
            CV::Canvas::line<1u>(*frame, projectedObjectPoint, imagePoints[n], colorImagePoint);
        }
 
        if (drawImagePoints)
        {
            CV::Canvas::point<tImagePointSize>(*frame, imagePoints[n], colorImagePoint);
        }
    }
}
 
template <unsigned int tBlendChannel, unsigned int tChannels>
inline void Utilities::blendPixel(uint8_t* pixel, const uint8_t blendFactor)
{
    static_assert(tBlendChannel < tChannels, "Invalid blend channel!");
 
    ocean_assert(pixel);
 
    if (blendFactor == 0xFF)
    {
        for (unsigned int n = 0u; n < tChannels; ++n)
        {
            if (n == tBlendChannel)
            {
                pixel[n] = 0xFF;
            }
            else
            {
                pixel[n] >>= 1;
            }
        }
    }
}
 
}
 
}
 
#endif // META_OCEAN_TRACKING_UTILITIES_H