shapes_from_aabbs.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <map>
4 
5 #include <Eigen/Core>
6 
7 #include <pcl/PointIndices.h>
8 #include <pcl/point_cloud.h>
9 #include <pcl/point_types.h>
10 
11 #include <SimoxUtility/math/SoftMinMax.h>
12 #include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
13 
15 
16 #include <SemanticObjectRelations/Shapes/AxisAlignedBoundingBox.h>
17 #include <SemanticObjectRelations/Shapes/shape_containers.h>
18 
19 namespace armarx::semantic
20 {
21 
22  /// Get the AABBs of each point cloud segment in `pointCloud`.
23  semrel::ShapeList getShapesFromAABBs(const pcl::PointCloud<pcl::PointXYZL>& pointCloud);
24  /// Get the AABBs of each point cloud segment in `pointCloud`.
25  semrel::ShapeList getShapesFromAABBs(const pcl::PointCloud<pcl::PointXYZRGBL>& pointCloud);
26 
27  /// Return the AABBs of each point cloud segment in a `pointCloud`.
28  semrel::ShapeList getShapesFromSoftAABBs(const pcl::PointCloud<pcl::PointXYZL>& pointCloud,
29  float outlierRatio);
30  /// Return the AABBs of each point cloud segment in a `pointCloud`.
31  semrel::ShapeList getShapesFromSoftAABBs(const pcl::PointCloud<pcl::PointXYZRGBL>& pointCloud,
32  float outlierRatio);
33 
34  /// Get the given AABBs as a `semrel::ShapeList`.
35  semrel::ShapeList
36  getShapesFromAABBs(const std::map<uint32_t, semrel::AxisAlignedBoundingBox>& segmentAABBs);
37 
38 
39  /// Get the AABB of the given point cloud segment.
40  template <class PointT, class IndicesT = pcl::PointIndices>
41  semrel::AxisAlignedBoundingBox getAABB(const pcl::PointCloud<PointT>& pointCloud,
42  const IndicesT& segmentIndices);
43 
44  /**
45  * @brief Get the AABB of each point cloud segment.
46  * Only keys of `segmentIndices` are used; point labels are not used.
47  */
48  template <class PointT, class IndicesT = pcl::PointIndices>
49  std::map<uint32_t, semrel::AxisAlignedBoundingBox>
50  getAABBs(const pcl::PointCloud<PointT>& pointCloud,
51  const std::map<uint32_t, IndicesT>& segmentIndices);
52 
53 
54  /**
55  * @brief Get the soft AABB of the given point cloud segment.
56  * @param outlierRatio The allowed outlier ratio. (@see `semrel::SoftMinMax`)
57  */
58  template <class PointT>
59  semrel::AxisAlignedBoundingBox getSoftAABB(const pcl::PointCloud<PointT>& pointCloud,
60  const pcl::PointIndices& segmentIndices,
61  float outlierRatio);
62 
63  /**
64  * @brief Get the soft AABBs of each point cloud segment.
65  * @param outlierRatio The allowed outlier ratio. (@see `semrel::SoftMinMax`)
66  */
67  template <class PointT>
68  std::map<uint32_t, semrel::AxisAlignedBoundingBox>
69  getSoftAABBs(const pcl::PointCloud<PointT>& pointCloud,
70  const std::map<uint32_t, pcl::PointIndices>& segmentIndices,
71  float outlierRatio);
72 
73 } // namespace armarx::semantic
74 
76 {
77  /// @throws `armarx::ExpressionException` if `a` is not less or equal `b`.
78  void checkLessEqual(float a, float b, const std::string& msg = "");
79 } // namespace armarx::semantic::detail
80 
81 template <class PointT, class IndicesT>
83 armarx::semantic::getAABB(const pcl::PointCloud<PointT>& pointCloud, const IndicesT& segmentIndices)
84 {
85  return semrel::AxisAlignedBoundingBox(visionx::tools::getAABB(pointCloud, segmentIndices));
86 }
87 
88 template <class PointT, class IndicesT>
89 std::map<uint32_t, semrel::AxisAlignedBoundingBox>
90 armarx::semantic::getAABBs(const pcl::PointCloud<PointT>& pointCloud,
91  const std::map<uint32_t, IndicesT>& segmentIndices)
92 {
93  std::map<uint32_t, semrel::AxisAlignedBoundingBox> aabbs;
94  for (const auto& [label, aabb] : visionx::tools::getSegmentAABBs(pointCloud, segmentIndices))
95  {
96  aabbs.emplace(label, semrel::AxisAlignedBoundingBox(aabb.limits()));
97  }
98  return aabbs;
99 }
100 
101 template <class PointT>
103 armarx::semantic::getSoftAABB(const pcl::PointCloud<PointT>& pointCloud,
104  const pcl::PointIndices& indices,
105  float outlierRatio)
106 {
107  detail::checkLessEqual(0, outlierRatio);
108  detail::checkLessEqual(outlierRatio, 0.5f);
109 
110  simox::math::SoftMinMax minMaxX(outlierRatio, indices.indices.size());
111  simox::math::SoftMinMax minMaxY(outlierRatio, indices.indices.size());
112  simox::math::SoftMinMax minMaxZ(outlierRatio, indices.indices.size());
113 
114  for (int index : indices.indices)
115  {
116  const PointT& point = pointCloud[static_cast<std::size_t>(index)];
117 
118  minMaxX.add(point.x);
119  minMaxY.add(point.y);
120  minMaxZ.add(point.z);
121  }
122 
124  aabb.minX() = minMaxX.getSoftMin();
125  aabb.maxX() = minMaxX.getSoftMax();
126  aabb.minY() = minMaxY.getSoftMin();
127  aabb.maxY() = minMaxY.getSoftMax();
128  aabb.minZ() = minMaxZ.getSoftMin();
129  aabb.maxZ() = minMaxZ.getSoftMax();
130 
131  return aabb;
132 }
133 
134 template <class PointT>
135 std::map<uint32_t, semrel::AxisAlignedBoundingBox>
136 armarx::semantic::getSoftAABBs(const pcl::PointCloud<PointT>& pointCloud,
137  const std::map<uint32_t, pcl::PointIndices>& segmentIndices,
138  float outlierRatio)
139 {
140  std::map<uint32_t, semrel::AxisAlignedBoundingBox> segmentAABBs;
141  for (const auto& [label, indices] : segmentIndices)
142  {
143  segmentAABBs[label] = getSoftAABB(pointCloud, indices, outlierRatio);
144  ;
145  }
146  return segmentAABBs;
147 }
armarx::semantic::getShapesFromSoftAABBs
semrel::ShapeList getShapesFromSoftAABBs(const pcl::PointCloud< pcl::PointXYZL > &pointCloud, float outlierRatio)
Return the AABBs of each point cloud segment in a pointCloud.
Definition: shapes_from_aabbs.cpp:70
index
uint8_t index
Definition: EtherCATFrame.h:59
armarx::semantic::getShapesFromAABBs
semrel::ShapeList getShapesFromAABBs(const pcl::PointCloud< pcl::PointXYZL > &pointCloud)
Get the AABBs of each point cloud segment in pointCloud.
Definition: shapes_from_aabbs.cpp:27
armarx::semantic::getAABBs
std::map< uint32_t, semrel::AxisAlignedBoundingBox > getAABBs(const pcl::PointCloud< PointT > &pointCloud, const std::map< uint32_t, IndicesT > &segmentIndices)
Get the AABB of each point cloud segment.
Definition: shapes_from_aabbs.h:90
armarx::semantic::detail
Definition: shapes_from_aabbs.h:75
segments.h
armarx::aron::simox::arondto::AxisAlignedBoundingBox
::simox::arondto::AxisAlignedBoundingBox AxisAlignedBoundingBox
Definition: simox.h:14
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
pcl::graph::indices
pcl::PointIndices::Ptr indices(const PCG &g)
Retrieve the indices of the points of the point cloud stored in a point cloud graph that actually bel...
Definition: point_cloud_graph.h:717
visionx::tools::getSegmentAABBs
std::vector< simox::AxisAlignedBoundingBox > getSegmentAABBs(const pcl::PointCloud< PointT > &pointCloud, const std::vector< IndicesT > &segmentIndices)
Definition: segments.h:168
armarx::PointT
pcl::PointXYZRGBL PointT
Definition: Common.h:30
armarx::semantic::getAABB
semrel::AxisAlignedBoundingBox getAABB(const pcl::PointCloud< PointT > &pointCloud, const IndicesT &segmentIndices)
Get the AABB of the given point cloud segment.
Definition: shapes_from_aabbs.h:83
visionx::tools::getAABB
simox::AxisAlignedBoundingBox getAABB(const PointCloudT &pointCloud, const pcl::PointIndices &indices={})
Get the axis-aligned bounding-box of the given point cloud.
Definition: AABB.h:26
armarx::semantic::detail::checkLessEqual
void checkLessEqual(float a, float b, const std::string &msg="")
Definition: shapes_from_aabbs.cpp:84
armarx::semantic
Definition: ShapesSupportRelations.cpp:32
armarx::semantic::getSoftAABBs
std::map< uint32_t, semrel::AxisAlignedBoundingBox > getSoftAABBs(const pcl::PointCloud< PointT > &pointCloud, const std::map< uint32_t, pcl::PointIndices > &segmentIndices, float outlierRatio)
Get the soft AABBs of each point cloud segment.
Definition: shapes_from_aabbs.h:136
armarx::semantic::getSoftAABB
semrel::AxisAlignedBoundingBox getSoftAABB(const pcl::PointCloud< PointT > &pointCloud, const pcl::PointIndices &segmentIndices, float outlierRatio)
Get the soft AABB of the given point cloud segment.
Definition: shapes_from_aabbs.h:103