Skip to content

Commit 025af2d

Browse files
jlblancocjlblancoc
committed
Add TOrientedBox
1 parent 51dd2fe commit 025af2d

File tree

4 files changed

+441
-20
lines changed

4 files changed

+441
-20
lines changed

‎modules/mrpt_math/CMakeLists.txt‎

Lines changed: 23 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -31,17 +31,17 @@ endif()
3131
set(LIB_SRCS
3232
src/A_MatrixBase_instantiate_CMatrixDynamic_d.cpp
3333
src/A_MatrixBase_instantiate_CMatrixDynamic_f.cpp
34-
src/CAtan2LookUpTable.cpp
3534
src/CAtan2LookUpTable_unittest.cpp
35+
src/CAtan2LookUpTable.cpp
3636
src/CHistogram.cpp
3737
src/CLevenbergMarquardt_unittest.cpp
3838
src/CMatrixB.cpp
3939
src/CMatrixD.cpp
40-
src/CMatrixDynamic.cpp
4140
src/CMatrixDynamic_unittest.cpp
41+
src/CMatrixDynamic.cpp
4242
src/CMatrixF.cpp
43-
src/CMatrixFixed.cpp
4443
src/CMatrixFixed_unittest.cpp
44+
src/CMatrixFixed.cpp
4545
src/container_ops_unittest.cpp
4646
src/CPolygon.cpp
4747
src/CSplineInterpolator1D.cpp
@@ -51,50 +51,52 @@ set(LIB_SRCS
5151
src/EigenAlignment_unittest.cpp
5252
src/filters.cpp
5353
src/fourier.cpp
54-
src/fresnel.cpp
5554
src/fresnel_unittest.cpp
56-
src/geometry.cpp
55+
src/fresnel.cpp
5756
src/geometry_unittest.cpp
57+
src/geometry.cpp
5858
src/KDTreeCapable_unittest.cpp
5959
src/kmeans.cpp
6060
src/kmeans++/KMeans.cpp
6161
src/kmeans++/KmTree.cpp
6262
src/kmeans++/KmUtils.cpp
6363
src/math.cpp
64-
src/MatrixBase_instantiate_CMatrixFixed_22_d.cpp
65-
src/MatrixBase_instantiate_CMatrixFixed_22_f.cpp
66-
src/MatrixBase_instantiate_CMatrixFixed_33_d.cpp
67-
src/MatrixBase_instantiate_CMatrixFixed_33_f.cpp
64+
src/matrix_ops_unittest.cpp
6865
src/matrix_ops1_unittest.cpp
6966
src/matrix_ops2_unittest.cpp
7067
src/matrix_ops3_unittest.cpp
7168
src/matrix_ops4_unittest.cpp
7269
src/matrix_ops5_unittest.cpp
73-
src/matrix_ops_unittest.cpp
70+
src/matrix_yaml_unittest.cpp
71+
src/MatrixBase_instantiate_CMatrixFixed_22_d.cpp
72+
src/MatrixBase_instantiate_CMatrixFixed_22_f.cpp
73+
src/MatrixBase_instantiate_CMatrixFixed_33_d.cpp
74+
src/MatrixBase_instantiate_CMatrixFixed_33_f.cpp
7475
src/MatrixVectorBase_instantiate_CMatrixDynamic_d.cpp
7576
src/MatrixVectorBase_instantiate_CMatrixDynamic_f.cpp
7677
src/MatrixVectorBase_instantiate_CMatrixDynamic_ints.cpp
7778
src/MatrixVectorBase_instantiate_CMatrixFixed.cpp
7879
src/MatrixVectorBase_instantiate_CVectorDynamic.cpp
7980
src/MatrixVectorBase_instantiate_CVectorFixed.cpp
80-
src/matrix_yaml_unittest.cpp
8181
src/model_search.cpp
82-
src/poly_roots.cpp
8382
src/poly_roots_unittest.cpp
83+
src/poly_roots.cpp
8484
src/ransac_applications.cpp
8585
src/registerAllClasses.cpp
8686
src/robust_kernels_unittest.cpp
87-
src/slerp.cpp
88-
src/slerp_unittest.cpp
8987
src/S_MatrixBase_instantiate_CMatrixFixed_66_d.cpp
9088
src/S_MatrixBase_instantiate_CMatrixFixed_66_f.cpp
9189
src/S_MatrixBase_instantiate_CMatrixFixed_77_d.cpp
92-
src/TBoundingBox.cpp
90+
src/slerp_unittest.cpp
91+
src/slerp.cpp
9392
src/TBoundingBox_unittest.cpp
93+
src/TBoundingBox.cpp
9494
src/TLine2D.cpp
9595
src/TLine3D.cpp
9696
src/TObject2D.cpp
9797
src/TObject3D.cpp
98+
src/TOrientedBox_unittest.cpp
99+
src/TOrientedBox.cpp
98100
src/TPlane.cpp
99101
src/TPoint2D.cpp
100102
src/TPoint3D.cpp
@@ -150,22 +152,22 @@ set(LIB_PUBLIC_HDRS
150152
include/mrpt/math/mat2eig.h
151153
include/mrpt/math/math_frwds.h
152154
include/mrpt/math/matrix_adaptors.h
153-
include/mrpt/math/MatrixBase.h
154-
include/mrpt/math/MatrixBlockSparseCols.h
155155
include/mrpt/math/matrix_serialization.h
156156
include/mrpt/math/matrix_size_t.h
157+
include/mrpt/math/MatrixBase.h
158+
include/mrpt/math/MatrixBlockSparseCols.h
157159
include/mrpt/math/MatrixVectorBase.h
158-
include/mrpt/math/model_search.h
159160
include/mrpt/math/model_search_impl.h
161+
include/mrpt/math/model_search.h
160162
include/mrpt/math/num_jacobian.h
161163
include/mrpt/math/ops_containers.h
162164
include/mrpt/math/ops_matrices.h
163165
include/mrpt/math/ops_vectors.h
164166
include/mrpt/math/point_poses2vectors.h
165167
include/mrpt/math/poly_roots.h
166168
include/mrpt/math/ransac_applications.h
167-
include/mrpt/math/ransac.h
168169
include/mrpt/math/ransac_impl.h
170+
include/mrpt/math/ransac.h
169171
include/mrpt/math/registerAllClasses.h
170172
include/mrpt/math/robust_kernels.h
171173
include/mrpt/math/slerp.h
@@ -174,6 +176,7 @@ set(LIB_PUBLIC_HDRS
174176
include/mrpt/math/TLine3D.h
175177
include/mrpt/math/TObject2D.h
176178
include/mrpt/math/TObject3D.h
179+
include/mrpt/math/TOrientedBox.h
177180
include/mrpt/math/TPlane.h
178181
include/mrpt/math/TPoint2D.h
179182
include/mrpt/math/TPoint3D.h
@@ -189,8 +192,8 @@ set(LIB_PUBLIC_HDRS
189192
include/mrpt/math/TSegment3D.h
190193
include/mrpt/math/TTwist2D.h
191194
include/mrpt/math/TTwist3D.h
192-
include/mrpt/math/utils.h
193195
include/mrpt/math/utils_matlab.h
196+
include/mrpt/math/utils.h
194197
include/mrpt/math/wrap2pi.h
195198
)
196199

‎modules/mrpt_math/include/mrpt/math/TOrientedBox.h‎

Lines changed: 161 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,161 @@
1+
/* _
2+
| | Mobile Robot Programming Toolkit (MRPT)
3+
_ __ ___ _ __ _ __ | |_
4+
| '_ ` _ \| '__| '_ \| __| https://www.mrpt.org/
5+
| | | | | | | | |_) | |_
6+
|_| |_| |_|_| | .__/ \__| https://github.com/MRPT/mrpt/
7+
| |
8+
|_|
9+
10+
Copyright (c) 2005-2025, Individual contributors, see AUTHORS file
11+
See: https://www.mrpt.org/Authors - All rights reserved.
12+
SPDX-License-Identifier: BSD-3-Clause
13+
*/
14+
15+
#pragma once
16+
17+
#include <mrpt/core/exceptions.h>
18+
#include <mrpt/math/TBoundingBox.h>
19+
#include <mrpt/math/TPlane.h>
20+
#include <mrpt/math/TPoint3D.h>
21+
#include <mrpt/math/TPose3D.h>
22+
#include <mrpt/serialization/serialization_frwds.h>
23+
#include <mrpt/typemeta/TTypeName.h>
24+
25+
#include <optional>
26+
#include <vector>
27+
28+
namespace mrpt::math
29+
{
30+
/** \addtogroup geometry_grp
31+
* @{ */
32+
33+
/** \brief 3D oriented bounding box: defined by dimensions and pose
34+
*
35+
* \details The box is described by the SE(3) pose of its geometric
36+
* center, plus its lengths along the local (X',Y',Z') axes determined
37+
* by that pose.
38+
*
39+
* Numbering of vertices:
40+
* \code
41+
* 4 +---------+ 6
42+
* /| /|
43+
* / | / |
44+
* / | / |
45+
* 5 +---------+ 7 | +Z'
46+
* | | | | ^
47+
* | | | | |
48+
* | 0 +----|---+ 2 +---> +Y'
49+
* | / | / /
50+
* | / | / v
51+
* |/ |/ +X'
52+
* 1 +---------+ 3
53+
* \endcode
54+
*/
55+
template <typename T>
56+
class TOrientedBox_
57+
{
58+
public:
59+
/** The number of planes defined by a box */
60+
constexpr static std::size_t PLANES_PER_BOX = 6;
61+
62+
TOrientedBox_() = default;
63+
64+
/** Constructor pose of the center and dimensions in local coordinates */
65+
TOrientedBox_(const mrpt::math::TPose3D& center_pose, const mrpt::math::TPoint3D_<T>& box_size) :
66+
m_pose(center_pose), m_size(box_size)
67+
{
68+
}
69+
70+
TOrientedBox_(const TOrientedBox_<T>& o) : m_pose(o.pose()), m_size(o.size()) {}
71+
TOrientedBox_<T>& operator=(const TOrientedBox_<T>& o)
72+
{
73+
setPose(o.pose());
74+
setSize(o.size());
75+
return *this;
76+
}
77+
78+
template <typename U>
79+
mrpt::math::TOrientedBox_<U> cast() const
80+
{
81+
return {m_pose, m_size.template cast<U>()};
82+
}
83+
84+
/** Gets the 8 vertices (cached to avoid recalculation).
85+
* The order or vertices is ensured to be as defined above */
86+
[[nodiscard]] const std::vector<mrpt::math::TPoint3D_<T>>& vertices() const;
87+
88+
/** Gets the box center pose */
89+
[[nodiscard]] const mrpt::math::TPose3D& pose() const { return m_pose; }
90+
91+
/** Changes the the box center SE(3) pose */
92+
void setPose(const mrpt::math::TPose3D& p)
93+
{
94+
m_pose = p;
95+
m_vertices.reset();
96+
}
97+
98+
/** Gets the box size (local coordinates) */
99+
[[nodiscard]] const mrpt::math::TPoint3D_<T>& size() const { return m_size; }
100+
101+
/** Changes the box size (local coordinates) */
102+
void setSize(const mrpt::math::TPoint3D_<T>& size)
103+
{
104+
m_size = size;
105+
m_vertices.reset();
106+
}
107+
108+
/** Returns a human-readable description of this box */
109+
[[nodiscard]] std::string asString() const;
110+
111+
/// @brief Returns the axis-aligned bounding box enclosing this oriented box
112+
[[nodiscard]] TBoundingBox_<T> getAxisAlignedBox() const;
113+
114+
/** Returns the 6 planes enclosing the 3D box.
115+
* The ordering of the planes is: [0]=front, [1]=left, [2]=right, [3]=top, [4]=back, [5]=bottom.
116+
* Plane normals are all pointing outwards
117+
*/
118+
[[nodiscard]] std::array<mrpt::math::TPlane, PLANES_PER_BOX> getBoxPlanes() const;
119+
120+
[[nodiscard]] bool operator==(const TOrientedBox_<T>& o) const
121+
{
122+
return m_pose == o.m_pose && m_size == o.m_size;
123+
}
124+
125+
[[nodiscard]] bool operator!=(const TOrientedBox_<T>& o) const
126+
{
127+
return m_pose != o.m_pose || m_size != o.m_size;
128+
}
129+
130+
private:
131+
mrpt::math::TPose3D m_pose;
132+
mrpt::math::TPoint3D_<T> m_size{0, 0, 0};
133+
mutable std::optional<std::vector<mrpt::math::TPoint3D_<T>>> m_vertices;
134+
};
135+
136+
/** 3D oriented bounding box, defined by dimensions and pose
137+
* \sa mrpt::math::TBoundingBox
138+
*/
139+
using TOrientedBox = TOrientedBox_<double>;
140+
using TOrientedBoxf = TOrientedBox_<float>;
141+
142+
mrpt::serialization::CArchive& operator>>(
143+
mrpt::serialization::CArchive& in, mrpt::math::TOrientedBox& bb);
144+
mrpt::serialization::CArchive& operator<<(
145+
mrpt::serialization::CArchive& out, const mrpt::math::TOrientedBox& bb);
146+
147+
mrpt::serialization::CArchive& operator>>(
148+
mrpt::serialization::CArchive& in, mrpt::math::TOrientedBoxf& bb);
149+
mrpt::serialization::CArchive& operator<<(
150+
mrpt::serialization::CArchive& out, const mrpt::math::TOrientedBoxf& bb);
151+
152+
/** @} */
153+
154+
} // namespace mrpt::math
155+
156+
namespace mrpt::typemeta
157+
{
158+
// Specialization must occur in the same namespace
159+
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TOrientedBox, mrpt::math)
160+
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TOrientedBoxf, mrpt::math)
161+
} // namespace mrpt::typemeta

0 commit comments

Comments
 (0)