AABB.h

Go to the documentation of this file.

#pragma once
 
#include <pcl/PointIndices.h>
#include <pcl/point_cloud.h>
 
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
 
#include <VisionX/interface/core/DataTypes.h>
 
namespace visionx::tools
{
 
    BoundingBox3D toBoundingBox3D(const Eigen::Vector3f& min, const Eigen::Vector3f& max);
    BoundingBox3D toBoundingBox3D(const simox::AxisAlignedBoundingBox& aabb);
 
    simox::AxisAlignedBoundingBox toAABB(const BoundingBox3D& boundingBox);
 
    /**
     * @brief Get the axis-aligned bounding-box of the given point cloud.
     * @param pointCloud The point cloud.
     * @param indices If not empty, only the specified point indices are used.
     * @return The AABB in a 3x2 matrix, where with min's in col(0) and max'x in col(1).
     */
    template <typename PointCloudT>
    simox::AxisAlignedBoundingBox
    getAABB(const PointCloudT& pointCloud, const pcl::PointIndices& indices = {})
    {
        simox::AxisAlignedBoundingBox aabb(
            Eigen::Vector3f::Constant(std::numeric_limits<float>::max()),
            Eigen::Vector3f::Constant(std::numeric_limits<float>::min()));
 
        if (indices.indices.empty())
        {
            // Consider whole point cloud.
            for (const auto& p : pointCloud)
            {
                aabb.expand_to(p);
            }
        }
        else
        {
            // Consider only indices.
            for (int i : indices.indices)
            {
                aabb.expand_to(pointCloud.at(static_cast<std::size_t>(i)));
            }
        }
 
        return aabb;
    }
 
    template <class PointCloudT>
    simox::AxisAlignedBoundingBox
    getAABB(const PointCloudT& pointCloud, const pcl::PointIndices::Ptr& indices)
    {
        return getAABB(pointCloud, indices ? *indices : pcl::PointIndices());
    }
 
} // namespace visionx::tools