FitKnownRectangleRotationMaxPoints.h

Go to the documentation of this file.

#pragma once
 
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
 
#include <Eigen/Geometry>
 
 
namespace Eigen
{
    using Hyperplane3f = Hyperplane<float, 3>;
}
 
 
namespace visionx
{
 
    /**
     * @brief Finds the rotation of a rectangle of known size at a given position
     * that contains the most points from a point cloud.
     */
    class FitKnownRectangleRotationMaxPoints
    {
    public:
        using PointT = pcl::PointXYZRGBA;
 
 
    public:
 
        FitKnownRectangleRotationMaxPoints();
 
 
        // Input
 
        /// Set the rectangle size.
        void setRectangleSize(const Eigen::Vector2f& value);
        Eigen::Vector2f getRectangleSize() const;
 
        /// Set the number of solution steps, i.e. the resolution of the solution space.
        void setNumSteps(const size_t& value);
        size_t getNumSteps() const;
 
 
        /**
         * @brief Set the input cloud and the rectangle's position and normal.
         * @param cloud The input cloud.
         * @param normal The rectangle normal.
         * @param center The rectangle center.
         */
        void setInputCloud(const pcl::PointCloud<PointT>& cloud,
                           Eigen::Vector3f normal, Eigen::Vector3f center);
 
 
        // Run
 
        /**
         * @brief Count the number of inliers per rotation step, and return
         * the global pose corresponding to the rotation angle with the highest
         * number of inliers.
         *
         * The best rotation angle can be retrieved via `getAngle()`.
         * The number of inliers per step retrieved via `getInlierCounts()`.
         *
         * @return The global pose of the fitted rectangle.
         */
        Eigen::Matrix4f fit();
        /// @see `setInputCloud()`, `fit()`
        Eigen::Matrix4f fit(const pcl::PointCloud<PointT>& cloud,
                            Eigen::Vector3f normal, Eigen::Vector3f center);
 
 
        /// Count the inliers for a given local rotation angle.
        size_t countInliersOfAngle(float angle) const;
        /// Get the inliers for a given local rotation angle.
        pcl::PointIndicesPtr findInliersOfAngle(float angle) const;
 
 
        // Results
 
        /// Get the best local rotation angle.
        float getBestAngle() const;
        /// Get the best local rotation angle.
        size_t getNumInliers() const
        {
            return inlierCounts.at(bestStep);
        }
        /// Get the inliers of the best local rotation angle.
        pcl::PointIndicesPtr getInliers() const
        {
            return findInliersOfAngle(bestAngle);
        }
 
 
        /// Get the inlier counts per step.
        std::vector<size_t> getInlierCounts() const;
 
 
        // Helpers
 
        /// Get the local rotation angle of a solution step.
        template <typename Int>
        float stepToAngle(Int step) const
        {
            return (step / float(numSteps)) * maxAngle;
        }
        /// Get the global pose corresponding to a local rotation angle.
        Eigen::Matrix4f angleToPose(float angle);
 
 
    private:
 
        /// Initialize `aligned` and `alignedTransform`.
        void initAlignedPointCloud(const pcl::PointCloud<PointT>& cloud, Eigen::Vector3f normal, Eigen::Vector3f center);
        pcl::PointCloud<PointT>::ConstPtr getAlignedPointCloud() const;
 
        /// Set `rotated` by rotating `aligned` by the given angle.
        void setRotated(float angle) const;
        /// Checks whether a rotated point is an inlier.
        bool isInlier(const PointT& p) const;
 
 
    public:
 
        // Settings.
 
        /// The rectangle size.
        Eigen::Vector2f rectangleSize = Eigen::Vector2f::Ones();
 
        /// The number of steps (i.e. resolution of solution space).
        size_t numSteps = 180;
        const float maxAngle = float(M_PI);  // Need to cover 180 degrees.
 
 
        // Runtime.
 
        /// Transformed so center is at 0 and z = normal.
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr aligned {new pcl::PointCloud<pcl::PointXYZRGBA>};
        /// Transformation applied to get `aligned`.
        Eigen::Matrix4f alignedTransform = Eigen::Matrix4f::Identity();
 
        /// Buffer for point clouds rotated for inlier counting.
        mutable pcl::PointCloud<pcl::PointXYZRGBA>::Ptr rotated {new pcl::PointCloud<pcl::PointXYZRGBA>};
 
 
        // Results
 
        size_t bestStep = 0;
        /// The best angle.
        float bestAngle = 0;
        /// The inlier counts per step.
        std::vector<size_t> inlierCounts;
 
 
    };
 
}