VoxelGridStructure.h
Go to the documentation of this file.
1 #pragma once
2
3 #include <Eigen/Geometry>
4
5
6 namespace Eigen
7 {
9 }
10
11 namespace visionx::voxelgrid
12 {
13 }
14
15
16 namespace visionx::voxelgrid
17 {
18
19  /*
20  /// A flat index, i.e. running index of a voxel in { 0, ..., number of voxels - 1 }.
21  using FlatIndex = std::size_t;
22  /// A grid index, i.e. index of voxel along each dimension (can be negative).
23  using GridIndex = Eigen::Vector3i;
24  */
25
26  /**
27  * @brief Geometric structure of a 3D voxel grid.
28  *
29  * The grid structure is defined by the voxel size(s) and grid size(s).
30  * The voxel size (R^3) is the extent of a single voxel along each
31  * axis (x, y, z). (Cubic voxels have constant extent along all axes).
32  * The grid size (N^3) is the number of voxels along each axis.
33  * (Cubic grids have constant number of voxels along all axes.)
34  *
35  * In addition, the voxel grid can have a pose, i.e. position and orientation.
36  *
37  * The total number of voxels to be contained in a structure is the
38  * product of the grid size coefficients.
39  *
40  *
41  * @par Grid Size and Voxel Index
42  *
43  * To indicate a voxel, one can use either of:
44  * - a grid index i = (i_x i_y i_z) in Z^3 (Eigen::Vector3i)
45  * - a flat index j in N^1 (std::size_t)
46  *
47  * A grid index specifies the voxel index along each axis. A flat index
48  * specifies a voxel's running index (from 0 to #voxels - 1).
49  *
50  * Each coefficient of a grid index is in the range
51  * `[ - gridSize/2, gridSize/2 )`
52  * More precisely, it is in the set
53  * `{ - floor(gridSize/2), ..., 0, ..., ceil(gridSize/2) - 1 }`
54  *
55  * In other words,
56  * - for an ODD grid size: [ - floor(gridSize/2), floor(gridSize/2) ]
57  * - for an EVEN grid size: [ - gridSize/2 , gridSize/2 - 1 ]
58  *
59  * For example (along one axis):
60  *
61  * | Grid Size | Valid grid indices |
62  * +-----------+--------------------------+
63  * | 3 | { -1, 0, 1 } |
64  * | 4 | { -2, -1, 0, 1 } |
65  * | 5 | { -2, -1, 0, 1, 2 } |
66  *
67  * Note that grid indices are centered around zero and can be negative.
68  * A flat index is always non-negative.
69  *
70  *
71  * @par Voxel Grid Origin
72  *
73  * The grid origin coincides with the center of voxel (0 0 0) and can be
74  * chosen freely upon construction. It differs from the grid center in
75  * axes with even grid size.
76  *
77  * @code
78  *
79  * odd grid size (3) even grid size (4)
80  * +---+---+---+ +---+---+---+---+ ^
81  * | | | | 1 | | | | | 1 | voxel size
82  * +---+---+---+ +---+---+---+---+ v
83  * c | | o | | 0 | | | o | | 0
84  * +---+---+---+ +---+---+---+---+----- c
85  * | | | | -1 | | | | | -1
86  * +---+---+---+ +---+---+---+---+
87  * -1 0 1 | | | | | -2
88  * c +---+---+---+---+
89  * -2 -1 | 0 1
90  * c
91  * (i = origin, c = center)
92  *
93  * @endcode
94  *
95  */
97  {
98  public:
99
100  /// Construct an empty grid structure (0 voxels with size 1.0).
102
103  /// Construct a voxel grid structure with cubic voxels and cubic grid.
104  VoxelGridStructure(int gridSize,
105  float voxelSize,
106  const Eigen::Vector3f& origin = Eigen::Vector3f::Zero(),
107  const Eigen::Quaternionf& orientation = Eigen::Quaternionf::Identity());
108
109  /// Construct a voxel grid structure with cubic voxels.
110  VoxelGridStructure(const Eigen::Vector3i& gridSize,
111  float voxelSize,
112  const Eigen::Vector3f& origin = Eigen::Vector3f::Zero(),
113  const Eigen::Quaternionf& orientation = Eigen::Quaternionf::Identity());
114
115  /// Construct a voxel grid structure with cubic grid.
116  VoxelGridStructure(int gridSize,
117  const Eigen::Vector3f& voxelSize,
118  const Eigen::Vector3f& origin = Eigen::Vector3f::Zero(),
119  const Eigen::Quaternionf& orientation = Eigen::Quaternionf::Identity());
120
121  /// Construct a voxel grid structure.
122  VoxelGridStructure(const Eigen::Vector3i& gridSize,
123  const Eigen::Vector3f& voxelSize,
124  const Eigen::Vector3f& origin = Eigen::Vector3f::Zero(),
125  const Eigen::Quaternionf& orientation = Eigen::Quaternionf::Identity());
126
127
128  /// Get the grid size.
129  Eigen::Vector3i getGridSize() const;
130  /// Set the grid size for a cubic grid.
131  void setGridSize(int gridSize);
132  /// Set the grid size.
133  void setGridSize(const Eigen::Vector3i& gridSize);
134
135  /// Get the voxel size.
136  Eigen::Vector3f getVoxelSize() const;
137  /// Set the voxel size for cubic voxels.
138  void setVoxelSize(float voxelSize);
139  /// Set the voxel size.
140  void setVoxelSize(const Eigen::Vector3f& voxelSize);
141
142
143  /// Get the number of voxels contained in the structure.
144  std::size_t getNumVoxels() const;
145
146  /// Get the flat index for the given grid indices.
147  std::size_t getFlatIndex(int x, int y, int z) const;
148  /// Get the flat index for the given grid indices.
149  std::size_t getFlatIndex(const Eigen::Vector3i& indices) const;
150  /// Get the flat index into the grid data of the voxel closest to point.
151  std::size_t getFlatIndex(const Eigen::Vector3f& point, bool local = false) const;
152
153  /// Get the voxel indices of the voxel with the given index.
154  Eigen::Vector3i getGridIndex(size_t index) const;
155  /// Get the indices of the voxel closest to point.
156  /// @param local If false (default), the point is transformed to local coordinate system.
157  Eigen::Vector3i getGridIndex(const Eigen::Vector3f& point, bool local = false) const;
158
159  /// Get the minimal (along each axis) grid index.
160  Eigen::Vector3i getGridIndexMin() const;
161  /// Get the maximal (along each axis) grid index.
162  Eigen::Vector3i getGridIndexMax() const;
163
164  /// Get the center of the voxel with the given indices.
165  Eigen::Vector3f getVoxelCenter(std::size_t index, bool local = false) const;
166  /// Get the center of the voxel with the given indices.
167  Eigen::Vector3f getVoxelCenter(int x, int y, int z, bool local = false) const;
168  /// Get the center of the voxel with the given indices.
169  Eigen::Vector3f getVoxelCenter(const Eigen::Vector3i& indices, bool local = false) const;
170
171
172  /// Get the grid origin in the world frame (center of voxel [0 0 0]).
173  /// Note that this differs from the geometric center for even grid sizes.
174  /// @see getCenter()
175  Eigen::Vector3f getOrigin() const;
176  /// Set the grid origin the world frame (center of voxel [0 0 0]).
177  /// Note that this differs from the geometric center for even grid sizes.
178  /// @see setCenter()
179  void setOrigin(const Eigen::Vector3f& value);
180
181  /// Get the grid orienation in the world frame.
183  /// Set the grid orienation in the world frame.
185
186  /**
187  * @brief Get the geometric center of the grid the world frame
188  * (which differs from the origin for even grid sizes).
189  */
190  Eigen::Vector3f getCenter() const;
191  /**
192  * @brief Set the geometric center of the grid the world frame
193  * (which differs from the origin for even grid sizes).
194  *
195  * @note This computes the origin from the given center using the
196  * current orientation. If you want to set grid center and orientation,
197  * set the orientation first, or (equivalently), use `setGridCenterPose()`.
198  *
199  * @see setPoseGridCenter()
200  */
201  void setCenter(const Eigen::Vector3f& center);
202
203
204  /// Get the grid pose in the world frame.
205  /// Note that this differs from the geometric center for even grid sizes.
206  /// @see getCenterPose()
207  Eigen::Matrix4f getPose() const;
208  /// Get the grid pose in the world frame.
209  /// Note that this differs from the geometric center for even grid sizes.
210  /// @see setCenterPose()
211  void setPose(const Eigen::Matrix4f& pose);
212
213  /// Get the grid pose positioned at the grid center in the world frame.
215  /// Set the grid pose so that the grid center is the position of the
216  /// given pose under the given orientation.
217  void setCenterPose(const Eigen::Matrix4f& pose);
218
219
220  /// Get extent of the grid along each axis (encompassing the whole voxels).
221  Eigen::Vector3f getExtent() const;
222  /// Get extent of the grid along each axis (encompassing only voxel centers).
223  Eigen::Vector3f getExtentOfCenters() const;
224  /// Get the local axis aligned bounding box of the grid (minimal/maximal values in columns).
226  /// Get the local axis aligned bounding box of the voxel centers (minimal/maximal values in columns).
228
229
230  /// Indicate whether the given point is inside the voxel.
231  bool isInside(const Eigen::Vector3i& indices) const;
232  /// Indicate whether the given point is inside the voxel.
233  bool isInside(const Eigen::Vector3f& point, bool local = false) const;
234
235  /// Assert that the given indices are valid grid indices.
236  void checkIsInside(const Eigen::Vector3i& indices) const;
237
238
239  /// Indicates whether `rhs` is equal to `*this`.
240  /// Uses `v.isApprox()` to compare voxel size, origin and orientation.
241  bool operator==(const VoxelGridStructure& rhs) const;
242  bool operator!=(const VoxelGridStructure& rhs) const;
243
244  /// Stream a human-readable description of the grid's structure.
245  friend std::ostream& operator<<(std::ostream& os, const VoxelGridStructure& grid);
246
247
248  private:
249
250  /// Get half the grid size.
251  Eigen::Vector3f halfGridSize() const;
252  /// Get the translation from origin to grid center (in local or global frame).
253  Eigen::Vector3f getOriginToGridCenter(bool local) const;
254
255
256  /// The number of voxels in one direction.
257  Eigen::Vector3i _gridSize = Eigen::Vector3i::Zero();
258  /// The extent of a single voxel.
259  Eigen::Vector3f _voxelSize = Eigen::Vector3f::Ones();
260
261
262
263  /// The grid origin in the world frame. Coincides with the center of voxel (0 0 0).
264  Eigen::Vector3f _origin = Eigen::Vector3f::Zero();
265  /// The grid orientation in the world frame (pivots around origin).
267
268
269  private:
270
271  /// IO format for vectors.
272  static const Eigen::IOFormat _iofVector;
273  /// IO format for sizes (delimited by 'x').
274  static const Eigen::IOFormat _iofTimes;
275
276  };
277
278
279 }
visionx::voxelgrid::VoxelGridStructure::getGridIndexMin
Eigen::Vector3i getGridIndexMin() const
Get the minimal (along each axis) grid index.
Definition: VoxelGridStructure.cpp:159
visionx::voxelgrid::VoxelGridStructure::getGridIndexMax
Eigen::Vector3i getGridIndexMax() const
Get the maximal (along each axis) grid index.
Definition: VoxelGridStructure.cpp:168
Eigen
Definition: Elements.h:36
index
uint8_t index
Definition: EtherCATFrame.h:59
visionx::voxelgrid::VoxelGridStructure::getFlatIndex
std::size_t getFlatIndex(int x, int y, int z) const
Get the flat index for the given grid indices.
Definition: VoxelGridStructure.cpp:102
visionx::voxelgrid::VoxelGridStructure::getVoxelSize
Eigen::Vector3f getVoxelSize() const
Get the voxel size.
Definition: VoxelGridStructure.cpp:82
visionx::voxelgrid::VoxelGridStructure::setGridSize
void setGridSize(int gridSize)
Set the grid size for a cubic grid.
Definition: VoxelGridStructure.cpp:72
visionx::voxelgrid::VoxelGridStructure::VoxelGridStructure
VoxelGridStructure()
Construct an empty grid structure (0 voxels with size 1.0).
Definition: VoxelGridStructure.cpp:31
visionx::voxelgrid::VoxelGridStructure::getOrientation
Eigen::Quaternionf getOrientation() const
Get the grid orienation in the world frame.
Definition: VoxelGridStructure.cpp:212
visionx::voxelgrid::VoxelGridStructure::setPose
void setPose(const Eigen::Matrix4f &pose)
Get the grid pose in the world frame.
Definition: VoxelGridStructure.cpp:237
visionx::voxelgrid::VoxelGridStructure::getPose
Eigen::Matrix4f getPose() const
Get the grid pose in the world frame.
Definition: VoxelGridStructure.cpp:232
visionx::voxelgrid::VoxelGridStructure::getExtentOfCenters
Eigen::Vector3f getExtentOfCenters() const
Get extent of the grid along each axis (encompassing only voxel centers).
Definition: VoxelGridStructure.cpp:259
visionx::voxelgrid::VoxelGridStructure::getExtent
Eigen::Vector3f getExtent() const
Get extent of the grid along each axis (encompassing the whole voxels).
Definition: VoxelGridStructure.cpp:254
visionx::voxelgrid
Definition: exceptions.cpp:8
visionx::voxelgrid::VoxelGridStructure::getLocalBoundingBoxOfCenters
Eigen::Matrix32f getLocalBoundingBoxOfCenters() const
Get the local axis aligned bounding box of the voxel centers (minimal/maximal values in columns).
Definition: VoxelGridStructure.cpp:272
visionx::voxelgrid::VoxelGridStructure::isInside
bool isInside(const Eigen::Vector3i &indices) const
Indicate whether the given point is inside the voxel.
Definition: VoxelGridStructure.cpp:280
visionx::voxelgrid::VoxelGridStructure
Geometric structure of a 3D voxel grid.
Definition: VoxelGridStructure.h:96
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
visionx::voxelgrid::VoxelGridStructure::getGridSize
Eigen::Vector3i getGridSize() const
Get the grid size.
Definition: VoxelGridStructure.cpp:67
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:737
visionx::voxelgrid::VoxelGridStructure::operator<<
friend std::ostream & operator<<(std::ostream &os, const VoxelGridStructure &grid)
Stream a human-readable description of the grid's structure.
Definition: VoxelGridStructure.cpp:341
visionx::voxelgrid::VoxelGridStructure::setCenter
void setCenter(const Eigen::Vector3f &center)
Set the geometric center of the grid the world frame (which differs from the origin for even grid siz...
Definition: VoxelGridStructure.cpp:227
visionx::voxelgrid::VoxelGridStructure::setOrigin
void setOrigin(const Eigen::Vector3f &value)
Set the grid origin the world frame (center of voxel [0 0 0]).
Definition: VoxelGridStructure.cpp:207
visionx::voxelgrid::VoxelGridStructure::setOrientation
void setOrientation(const Eigen::Quaternionf &value)
Set the grid orienation in the world frame.
Definition: VoxelGridStructure.cpp:217
visionx::voxelgrid::VoxelGridStructure::getCenterPose
Eigen::Matrix4f getCenterPose() const
Get the grid pose positioned at the grid center in the world frame.
Definition: VoxelGridStructure.cpp:243
visionx::voxelgrid::VoxelGridStructure::getVoxelCenter
Eigen::Vector3f getVoxelCenter(std::size_t index, bool local=false) const
Get the center of the voxel with the given indices.
Definition: VoxelGridStructure.cpp:178
visionx::voxelgrid::VoxelGridStructure::operator!=
bool operator!=(const VoxelGridStructure &rhs) const
Definition: VoxelGridStructure.cpp:317
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
visionx::voxelgrid::VoxelGridStructure::getNumVoxels
std::size_t getNumVoxels() const
Get the number of voxels contained in the structure.
Definition: VoxelGridStructure.cpp:97
armarx::Quaternion< float, 0 >
Eigen::Matrix32f
Matrix< float, 3, 2 > Matrix32f
A 3x2 matrix.
Definition: DebugDrawerTopic.h:21
visionx::voxelgrid::VoxelGridStructure::setVoxelSize
void setVoxelSize(float voxelSize)
Set the voxel size for cubic voxels.
Definition: VoxelGridStructure.cpp:87
visionx::voxelgrid::VoxelGridStructure::checkIsInside
void checkIsInside(const Eigen::Vector3i &indices) const
Assert that the given indices are valid grid indices.
Definition: VoxelGridStructure.cpp:300
Eigen::Matrix
Definition: EigenForwardDeclarations.h:27
visionx::voxelgrid::VoxelGridStructure::getCenter
Eigen::Vector3f getCenter() const
Get the geometric center of the grid the world frame (which differs from the origin for even grid siz...
Definition: VoxelGridStructure.cpp:222
visionx::voxelgrid::VoxelGridStructure::getOrigin
Eigen::Vector3f getOrigin() const
Get the grid origin in the world frame (center of voxel [0 0 0]).
Definition: VoxelGridStructure.cpp:202
visionx::voxelgrid::VoxelGridStructure::getGridIndex
Eigen::Vector3i getGridIndex(size_t index) const
Get the voxel indices of the voxel with the given index.
Definition: VoxelGridStructure.cpp:127
visionx::voxelgrid::VoxelGridStructure::getLocalBoundingBox
Eigen::Matrix32f getLocalBoundingBox() const
Get the local axis aligned bounding box of the grid (minimal/maximal values in columns).
Definition: VoxelGridStructure.cpp:264
visionx::voxelgrid::VoxelGridStructure::operator==
bool operator==(const VoxelGridStructure &rhs) const
Indicates whether rhs is equal to *this.
Definition: VoxelGridStructure.cpp:309
visionx::voxelgrid::VoxelGridStructure::setCenterPose
void setCenterPose(const Eigen::Matrix4f &pose)
Set the grid pose so that the grid center is the position of the given pose under the given orientati...
Definition: VoxelGridStructure.cpp:248