FitKnownRectangleRotationMaxPoints.cpp

Go to the documentation of this file.

#include "FitKnownRectangleRotationMaxPoints.h"
 
#include <SimoxUtility/math/pose/invert.h>
#include <SimoxUtility/math/pose/pose.h>
 
 
namespace visionx
{
 
    FitKnownRectangleRotationMaxPoints::FitKnownRectangleRotationMaxPoints()
    {
    }
 
 
    Eigen::Vector2f FitKnownRectangleRotationMaxPoints::getRectangleSize() const
    {
        return rectangleSize;
    }
    void FitKnownRectangleRotationMaxPoints::setRectangleSize(const Eigen::Vector2f& value)
    {
        rectangleSize = value;
    }
 
 
    size_t FitKnownRectangleRotationMaxPoints::getNumSteps() const
    {
        return numSteps;
    }
    void FitKnownRectangleRotationMaxPoints::setNumSteps(const size_t& value)
    {
        numSteps = value;
    }
 
 
    void FitKnownRectangleRotationMaxPoints::setInputCloud(
        const pcl::PointCloud<PointT>& cloud, Eigen::Vector3f normal, Eigen::Vector3f center)
    {
        initAlignedPointCloud(cloud, normal, center);
    }
 
 
    Eigen::Matrix4f FitKnownRectangleRotationMaxPoints::fit()
    {
        inlierCounts = std::vector<size_t>(numSteps, 0);
        for (size_t i = 0; i < numSteps; ++i)
        {
            inlierCounts[i] = countInliersOfAngle(stepToAngle(i));
        }
 
        auto it = std::max_element(inlierCounts.begin(), inlierCounts.end());
 
        this->bestStep = size_t(std::distance(inlierCounts.begin(), it));
        this->bestAngle = stepToAngle(bestStep);
        return angleToPose(bestAngle);
    }
 
 
    Eigen::Matrix4f FitKnownRectangleRotationMaxPoints::fit(
        const pcl::PointCloud<FitKnownRectangleRotationMaxPoints::PointT>& cloud,
        Eigen::Vector3f normal, Eigen::Vector3f center)
    {
        setInputCloud(cloud, normal, center);
        return fit();
    }
 
 
    void FitKnownRectangleRotationMaxPoints::initAlignedPointCloud(
        const pcl::PointCloud<FitKnownRectangleRotationMaxPoints::PointT>& cloud,
        Eigen::Vector3f normal, Eigen::Vector3f center)
    {
        using namespace Eigen;
 
        // Move to center.
        Eigen::Matrix4f transform = simox::math::pose(-center);
        // Rotate so normal = z.
        transform = simox::math::pose(Quaternionf::FromTwoVectors(normal, Vector3f::UnitZ()))
                    * transform;
 
        alignedTransform = transform;
        pcl::transformPointCloud(cloud, *aligned, transform);
    }
 
    auto FitKnownRectangleRotationMaxPoints::getAlignedPointCloud() const -> pcl::PointCloud<PointT>::ConstPtr
    {
        return aligned;
    }
 
 
    void FitKnownRectangleRotationMaxPoints::setRotated(float angle) const
    {
        const Eigen::Matrix3f rot = Eigen::AngleAxisf(- angle, Eigen::Vector3f::UnitZ()).toRotationMatrix();
        pcl::transformPointCloud(*aligned, *rotated, simox::math::pose(rot));
    }
 
 
    bool FitKnownRectangleRotationMaxPoints::isInlier(const PointT& p) const
    {
        return std::abs(p.x) <= 0.5f * rectangleSize.x()
               && std::abs(p.y) <= 0.5f * rectangleSize.y();
    }
 
 
    size_t FitKnownRectangleRotationMaxPoints::countInliersOfAngle(float angle) const
    {
        setRotated(angle);
        // Count inliers.
        long count = std::count_if(rotated->begin(), rotated->end(),
                                   [this](const PointT & p)
        {
            return isInlier(p);
        });
        return size_t(count);
    }
 
    pcl::PointIndicesPtr FitKnownRectangleRotationMaxPoints::findInliersOfAngle(float angle) const
    {
        setRotated(angle);
 
        // Find inliers.
        pcl::PointIndicesPtr indices(new pcl::PointIndices);
        for (size_t i = 0; i < rotated->size(); ++i)
        {
            if (isInlier(rotated->points[i]))
            {
                indices->indices.push_back(int(i));
            }
        }
 
        return indices;
    }
 
 
    Eigen::Matrix4f FitKnownRectangleRotationMaxPoints::angleToPose(float angle)
    {
        Eigen::AngleAxisf localRotation(angle, Eigen::Vector3f::UnitZ());
        return simox::math::inverted_pose(alignedTransform) * simox::math::pose(localRotation);
    }
 
 
    std::vector<size_t> FitKnownRectangleRotationMaxPoints::getInlierCounts() const
    {
        return inlierCounts;
    }
    float FitKnownRectangleRotationMaxPoints::getBestAngle() const
    {
        return bestAngle;
    }
 
 
}