PoseProjection.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_POSE_PROJECTION_H
#define META_OCEAN_TRACKING_POSE_PROJECTION_H
 
#include "ocean/tracking/Tracker.h"
 
#include "ocean/base/Worker.h"
 
#include "ocean/cv/detector/PointFeature.h"
 
#include "ocean/geometry/Error.h"
 
#include "ocean/geometry/Geometry.h"
#include "ocean/geometry/Estimator.h"
 
#include "ocean/math/AnyCamera.h"
#include "ocean/math/HomogenousMatrix4.h"
 
#include <algorithm>
 
namespace Ocean
{
 
namespace Tracking
{
 
/**
 * This class implements a pose projection.
 * @ingroup tracking
 */
class OCEAN_TRACKING_EXPORT PoseProjection
{
    public:
 
        /**
         * Creates an empty pose projection object.
         */
        PoseProjection() = default;
 
        /**
         * Creates a new pose projection object by a given pose and object points.
         * @param world_T_camera The transformation between camera and world, with default camera looking into the negative z-space with y-axis up, must be valid
         * @param camera The camera object defining the projection
         * @param objectPoints Object points to be projected
         * @param number Number Of object points to be projected
         */
        PoseProjection(const HomogenousMatrix4& world_T_camera, const AnyCamera& camera, const Geometry::ObjectPoint* objectPoints, const size_t number);
 
        /**
         * Returns the pose of this projection.
         * @return Projection pose
         */
        inline const HomogenousMatrix4& world_T_camera() const;
 
        /**
         * Returns the image points (the projected object points) of this pose projection.
         * @return Pose image points
         */
        inline const Geometry::ImagePoints& imagePoints() const;
 
        /**
         * Returns the number of stored pose points.
         * @return Pose points
         */
        inline size_t size() const;
 
        /**
         * Returns the robust minimal average square error between this pose projection and a given 2D point cloud.
         * The number of given image points must be equal or small to the number of internal pose points of this object.
         * @param imagePoints Image points to find corresponding pose points for
         * @param numberImagePoints Number of given image points
         * @param validImagePoints Number of valid image points expecting to have corresponding points in the pose points, with range [1, numberImagePoints]
         * @param errorDetermination Depending on this flag uniqueAveragedRobustErrorInPointCloud, approximatedAveragedRobustErrorInPointCloud() or ambiguousAveragedRobustErrorInPointCloud() will be used
         * @return Resulting minimal average square error
         * @tparam tEstimator Estimator to be applied
         */
        template <Geometry::Estimator::EstimatorType tEstimator>
        Scalar minimalAverageSquareError(const Geometry::ImagePoint* imagePoints, const size_t numberImagePoints, const size_t validImagePoints, const Geometry::Error::ErrorDetermination errorDetermination);
 
        /**
         * Returns whether this pose projection holds no points.
         * @return True, if so
         */
        inline bool isEmpty() const;
 
        /**
         * Returns whether this pose projection holds at least one point.
         * @return True, if so
         */
        explicit inline operator bool() const;
 
    private:
 
        /**
         * Converts an element to an object point.
         * @param element Element to be converted
         * @return Converted object point
         */
        static inline const Vector3& objectPoint2objectPoint(const Geometry::ObjectPoint& element);
 
    private:
 
        /// Pose of this projection.
        HomogenousMatrix4 world_T_camera_ = HomogenousMatrix4(false);
 
        /// Projection object points for this pose.
        Vectors2 imagePoints_;
};
 
/**
 * This class implements a set of pose projections.
 * @ingroup tracking
 */
 class OCEAN_TRACKING_EXPORT PoseProjectionSet
 {
    public:
 
        /**
         * Definition of a vector holding pose projections.
         */
        using PoseProjections = std::vector<PoseProjection>;
 
        /**
         * This class implements a error object.
         */
        class ErrorObject
        {
            public:
 
                /**
                 * Creates a new error object.
                 * @param index Index of the element
                 * @param error Error of the element
                 */
                inline ErrorObject(const unsigned int index, const Scalar error);
 
                /**
                 * Returns the index of this object.
                 * @return Object index
                 */
                inline unsigned int index() const;
 
                /**
                 * Returns the error of this object.
                 * @return Object error
                 */
                inline Scalar error() const;
 
