shapes_from_aabbs.cpp

Go to the documentation of this file.

#include "shapes_from_aabbs.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <SemanticObjectRelations/ShapeExtraction/util/SoftMinMax.h>
#include <SemanticObjectRelations/Shapes.h>
 
 
using namespace semrel;
using namespace armarx;
 
namespace
{
    template <class PointT>
    ShapeList
    getShapesFromAABBs(const pcl::PointCloud<PointT>& pointCloud)
    {
        const std::map<uint32_t, pcl::PointIndices> segmentIndices =
            visionx::tools::getSegmentIndices(pointCloud, false);
        const std::map<uint32_t, AxisAlignedBoundingBox> aabbs =
            semantic::getAABBs(pointCloud, segmentIndices);
        return semantic::getShapesFromAABBs(aabbs);
    }
} // namespace
 
ShapeList
semantic::getShapesFromAABBs(const pcl::PointCloud<pcl::PointXYZL>& pointCloud)
{
    return ::getShapesFromAABBs<pcl::PointXYZL>(pointCloud);
}
 
ShapeList
semantic::getShapesFromAABBs(const pcl::PointCloud<pcl::PointXYZRGBL>& pointCloud)
{
    return ::getShapesFromAABBs<pcl::PointXYZRGBL>(pointCloud);
}
 
ShapeList
semantic::getShapesFromAABBs(const std::map<uint32_t, AxisAlignedBoundingBox>& segmentAABBs)
{
    semrel::ShapeList shapes;
    shapes.reserve(segmentAABBs.size());
    for (const auto& [label, aabb] : segmentAABBs)
    {
        shapes.emplace_back(new semrel::Box(aabb, semrel::ShapeID(label)));
    }
    return shapes;
}
 
namespace
{
    template <class PointT>
    ShapeList
    getShapesFromSoftAABBs(const pcl::PointCloud<PointT>& pointCloud, float outlierRatio)
    {
        const std::map<uint32_t, pcl::PointIndices> segmentIndices =
            visionx::tools::getSegmentIndices(pointCloud, false);
 
        std::map<uint32_t, AxisAlignedBoundingBox> segmentAABBs;
        for (const auto& [label, indices] : segmentIndices)
        {
            segmentAABBs[label] = semantic::getSoftAABB(pointCloud, indices, outlierRatio);
        }
 
        return semantic::getShapesFromAABBs(segmentAABBs);
    }
} // namespace
 
ShapeList
semantic::getShapesFromSoftAABBs(const pcl::PointCloud<pcl::PointXYZL>& pointCloud,
                                 float outlierRatio)
{
    return ::getShapesFromSoftAABBs(pointCloud, outlierRatio);
}
 
ShapeList
semantic::getShapesFromSoftAABBs(const pcl::PointCloud<pcl::PointXYZRGBL>& pointCloud,
                                 float outlierRatio)
{
    return ::getShapesFromSoftAABBs(pointCloud, outlierRatio);
}
 
void
armarx::semantic::detail::checkLessEqual(float a, float b, const std::string& msg)
{
    ARMARX_CHECK_LESS_EQUAL(a, b) << msg;
}