Skip to content

Commit d33b05a

Browse files
chutsuchutsu
committed
Add xyz_octree
1 parent 4412b98 commit d33b05a

9 files changed

+315
-13
lines changed

‎Makefile

+3-3
Original file line numberDiff line numberDiff line change
@@ -30,18 +30,18 @@ $(BLD_DIR)/test_%: src/test_%.c libxyz
3030

3131
$(BLD_DIR)/%.o: src/%.c src/%.h Makefile
3232
@echo "CC [$(notdir $<)]"
33-
$(CC) $(CFLAGS) -c $< -o $@
33+
@$(CC) $(CFLAGS) -c $< -o $@
3434

3535
$(BLD_DIR)/xyz_ceres.o: src/xyz_ceres.cpp Makefile
3636
@echo "CXX [$(notdir $<)]"
37-
g++ -Wall -O3 \
37+
@g++ -Wall -O3 \
3838
-c $< \
3939
-o $(BLD_DIR)/$(basename $(notdir $<)).o \
4040
-I/usr/include/eigen3
4141

4242
$(BLD_DIR)/libxyz.a: $(LIBXYZ_OBJS)
4343
@echo "AR [libxyz.a]"
44-
$(AR) $(ARFLAGS) \
44+
@$(AR) $(ARFLAGS) \
4545
$(BLD_DIR)/libxyz.a \
4646
$(LIBXYZ_OBJS) \
4747
> /dev/null 2>&1

‎compile_commands.json