                /**
                 * Returns whether the left object has a lesser error value than the right one.
                 * @param object Right error object to compare
                 * @return True, if so
                 */
                inline bool operator<(const ErrorObject& object) const;
 
            private:
 
                /// Object index.
                unsigned int objectIndex;
 
                /// Object error.
                Scalar objectError;
        };
 
        /**
         * Definition of a vector holding error objects.
         */
        using ErrorObjects = std::vector<ErrorObject>;
 
    public:
 
        /**
         * Creates an empty set of pose projections.
         */
        PoseProjectionSet() = default;
 
        /**
         * Destructs a set of pose projections.
         */
        ~PoseProjectionSet();
 
        /**
         * Returns the width of the camera in pixel used for all projections.
         * @return The camera width in pixels
         * @see height().
         */
        inline unsigned int width() const;
 
        /**
         * Returns the height of the camera in pixel used for all projections.
         * @return The camera height in pixels
         * @see width().
         */
        inline unsigned int height() const;
 
        /**
         * Adds a new pose projection.
         * @param poseProjection Pose projection to be added
         */
        inline void addPoseProjection(const PoseProjection& poseProjection);
 
        /**
         * Returns the registered pose projections.
         * @return Pose projections
         */
        inline const PoseProjections& poseProjections() const;
 
        /**
         * Returns the number of registered pose projections.
         * @return Pose projection number
         */
        inline size_t size() const;
 
        /**
         * Sets the dimension of the camera used for all pose projections.
         * @param width The camera width in pixel
         * @param height The camera height in pixel
         * @see width(), height().
         */
        inline void setDimension(const unsigned int width, const unsigned int height);
 
        /**
         * Clears the projection set.
         */
        void clear();
 
        /**
         * Returns the pose with the minimal distance error.
         * The number of given image points must be equal or small to the number of internal pose points of this object.
         * @param imagePoints Image points to find corresponding pose points for
         * @param numberImagePoints Number of given image points
         * @param validImagePoints Number of valid image points expecting to have corresponding points in the pose points, with range [1, numberImagePoints]
         * @param errorDetermination Depending on this flag uniqueAveragedRobustErrorInPointCloud, approximatedAveragedRobustErrorInPointCloud() or ambiguousAveragedRobustErrorInPointCloud() will be used
         * @param resultingError Optional resulting minimal average square error
         * @param worker Optional worker object to distribute the computation
         * @return Pose with minimal error
         * @tparam tEstimator Estimator to be applied
         */
        template <Geometry::Estimator::EstimatorType tEstimator>
        inline HomogenousMatrix4 findPoseWithMinimalError(const Geometry::ImagePoint* imagePoints, const size_t numberImagePoints, const size_t validImagePoints, const Geometry::Error::ErrorDetermination errorDetermination, Scalar* resultingError = nullptr, Worker* worker = nullptr);
 
        /**
         * Returns the poses with the minimal distance error.
         * The number of given image points must be equal or small to the number of internal pose points of this object.
         * @param imagePoints Image points to find corresponding pose points for
         * @param numberImagePoints Number of given image points
         * @param validImagePoints Number of valid image points expecting to have corresponding points in the pose points, with range [1, numberImagePoints]
         * @param errorDetermination Depending on this flag uniqueAveragedRobustErrorInPointCloud, approximatedAveragedRobustErrorInPointCloud() or ambiguousAveragedRobustErrorInPointCloud() will be used
         * @param numberPoses Number of poses to be found
         * @param poses Resulting poses, make sure that the memory block provides enough space
         * @param resultingErrors Optional resulting minimal average square errors individual for each returned pose, make sure that the memory block provides enough space
         * @param worker Optional worker object to distribute the computation
         * @return Number of found best poses
         * @tparam tEstimator Estimator to be applied
         */
        template <Geometry::Estimator::EstimatorType tEstimator>
        unsigned int findPosesWithMinimalError(const Geometry::ImagePoint* imagePoints, const size_t numberImagePoints, const size_t validImagePoints, const Geometry::Error::ErrorDetermination errorDetermination, const size_t numberPoses, HomogenousMatrix4* poses, Scalar* resultingErrors = nullptr, Worker* worker = nullptr);
 
        /**
         * Returns whether this set holds no pose projections.
         * @return True, if so
         */
        inline bool isEmpty() const;
 
        /**
         * Returns whether this set holds at least one pose projection.
         * @return True, if so
         */
        explicit inline operator bool() const;
 
