shapes_from_aabbs.h

Go to the documentation of this file.

#pragma once
 
#include <map>
 
#include <Eigen/Core>
 
#include <pcl/PointIndices.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 
#include <SimoxUtility/math/SoftMinMax.h>
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
 
#include <VisionX/libraries/PointCloudTools/segments.h>
 
#include <SemanticObjectRelations/Shapes/AxisAlignedBoundingBox.h>
#include <SemanticObjectRelations/Shapes/shape_containers.h>
 
namespace armarx::semantic
{
 
    /// Get the AABBs of each point cloud segment in `pointCloud`.
    semrel::ShapeList getShapesFromAABBs(const pcl::PointCloud<pcl::PointXYZL>& pointCloud);
    /// Get the AABBs of each point cloud segment in `pointCloud`.
    semrel::ShapeList getShapesFromAABBs(const pcl::PointCloud<pcl::PointXYZRGBL>& pointCloud);
 
    /// Return the AABBs of each point cloud segment in a `pointCloud`.
    semrel::ShapeList getShapesFromSoftAABBs(const pcl::PointCloud<pcl::PointXYZL>& pointCloud,
                                             float outlierRatio);
    /// Return the AABBs of each point cloud segment in a `pointCloud`.
    semrel::ShapeList getShapesFromSoftAABBs(const pcl::PointCloud<pcl::PointXYZRGBL>& pointCloud,
                                             float outlierRatio);
 
    /// Get the given AABBs as a `semrel::ShapeList`.
    semrel::ShapeList
    getShapesFromAABBs(const std::map<uint32_t, semrel::AxisAlignedBoundingBox>& segmentAABBs);
 
 
    /// Get the AABB of the given point cloud segment.
    template <class PointT, class IndicesT = pcl::PointIndices>
    semrel::AxisAlignedBoundingBox getAABB(const pcl::PointCloud<PointT>& pointCloud,
                                           const IndicesT& segmentIndices);
 
    /**
     * @brief Get the AABB of each point cloud segment.
     * Only keys of `segmentIndices` are used; point labels are not used.
     */
    template <class PointT, class IndicesT = pcl::PointIndices>
    std::map<uint32_t, semrel::AxisAlignedBoundingBox>
    getAABBs(const pcl::PointCloud<PointT>& pointCloud,
             const std::map<uint32_t, IndicesT>& segmentIndices);
 
 
    /**
     * @brief Get the soft AABB of the given point cloud segment.
     * @param outlierRatio The allowed outlier ratio. (@see `semrel::SoftMinMax`)
     */
    template <class PointT>
    semrel::AxisAlignedBoundingBox getSoftAABB(const pcl::PointCloud<PointT>& pointCloud,
                                               const pcl::PointIndices& segmentIndices,
                                               float outlierRatio);
 
    /**
     * @brief Get the soft AABBs of each point cloud segment.
     * @param outlierRatio The allowed outlier ratio. (@see `semrel::SoftMinMax`)
     */
    template <class PointT>
    std::map<uint32_t, semrel::AxisAlignedBoundingBox>
    getSoftAABBs(const pcl::PointCloud<PointT>& pointCloud,
                 const std::map<uint32_t, pcl::PointIndices>& segmentIndices,
                 float outlierRatio);
 
} // namespace armarx::semantic
 
namespace armarx::semantic::detail
{
    /// @throws `armarx::ExpressionException` if `a` is not less or equal `b`.
    void checkLessEqual(float a, float b, const std::string& msg = "");
} // namespace armarx::semantic::detail
 
template <class PointT, class IndicesT>
semrel::AxisAlignedBoundingBox
armarx::semantic::getAABB(const pcl::PointCloud<PointT>& pointCloud, const IndicesT& segmentIndices)
{
    return semrel::AxisAlignedBoundingBox(visionx::tools::getAABB(pointCloud, segmentIndices));
}
 
template <class PointT, class IndicesT>
std::map<uint32_t, semrel::AxisAlignedBoundingBox>
armarx::semantic::getAABBs(const pcl::PointCloud<PointT>& pointCloud,
                           const std::map<uint32_t, IndicesT>& segmentIndices)
{
    std::map<uint32_t, semrel::AxisAlignedBoundingBox> aabbs;
    for (const auto& [label, aabb] : visionx::tools::getSegmentAABBs(pointCloud, segmentIndices))
    {
        aabbs.emplace(label, semrel::AxisAlignedBoundingBox(aabb.limits()));
    }
    return aabbs;
}
 
template <class PointT>
semrel::AxisAlignedBoundingBox
armarx::semantic::getSoftAABB(const pcl::PointCloud<PointT>& pointCloud,
                              const pcl::PointIndices& indices,
                              float outlierRatio)
{
    detail::checkLessEqual(0, outlierRatio);
    detail::checkLessEqual(outlierRatio, 0.5f);
 
    simox::math::SoftMinMax minMaxX(outlierRatio, indices.indices.size());
    simox::math::SoftMinMax minMaxY(outlierRatio, indices.indices.size());
    simox::math::SoftMinMax minMaxZ(outlierRatio, indices.indices.size());
 
    for (int index : indices.indices)
    {
        const PointT& point = pointCloud[static_cast<std::size_t>(index)];
 
        minMaxX.add(point.x);
        minMaxY.add(point.y);
        minMaxZ.add(point.z);
    }
 
    semrel::AxisAlignedBoundingBox aabb;
    aabb.minX() = minMaxX.getSoftMin();
    aabb.maxX() = minMaxX.getSoftMax();
    aabb.minY() = minMaxY.getSoftMin();
    aabb.maxY() = minMaxY.getSoftMax();
    aabb.minZ() = minMaxZ.getSoftMin();
    aabb.maxZ() = minMaxZ.getSoftMax();
 
    return aabb;
}
 
template <class PointT>
std::map<uint32_t, semrel::AxisAlignedBoundingBox>
armarx::semantic::getSoftAABBs(const pcl::PointCloud<PointT>& pointCloud,
                               const std::map<uint32_t, pcl::PointIndices>& segmentIndices,
                               float outlierRatio)
{
    std::map<uint32_t, semrel::AxisAlignedBoundingBox> segmentAABBs;
    for (const auto& [label, indices] : segmentIndices)
    {
        segmentAABBs[label] = getSoftAABB(pointCloud, indices, outlierRatio);
        ;
    }
    return segmentAABBs;
}