+102
Original file line numberDiff line numberDiff line change
@@ -74,6 +74,31 @@
7474
"file": "/home/chutsu/code/xyz/src/xyz_http.c",
7575
"output": "/home/chutsu/code/xyz/build/xyz_http.o"
7676
},
77+
{
78+
"arguments": [
79+
"/usr/bin/gcc",
80+
"-Wall",
81+
"-Wpedantic",
82+
"-Wstrict-prototypes",
83+
"-g",
84+
"-fopenmp",
85+
"-fsanitize=address",
86+
"-I/home/chutsu/code/xyz",
87+
"-I/home/chutsu/code/xyz/third_party/include",
88+
"-fPIC",
89+
"-I/home/chutsu/code/xyz/third_party/src/stb",
90+
"-I/usr/include/freetype2",
91+
"-I/usr/include/libpng16",
92+
"-I/usr/include/eigen3",
93+
"-c",
94+
"-o",
95+
"build/xyz_kitti.o",
96+
"src/xyz_kitti.c"
97+
],
98+
"directory": "/home/chutsu/code/xyz",
99+
"file": "/home/chutsu/code/xyz/src/xyz_kitti.c",
100+
"output": "/home/chutsu/code/xyz/build/xyz_kitti.o"
101+
},
77102
{
78103
"arguments": [
79104
"/usr/bin/gcc",
@@ -349,6 +374,31 @@
349374
"file": "/home/chutsu/code/xyz/src/xyz_calib.c",
350375
"output": "/home/chutsu/code/xyz/build/xyz_calib.o"
351376
},
377+
{
378+
"arguments": [
379+
"/usr/bin/gcc",
380+
"-Wall",
381+
"-Wpedantic",
382+
"-Wstrict-prototypes",
383+
"-g",
384+
"-fopenmp",
385+
"-fsanitize=address",
386+
"-I/home/chutsu/code/xyz",
387+
"-I/home/chutsu/code/xyz/third_party/include",
388+
"-fPIC",
389+
"-I/home/chutsu/code/xyz/third_party/src/stb",
390+
"-I/usr/include/freetype2",
391+
"-I/usr/include/libpng16",
392+
"-I/usr/include/eigen3",
393+
"-c",
394+
"-o",
395+
"build/xyz_octree.o",
396+
"src/xyz_octree.c"
397+
],
398+
"directory": "/home/chutsu/code/xyz",
399+
"file": "/home/chutsu/code/xyz/src/xyz_octree.c",
400+
"output": "/home/chutsu/code/xyz/build/xyz_octree.o"
401+
},
352402
{
353403
"arguments": [
354404
"/usr/bin/g++",
@@ -415,6 +465,32 @@
415465
"file": "/home/chutsu/code/xyz/src/test_aprilgrid.c",
416466
"output": "/home/chutsu/code/xyz/build/test_aprilgrid"
417467
},
468+
{
469+
"arguments": [
470+
"/usr/bin/gcc",
471+
"-c",
472+
"-Wall",
473+
"-Wpedantic",
474+
"-Wstrict-prototypes",
475+
"-g",
476+
"-fopenmp",
477+
"-fsanitize=address",
478+
"-I/home/chutsu/code/xyz",
479+
"-I/home/chutsu/code/xyz/third_party/include",
480+
"-fPIC",
481+
"-I/home/chutsu/code/xyz/third_party/src/stb",
482+
"-I/usr/include/freetype2",
483+
"-I/usr/include/libpng16",
484+
"-I/usr/include/eigen3",
485+
"-Wl,-rpath,/home/chutsu/code/xyz/third_party/lib",
486+
"-o",
487+
"build/test_calib",
488+
"src/test_calib.c"
489+
],
490+
"directory": "/home/chutsu/code/xyz",
491+
"file": "/home/chutsu/code/xyz/src/test_calib.c",
492+
"output": "/home/chutsu/code/xyz/build/test_calib"
493+
},
418494
{
419495
"arguments": [
420496
"/usr/bin/gcc",
@@ -675,6 +751,32 @@
675751
"file": "/home/chutsu/code/xyz/src/test_mav.c",
676752
"output": "/home/chutsu/code/xyz/build/test_mav"
677753
},
754+
{
755+
"arguments": [
756+
"/usr/bin/gcc",
757+
"-c",
758+
"-Wall",
759+
"-Wpedantic",
760+
"-Wstrict-prototypes",
761+
"-g",
762+
"-fopenmp",
763+
"-fsanitize=address",
764+
"-I/home/chutsu/code/xyz",
765+
"-I/home/chutsu/code/xyz/third_party/include",
766+
"-fPIC",
767+
"-I/home/chutsu/code/xyz/third_party/src/stb",
768+
"-I/usr/include/freetype2",
769+
"-I/usr/include/libpng16",
770+
"-I/usr/include/eigen3",
771+
"-Wl,-rpath,/home/chutsu/code/xyz/third_party/lib",
772+
"-o",
773+
"build/test_octree",
774+
"src/test_octree.c"
775+
],
776+
"directory": "/home/chutsu/code/xyz",
777+
"file": "/home/chutsu/code/xyz/src/test_octree.c",
778+
"output": "/home/chutsu/code/xyz/build/test_octree"
779+
},
678780
{
679781
"arguments": [
680782
"/usr/bin/gcc",

‎config.mk

+2
Original file line numberDiff line numberDiff line change
@@ -110,6 +110,7 @@ LIBXYZ_OBJS := \
110110
$(BLD_DIR)/xyz_mav.o \
111111
$(BLD_DIR)/xyz_euroc.o \
112112
$(BLD_DIR)/xyz_calib.o \
113+
$(BLD_DIR)/xyz_octree.o \
113114
$(BLD_DIR)/xyz_ceres.o \
114115
$(BLD_DIR)/xyz_gui.o
115116

@@ -128,6 +129,7 @@ TESTS := \
128129
$(BLD_DIR)/test_http \
129130
$(BLD_DIR)/test_kitti \
130131
$(BLD_DIR)/test_mav \
132+
$(BLD_DIR)/test_octree \
131133
$(BLD_DIR)/test_se \
132134
$(BLD_DIR)/test_sim \
133135
$(BLD_DIR)/test_xyz

‎src/test_cv.c

-1
Original file line numberDiff line numberDiff line change
@@ -990,7 +990,6 @@ int test_solvepnp(void) {
990990
}
991991

992992
void test_suite(void) {
993-
// CV
994993
MU_ADD_TEST(test_image_setup);
995994
MU_ADD_TEST(test_image_load);
996995
MU_ADD_TEST(test_image_print_properties);

‎src/test_octree.c

+49
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
#include "munit.h"
2+
#include "xyz.h"
3+
#include "xyz_octree.h"
4+
5+
int test_octree_node(void) {
6+
const double center[3] = {0.0, 0.0, 0.0};
7+
const double size = 1.0;
8+
const int depth = 0;
9+
const int max_depth = 10;
10+
11+
octree_node_t *node = octree_node_malloc(center, size, depth, max_depth);
12+
octree_node_free(node);
13+
14+
return 0;
15+
}
16+
17+
int test_octree(void) {
18+
// Setup
19+
const double center[3] = {0.0, 0.0, 0.0};
20+
const double size = 0.1;
21+
const int depth = 0;
22+
const int max_depth = 10;
23+
24+
const int N = 10000;
25+
double *points = malloc(sizeof(double) * 3 * N);
26+
for (int i = 0; i < N; ++i) {
27+
const float x = randf(-1.0, 1.0);
28+
const float y = randf(-1.0, 1.0);
29+
const float z = randf(-1.0, 1.0);
30+
points[i * 3 + 0] = x;
31+
points[i * 3 + 1] = y;
32+
points[i * 3 + 2] = z;
33+
}
34+
35+
// Build octree
36+
octree_t *octree = octree_malloc(center, size, depth, max_depth, points, N);
37+
38+
// Clean up
39+
free(points);
40+
octree_free(octree);
41+
42+
return 0;
43+
}
44+
45+
void test_suite(void) {
46+
MU_ADD_TEST(test_octree_node);
47+
MU_ADD_TEST(test_octree);
48+
}
49+
MU_RUN_TESTS(test_suite)

‎src/xyz_gui.c

-9
Original file line numberDiff line numberDiff line change
@@ -1269,15 +1269,6 @@ void gui_loop(void) {
12691269
stbi_set_flip_vertically_on_load(1);
12701270
uint8_t *image_data = stbi_load(image_path, &width, &height, &channels, 0);
12711271

1272-
// Button
1273-
// gl_bounds_t button_bounds = (gl_bounds_t){200, 200, 100, 100};
1274-
1275-
// Checkbox
1276-
// gl_bounds_t checkbox_bounds = (gl_bounds_t){50, 50, 120, 40};
1277-
1278-
// Checkbox
1279-
// gl_bounds_t menu_bounds = (gl_bounds_t){50, 50, 120, 200};
1280-
12811272
// Render loop
12821273
_window_loop = 1;
12831274
glfwMakeContextCurrent(_window);

‎src/xyz_gui.h

+9
Original file line numberDiff line numberDiff line change
@@ -358,6 +358,15 @@ void draw_grid3d(const gl_float_t size,
358358

359359
// POINTS 3D /////////////////////////////////////////////////////////////////
360360

361+
typedef struct gl_points3d {
362+
gl_uint_t VAO;
363+
gl_uint_t VBO;
364+
365+
gl_float_t *points;
366+
size_t num_points;
367+
gl_float_t point_size;
368+
} gl_points3d;
369+
361370
void setup_points3d_shader(gl_shader_t *points);
362371
void draw_points3d(const gl_float_t *points_data,
363372
const size_t num_points,

‎src/xyz_octree.c

+110
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
#include <math.h>
2+
#include <string.h>
3+
#include <stdlib.h>
4+
5+
#include "xyz_octree.h"
6+
7+
octree_node_t *octree_node_malloc(const double center[3],
8+
const double size,
9+
const int depth,
10+
const int max_depth) {
11+
octree_node_t *node = malloc(sizeof(octree_node_t));
12+
13+
14+
node->center[0] = center[0];
15+
node->center[1] = center[1];
16+
node->center[2] = center[2];
17+
node->size = size;
18+
node->depth = depth;
19+
node->max_depth = max_depth;
20+
21+
for (int i = 0; i < 8; ++i) {
22+
node->children[i] = NULL;
23+
}
24+
memset(node->points, 0, sizeof(node->points));
25+
node->num_points = 0;
26+
27+
return node;
28+
}
29+
30+
void octree_node_free(octree_node_t *node) {
31+
if (node == NULL) {
32+
return;
33+
}
34+
35+
for (int i = 0; i < 8; ++i) {
36+
octree_node_free(node->children[i]);
37+
}
38+
free(node);
39+
}
40+
41+
octree_t *octree_malloc(const double center[3],
42+
const double size,
43+
const int depth,
44+
const int max_depth,
45+
const double *points,
46+
const size_t num_points) {
47+
octree_t *octree = malloc(sizeof(octree_t));
48+
49+
octree->center[0] = center[0];
50+
octree->center[1] = center[1];
51+
octree->center[2] = center[2];
52+
octree->size = size;
53+
octree->root = octree_node_malloc(center, size, depth, max_depth);
54+
55+
for (size_t i = 0; i < num_points; i++) {
56+
octree_add_point(octree->root, &(points[i]));
57+
}
58+
59+
return octree;
60+
}
61+
62+
void octree_free(octree_t *octree) {
63+
if (octree == NULL) {
64+
return;
65+
}
66+
octree_node_free(octree->root);
67+
free(octree);
68+
}
69+
70+
void octree_add_point(octree_node_t *node, const double point[3]) {
71+
// Max depth reached? Add the point
72+
if (node->depth == node->max_depth) {
73+
node->points[node->num_points * 3 + 0] = point[0];
74+
node->points[node->num_points * 3 + 1] = point[1];
75+
node->points[node->num_points * 3 + 2] = point[2];
76+
node->num_points++;
77+
return;
78+
}
79+
80+
// Calculate node index
81+
int index = 0;
82+
for (int i = 0; i < 3; ++i) {
83+
if (point[i] < node->center[i]) {
84+
index |= (1 << i);
85+
}
86+
}
87+
88+
// Create new child node if it doesn't exist already
89+
octree_node_t *child = node->children[index];
90+
if (child == NULL) {
91+
const double offset_x = -1 * ((index >> 0) & 1) * node->size / 4.0;
92+
const double offset_y = -1 * ((index >> 1) & 1) * node->size / 4.0;
93+
const double offset_z = -1 * ((index >> 2) & 1) * node->size / 4.0;
94+
95+
const double center[3] = {
96+
node->center[0] + offset_x,
97+
node->center[1] + offset_y,
98+
node->center[2] + offset_z,
99+
};
100+
const double size = node->size / 2.0;
101+
const int depth = node->depth + 1;
102+
const int max_depth = node->max_depth;
103+
child = octree_node_malloc(center, size, depth, max_depth);
104+
105+
node->children[index] = child;
106+
}
107+
108+
// Recurse down the octree
109+
octree_add_point(child, point);
110+
}

0 commit comments

Comments
 (0)