    private:
 
        /**
         * Returns the pose with the minimal distance error for a subset of the pose projections.
         * @param imagePoints Image points to find corresponding pose points for
         * @param numberImagePoints Number of given image points
         * @param validImagePoints Number of valid image points expecting to have corresponding points in the pose points, with range [1, numberImagePoints]
         * @param errorDetermination Depending on this flag uniqueAveragedRobustErrorInPointCloud, approximatedAveragedRobustErrorInPointCloud() or ambiguousAveragedRobustErrorInPointCloud() will be used
         * @param errorObjects Error objects receiving the estimated error values
         * @param firstProjection First projection to be handled
         * @param numberProjections Number of projections to be handled
         * @tparam tEstimator Estimator to be applied
         */
        template <Geometry::Estimator::EstimatorType tEstimator>
        void findPoseWithMinimalErrorSubset(const Geometry::ImagePoint* imagePoints, const size_t numberImagePoints, const size_t validImagePoints, const Geometry::Error::ErrorDetermination errorDetermination, ErrorObject* errorObjects, const unsigned int firstProjection, const unsigned int numberProjections);
 
    private:
 
        /// All registered pose projections.
        PoseProjections poseProjections_;
 
        /// Width of the camera in pixel used for all pose projections
        unsigned int cameraWidth_ = 0u;
 
