Skip to content

Commit d2ac87f

Browse files
author
Martin Garbade
committed
Merge branch 'master' into martin
Merging master into martin
2 parents 3d62cee + 278a515 commit d2ac87f

9 files changed

+371
-62
lines changed

‎CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,7 @@ add_executable(voxelizer
6666

6767
src/data/label_utils.cpp
6868
src/data/kitti_utils.cpp
69+
src/data/VoxelGrid.cpp
6970
src/rv/string_utils.cpp
7071
src/rv/Stopwatch.cpp
7172
src/data/misc.cpp

‎src/VoxelizerMainFrame.ui

+10-3
Original file line numberDiff line numberDiff line change
@@ -166,7 +166,7 @@
166166
<x>0</x>
167167
<y>0</y>
168168
<width>232</width>
169-
<height>459</height>
169+
<height>455</height>
170170
</rect>
171171
</property>
172172
<attribute name="label">
@@ -301,6 +301,13 @@
301301
</property>
302302
</widget>
303303
</item>
304+
<item row="7" column="0" colspan="2">
305+
<widget class="QCheckBox" name="chkShowOccluded">
306+
<property name="text">
307+
<string>occluded voxels</string>
308+
</property>
309+
</widget>
310+
</item>
304311
</layout>
305312
</item>
306313
<item>
@@ -323,8 +330,8 @@
323330
<rect>
324331
<x>0</x>
325332
<y>0</y>
326-
<width>146</width>
327-
<height>126</height>
333+
<width>153</width>
334+
<height>129</height>
328335
</rect>
329336
</property>
330337
<attribute name="label">

‎src/data/VoxelGrid.cpp

+171
Original file line numberDiff line numberDiff line change
@@ -1 +1,172 @@
11
#include <data/VoxelGrid.h>
2+
3+
#include <iostream>
4+
5+
void VoxelGrid::initialize(float resolution, const Eigen::Vector4f& min, const Eigen::Vector4f& max) {
6+
clear();
7+
8+
resolution_ = resolution;
9+
sizex_ = std::ceil((max.x() - min.x()) / resolution_);
10+
sizey_ = std::ceil((max.y() - min.y()) / resolution_);
11+
sizez_ = std::ceil((max.z() - min.z()) / resolution_);
12+
13+
voxels_.resize(sizex_ * sizey_ * sizez_);
14+
// ensure that min, max are always inside the voxel grid.
15+
float ox = min.x() - 0.5 * (sizex_ * resolution - (max.x() - min.x()));
16+
float oy = min.y() - 0.5 * (sizey_ * resolution - (max.y() - min.y()));
17+
float oz = min.z() - 0.5 * (sizez_ * resolution - (max.z() - min.z()));
18+
offset_ = Eigen::Vector4f(ox, oy, oz, 1);
19+
20+
// center_.head(3) = 0.5 * (max - min) + min;
21+
// center_[3] = 0;
22+
23+
occlusions_.resize(sizex_ * sizey_ * sizez_);
24+
}
25+
26+
void VoxelGrid::clear() {
27+
for (auto idx : occupied_) {
28+
voxels_[idx].count = 0;
29+
voxels_[idx].labels.clear();
30+
}
31+
32+
for (auto idx : occluded_) {
33+
voxels_[idx].count = 0;
34+
voxels_[idx].labels.clear();
35+
}
36+
37+
occupied_.clear();
38+
occluded_.clear();
39+
}
40+
41+
void VoxelGrid::insert(const Eigen::Vector4f& p, uint32_t label) {
42+
Eigen::Vector4f tp = p - offset_;
43+
int32_t i = std::floor(tp.x() / resolution_);
44+
int32_t j = std::floor(tp.y() / resolution_);
45+
int32_t k = std::floor(tp.z() / resolution_);
46+
47+
if ((i >= int32_t(sizex_)) || (j >= int32_t(sizey_)) || (k >= int32_t(sizez_))) return;
48+
if ((i < 0) || (j < 0) || (k < 0)) return;
49+
50+
int32_t gidx = index(i, j, k);
51+
if (gidx < 0 || gidx >= int32_t(voxels_.size())) return;
52+
53+
occupied_.push_back(gidx);
54+
55+
// float n = voxels_[gidx].count;
56+
voxels_[gidx].labels[label] += 1; //(1. / (n + 1)) * (n * voxels_[gidx].point + p);
57+
voxels_[gidx].count += 1;
58+
occlusionsValid_ = false;
59+
}
60+
61+
bool VoxelGrid::isOccluded(int32_t i, int32_t j, int32_t k) const { return occlusions_[index(i, j, k)] > -1; }
62+
63+
bool VoxelGrid::isFree(int32_t i, int32_t j, int32_t k) const { return occlusions_[index(i, j, k)] == -1; }
64+
65+
void VoxelGrid::insertOcclusionLabels() {
66+
if (!occlusionsValid_) updateOcclusions();
67+
68+
for (uint32_t i = 0; i < sizex_; ++i) {
69+
for (uint32_t j = 0; j < sizey_; ++j) {
70+
for (uint32_t k = 0; k < sizez_; ++k) {
71+
// heuristic: find label from above.
72+
if (occlusions_[index(i, j, k)] != index(i, j, k)) {
73+
int32_t n = 1;
74+
while ((k + n < sizez_) && isOccluded(i, j, k + n) && voxels_[index(i, j, k + n)].count == 0) n += 1;
75+
if (k + n < sizez_ && voxels_[index(i, j, k + n)].count > 0) {
76+
voxels_[index(i, j, k)].count = voxels_[index(i, j, k + n)].count;
77+
voxels_[index(i, j, k)].labels = voxels_[index(i, j, k + n)].labels;
78+
}
79+
}
80+
}
81+
}
82+
}
83+
}
84+
85+
void VoxelGrid::updateOcclusions() {
86+
for (uint32_t i = 0; i < sizex_; ++i) {
87+
for (uint32_t j = 0; j < sizey_; ++j) {
88+
for (uint32_t k = 0; k < sizez_; ++k) {
89+
occlusions_[index(i, j, k)] = occludedBy(i, j, k);
90+
if (occlusions_[index(i, j, k)] != index(i, j, k)) occluded_.push_back(index(i, j, k));
91+
}
92+
}
93+
}
94+
95+
occlusionsValid_ = true;
96+
}
97+
98+
int32_t VoxelGrid::occludedBy(int32_t i, int32_t j, int32_t k, std::vector<Eigen::Vector3i>* visited) const {
99+
float NextCrossingT[3], DeltaT[3]; /** t for next intersection with voxel boundary of axis, t increment for axis
100+
**/
101+
int32_t Step[3], Out[3], Pos[3]; /** voxel increment for axis, index of of outside voxels, current position **/
102+
float dir[3]; /** ray direction **/
103+
104+
if (visited != nullptr) visited->clear();
105+
106+
Pos[0] = i;
107+
Pos[1] = j;
108+
Pos[2] = k;
109+
110+
/** calculate direction vector assuming sensor at (0,0,_heightOffset) **/
111+
Eigen::Vector3f startpoint = voxel2position(Pos[0], Pos[1], Pos[2]);
112+
113+
double halfResolution = 0.5 * resolution_;
114+
115+
// startpoint[0] += halfResolution;
116+
// startpoint[1] += halfResolution;
117+
// startpoint[2] += halfResolution;
118+
119+
dir[0] = -startpoint[0];
120+
dir[1] = -startpoint[1];
121+
dir[2] = -startpoint[2];
122+
123+
/** initialize variables for traversal **/
124+
for (uint32_t axis = 0; axis < 3; ++axis) {
125+
if (dir[axis] < 0) {
126+
NextCrossingT[axis] = -halfResolution / dir[axis];
127+
DeltaT[axis] = -resolution_ / dir[axis];
128+
Step[axis] = -1;
129+
Out[axis] = 0;
130+
} else {
131+
NextCrossingT[axis] = halfResolution / dir[axis];
132+
DeltaT[axis] = resolution_ / dir[axis];
133+
Step[axis] = 1;
134+
Out[axis] = size(axis);
135+
}
136+
}
137+
138+
Eigen::Vector3f endpoint(0, 0, 0);
139+
Eigen::Vector3i endindexes = position2voxel(endpoint);
140+
int32_t i_end = endindexes[0];
141+
int32_t j_end = endindexes[1];
142+
int32_t k_end = endindexes[2];
143+
144+
const int32_t cmpToAxis[8] = {2, 1, 2, 1, 2, 2, 0, 0};
145+
int32_t iteration = 0;
146+
for (;;) // loop infinitely...
147+
{
148+
if (Pos[0] < 0 || Pos[1] < 0 || Pos[2] < 0) break;
149+
if (Pos[0] >= int32_t(sizex_) || Pos[1] >= int32_t(sizey_) || Pos[2] >= int32_t(sizez_)) break;
150+
151+
int32_t idx = index(Pos[0], Pos[1], Pos[2]);
152+
bool occupied = voxels_[idx].count > 0;
153+
if (visited != nullptr) visited->push_back(Eigen::Vector3i(Pos[0], Pos[1], Pos[2]));
154+
155+
if (occupied) return idx;
156+
157+
int32_t bits = ((NextCrossingT[0] < NextCrossingT[1]) << 2) + ((NextCrossingT[0] < NextCrossingT[2]) << 1) +
158+
((NextCrossingT[1] < NextCrossingT[2]));
159+
int32_t stepAxis = cmpToAxis[bits]; /* branch-free looping */
160+
161+
Pos[stepAxis] += Step[stepAxis];
162+
NextCrossingT[stepAxis] += DeltaT[stepAxis];
163+
164+
/** note the first condition should never happen, since we want to reach a point inside the grid **/
165+
if (Pos[stepAxis] == Out[stepAxis]) break; //... until outside, and leaving the loop here!
166+
if (Pos[0] == i_end && Pos[1] == j_end && Pos[2] == k_end) break; // .. or the sensor origin is reached.
167+
168+
++iteration;
169+
}
170+
171+
return -1;
172+
}

‎src/data/VoxelGrid.h

+55-48
Original file line numberDiff line numberDiff line change
@@ -21,73 +21,80 @@ class VoxelGrid {
2121
uint32_t count{0};
2222
};
2323

24-
void initialize(float resolution, const Eigen::Vector4f& min, const Eigen::Vector4f& max) {
25-
clear();
26-
27-
resolution_ = resolution;
28-
sizex_ = std::ceil((max.x() - min.x()) / resolution_);
29-
sizey_ = std::ceil((max.y() - min.y()) / resolution_);
30-
sizez_ = std::ceil((max.z() - min.z()) / resolution_);
31-
32-
voxels_.resize(sizex_ * sizey_ * sizez_);
33-
// ensure that min, max are always inside the voxel grid.
34-
float ox = min.x() - 0.5 * (sizex_ * resolution - (max.x() - min.x()));
35-
float oy = min.y() - 0.5 * (sizey_ * resolution - (max.y() - min.y()));
36-
float oz = min.z() - 0.5 * (sizez_ * resolution - (max.z() - min.z()));
37-
offset_ = Eigen::Vector4f(ox, oy, oz, 1);
38-
39-
// center_.head(3) = 0.5 * (max - min) + min;
40-
// center_[3] = 0;
41-
}
42-
43-
Voxel& operator()(uint32_t i, uint32_t j, uint32_t k) { return voxels_[i + j * sizex_ + k * sizex_ * sizey_]; }
44-
const Voxel& operator()(uint32_t i, uint32_t j, uint32_t k) const {
45-
return voxels_[i + j * sizex_ + k * sizex_ * sizey_];
46-
}
47-
48-
void clear() {
49-
for (auto idx : occupied_) {
50-
voxels_[idx].count = 0;
51-
voxels_[idx].labels.clear();
52-
}
53-
occupied_.clear();
54-
}
55-
56-
void insert(const Eigen::Vector4f& p, uint32_t label) {
57-
Eigen::Vector4f tp = p - offset_;
58-
int32_t i = std::floor(tp.x() / resolution_);
59-
int32_t j = std::floor(tp.y() / resolution_);
60-
int32_t k = std::floor(tp.z() / resolution_);
61-
62-
if ((i >= int32_t(sizex_)) || (j >= int32_t(sizey_)) || (k >= int32_t(sizez_))) return;
63-
if ((i < 0) || (j < 0) || (k < 0)) return;
64-
65-
int32_t gidx = i + j * sizex_ + k * sizex_ * sizey_;
66-
if (gidx < 0 || gidx >= int32_t(voxels_.size())) return;
24+
/** \brief set parameters of voxel grid and compute internal offsets, etc. **/
25+
void initialize(float resolution, const Eigen::Vector4f& min, const Eigen::Vector4f& max);
6726

68-
occupied_.push_back(gidx);
27+
Voxel& operator()(uint32_t i, uint32_t j, uint32_t k) { return voxels_[index(i, j, k)]; }
28+
const Voxel& operator()(uint32_t i, uint32_t j, uint32_t k) const { return voxels_[index(i, j, k)]; }
6929

70-
// float n = voxels_[gidx].count;
71-
voxels_[gidx].labels[label] += 1; //(1. / (n + 1)) * (n * voxels_[gidx].point + p);
72-
voxels_[gidx].count += 1;
73-
}
30+
/** \brief cleanup voxelgrid. **/
31+
void clear();
7432

33+
/** \brief add label for specific point to voxel grid **/
34+
void insert(const Eigen::Vector4f& p, uint32_t label);
35+
/** \brief get all voxels. **/
7536
const std::vector<Voxel>& voxels() const { return voxels_; }
7637

7738
const Eigen::Vector4f& offset() const { return offset_; }
7839

40+
/** \brief get size in specific dimension **/
7941
uint32_t size(uint32_t dim) const { return (&sizex_)[std::max<uint32_t>(std::min<uint32_t>(dim, 3), 0)]; }
8042

43+
/** \brief resolutions aka sidelength of a voxel **/
8144
float resolution() const { return resolution_; }
8245

46+
/** \brief check for each voxel if there is voxel occluding the voxel. **/
47+
void updateOcclusions();
48+
49+
/** \brief fill occluded areas with labels. **/
50+
void insertOcclusionLabels();
51+
52+
/** \brief get if voxel at (i,j,k) is occluded.
53+
* Assumes that updateOcclusions has been called before,
54+
*
55+
* \see updateOcclusions
56+
**/
57+
bool isOccluded(int32_t i, int32_t j, int32_t k) const;
58+
/** \brief get if voxel at (i,j,k) is free.
59+
* Assumes that updateOcclusions has been called before,
60+
*
61+
* \see updateOcclusions
62+
**/
63+
bool isFree(int32_t i, int32_t j, int32_t k) const;
64+
65+
/** \brief check if given voxel is occluded.
66+
*
67+
*
68+
* \return index of voxel that occludes given voxel, -1 if voxel is not occluded.
69+
**/
70+
int32_t occludedBy(int32_t i, int32_t j, int32_t k, std::vector<Eigen::Vector3i>* visited = nullptr) const;
71+
8372
protected:
73+
/** \brief get position of voxel center. **/
74+
inline Eigen::Vector3f voxel2position(int32_t i, int32_t j, int32_t k) const {
75+
return Eigen::Vector3f(offset_[0] + i * resolution_ + 0.5 * resolution_,
76+
offset_[1] + j * resolution_ + 0.5 * resolution_,
77+
offset_[2] + k * resolution_ + 0.5 * resolution_);
78+
}
79+
80+
inline Eigen::Vector3i position2voxel(const Eigen::Vector3f& pos) const {
81+
return Eigen::Vector3i((pos[0] - offset_[0]) / resolution_, (pos[1] - offset_[1]) / resolution_,
82+
(pos[2] - offset_[2]) / resolution_);
83+
}
84+
85+
inline int32_t index(int32_t i, int32_t j, int32_t k) const { return i + j * sizex_ + k * sizex_ * sizey_; }
86+
8487
float resolution_;
8588

8689
uint32_t sizex_, sizey_, sizez_;
8790
std::vector<Voxel> voxels_;
8891
std::vector<uint32_t> occupied_;
8992

9093
Eigen::Vector4f offset_;
94+
95+
std::vector<int32_t> occlusions_; // filled by updateOcclusions.
96+
bool occlusionsValid_{false};
97+
std::vector<uint32_t> occluded_; // filled by updateOcclusions.
9198
};
9299

93100
#endif /* SRC_DATA_VOXELGRID_H_ */

‎src/shaders/draw_voxels.vert

+3-1
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,8 @@ layout (location = 0) in vec3 in_position;
44
layout (location = 1) in uint in_label;
55

66
uniform sampler2DRect label_colors;
7-
7+
uniform vec4 custom_color;
8+
uniform bool use_custom_color;
89

910
#include "shaders/color.glsl"
1011

@@ -18,6 +19,7 @@ out VOXEL {
1819
void main()
1920
{
2021
vec4 in_color = texture(label_colors, vec2(in_label, 0));
22+
if (use_custom_color) in_color = custom_color;
2123

2224
vs_out.pos = in_position;
2325
vs_out.color = in_color;

0 commit comments

Comments
 (0)