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;
    }
 
 
} // namespace visionx