        /// Height of the camera in pixel used for all pose projections
        unsigned int cameraHeight_ = 0u;
};
 
inline const Vector3& PoseProjection::objectPoint2objectPoint(const Geometry::ObjectPoint& objectPoint)
{
    return objectPoint;
}
 
inline const HomogenousMatrix4& PoseProjection::world_T_camera() const
{
    return world_T_camera_;
}
 
inline const Geometry::ImagePoints& PoseProjection::imagePoints() const
{
    return imagePoints_;
}
 
inline size_t PoseProjection::size() const
{
    return imagePoints_.size();
}
 
inline bool PoseProjection::isEmpty() const
{
    return imagePoints_.empty();
}
 
inline PoseProjection::operator bool() const
{
    return !imagePoints_.empty();
}
 
inline unsigned int PoseProjectionSet::width() const
{
    return cameraWidth_;
}
 
inline unsigned int PoseProjectionSet::height() const
{
    return cameraHeight_;
}
 
inline void PoseProjectionSet::setDimension(const unsigned int width, const unsigned int height)
{
    cameraWidth_ = width;
    cameraHeight_ = height;
}
 
inline void PoseProjectionSet::addPoseProjection(const PoseProjection& poseProjection)
{
    poseProjections_.push_back(poseProjection);
}
 
inline const PoseProjectionSet::PoseProjections& PoseProjectionSet::poseProjections() const
{
    return poseProjections_;
}
 
inline size_t PoseProjectionSet::size() const
{
    return poseProjections_.size();
}
 
inline bool PoseProjectionSet::isEmpty() const
{
    return poseProjections_.empty();
}
 
inline PoseProjectionSet::operator bool() const
{
    return !poseProjections_.empty();
}
 
inline PoseProjectionSet::ErrorObject::ErrorObject(const unsigned int index, const Scalar error) :
    objectIndex(index),
    objectError(error)
{
    // nothing to do here
}
 
inline unsigned int PoseProjectionSet::ErrorObject::index() const
{
    return objectIndex;
}
 
inline Scalar PoseProjectionSet::ErrorObject::error() const
{
    return objectError;
}
 
inline bool PoseProjectionSet::ErrorObject::operator<(const ErrorObject& element) const
{
    return objectError < element.objectError;
}
 
template <Geometry::Estimator::EstimatorType tEstimator>
Scalar PoseProjection::minimalAverageSquareError(const Geometry::ImagePoint* imagePoints, const size_t numberImagePoints, const size_t validImagePoints, const Geometry::Error::ErrorDetermination errorDetermination)
{
    if (isEmpty())
    {
        return Numeric::maxValue();
    }
 
    const size_t points = min(size(), numberImagePoints);
    const size_t validPoints = min(validImagePoints, points);
 
    return Geometry::Error::averagedRobustErrorInPointCloud<tEstimator>(imagePoints, points, validPoints, imagePoints_.data(), size(), errorDetermination);
}
 
template <Geometry::Estimator::EstimatorType tEstimator>
HomogenousMatrix4 PoseProjectionSet::findPoseWithMinimalError(const Geometry::ImagePoint* imagePoints, const size_t numberImagePoints, const size_t validImagePoints, const Geometry::Error::ErrorDetermination errorDetermination, Scalar* resultingError, Worker* worker)
{
    ocean_assert(!isEmpty());
 
    if (isEmpty())
    {
        return HomogenousMatrix4();
    }
 
    ErrorObjects errorObjects(poseProjections_.size(), ErrorObject(0xFFFFFFFF, Numeric::maxValue()));
 
    if (worker)
    {
        worker->executeFunction(Worker::Function::create(*this, &PoseProjectionSet::findPoseWithMinimalErrorSubset<tEstimator>, imagePoints, numberImagePoints, validImagePoints, errorDetermination, errorObjects.data(), 0u, 0u), 0u, size(), 5u, 6u);
    }
    else
    {
        findPoseWithMinimalErrorSubset<tEstimator>(imagePoints, numberImagePoints, validImagePoints, errorDetermination, errorObjects.data(), 0u, (unsigned int)(size()));
    }
 
    std::sort(errorObjects.begin(), errorObjects.end());
 
    if (resultingError != nullptr)
    {
        *resultingError = errorObjects.front().error();
    }
 
    ocean_assert(errorObjects.front().index() < poseProjections_.size());
    return poseProjections_[errorObjects.front().index()].world_T_camera();
}
 
template <Geometry::Estimator::EstimatorType tEstimator>
unsigned int PoseProjectionSet::findPosesWithMinimalError(const Geometry::ImagePoint* imagePoints, const size_t numberImagePoints, const size_t validImagePoints, const Geometry::Error::ErrorDetermination errorDetermination, const size_t numberPoses, HomogenousMatrix4* poses, Scalar* resultingErrors, Worker* worker)
{
    ocean_assert(poses);
 
    if (isEmpty() || numberPoses == 0)
    {
        return 0;
    }
 
    ErrorObjects errorObjects(poseProjections_.size(), ErrorObject(0xFFFFFFFF, Numeric::maxValue()));
 
    if (worker)
    {
        worker->executeFunction(Worker::Function::create(*this, &PoseProjectionSet::findPoseWithMinimalErrorSubset<tEstimator>, imagePoints, numberImagePoints, validImagePoints, errorDetermination, errorObjects.data(), 0u, 0u), 0u, (unsigned int)size(), 5u, 6u);
    }
    else
    {
        findPoseWithMinimalErrorSubset<tEstimator>(imagePoints, numberImagePoints, validImagePoints, errorDetermination, errorObjects.data(), 0u, (unsigned int)size());
    }
 
    // **NOTE** We should seek a slightly larger set of best matching poses as we should try to find good poses but also different poses
    // **NOTE** We should think about the application of more suitable data structures like KD-Trees to improve the performance
 
    std::sort(errorObjects.begin(), errorObjects.end());
 
    const size_t results = min(numberPoses, errorObjects.size());
 
    for (size_t n = 0; n < results; ++n)
    {
        poses[n] = poseProjections_[errorObjects[n].index()].world_T_camera();
 
        if (resultingErrors)
        {
            resultingErrors[n] = errorObjects[n].error();
        }
    }
 
    return (unsigned int)(results);
}
 
template <Geometry::Estimator::EstimatorType tEstimator>
void PoseProjectionSet::findPoseWithMinimalErrorSubset(const Geometry::ImagePoint* imagePoints, const size_t numberImagePoints, const size_t validImagePoints, const Geometry::Error::ErrorDetermination errorDetermination, ErrorObject* errorObjects, const unsigned int firstProjection, const unsigned int numberProjections)
{
    ocean_assert(imagePoints);
    ocean_assert(errorObjects);
 
    ocean_assert(firstProjection + numberProjections <= poseProjections_.size());
 
    for (unsigned int n = firstProjection; n < firstProjection + numberProjections; ++n)
    {
        const Scalar errorValue = poseProjections_[n].minimalAverageSquareError<tEstimator>(imagePoints, numberImagePoints, validImagePoints, errorDetermination);
        errorObjects[n] = ErrorObject(n, errorValue);
    }
}
 
}
 
}
 
#endif // META_OCEAN_TRACKING_POSE_PROJECTION_H