Rename proto to xyz

Author: chutsu

Makefile

@@ -2,7 +2,7 @@ MKFILE_PATH=$(abspath $(lastword $(MAKEFILE_LIST)))
 PROJ_PATH=$(patsubst %/,%,$(dir $(MKFILE_PATH)))
 CATKIN_WS=~/catkin_ws
 
-.PHONY: third_party docs build install tests ci run_test_proto clean
+.PHONY: third_party docs build install tests ci run_test_xyz clean
 
 help:
 	@echo "\033[1;34m[make targets]:\033[0m"
@@ -22,17 +22,17 @@ docs: ## Build docs
 	@rm -rf docs/build
 	@sphinx-autobuild docs/source docs/build/html
 
-build: ## Build libproto
-	@cd proto && make -s libproto
+build: ## Build libxyz
+	@cd xyz && make -s libxyz
 
-install: build ## Install libproto
-	@cd proto && make -s install
+install: build ## Install libxyz
+	@cd xyz && make -s install
 
 tests: ## Run unittests
-	@cd proto && make -s tests
+	@cd xyz && make -s tests
 
 ci: ## Run CI tests
-	@cd proto && make -s ci
+	@cd xyz && make -s ci
 
 clean: ## Clean
-	@cd proto && make -s clean
+	@cd xyz && make -s clean

proto/Makefile xyz/Makefile

@@ -3,7 +3,7 @@ CUR_DIR := $(shell pwd)
 BLD_DIR := build
 INC_DIR := $(realpath $(PWD))
 DEPS_DIR := $(realpath ../third_party)
-PREFIX := /opt/proto
+PREFIX := /opt/xyz
 PYTHON3_PATH := $(shell python3 -c "import site; print(site.getsitepackages()[0])")
 
 
@@ -34,7 +34,7 @@ ASSIMP_LDFLAGS := -lassimp
 
 APRILTAG_LDFLAGS := -L$(DEPS_DIR)/lib -lapriltag
 YAML_LDFLAGS := -lyaml
-PROTO_LDFLAGS := -L$(BLD_DIR) -lproto
+XYZ_LDFLAGS := -L$(BLD_DIR) -lxyz
 
 
 # CFLAGS
@@ -67,7 +67,7 @@ CFLAGS += \
 RPATH := -Wl,-rpath,$(DEPS_DIR)/lib
 LDFLAGS= \
 	$(RPATH) \
-  $(PROTO_LDFLAGS) \
+  $(XYZ_LDFLAGS) \
 	$(CERES_LDFLAGS) \
 	$(APRILTAG_LDFLAGS) \
 	$(OPENGL_LDFLAGS) \
@@ -88,17 +88,17 @@ ARFLAGS = rvs
 
 
 # TARGETS
-LIBPROTO := $(BLD_DIR)/libproto.a
-LIBPROTO_OBJS := \
-	$(BLD_DIR)/proto.o \
+LIBXYZ := $(BLD_DIR)/libxyz.a
+LIBXYZ_OBJS := \
+	$(BLD_DIR)/xyz.o \
 	$(BLD_DIR)/aprilgrid.o \
 	$(BLD_DIR)/euroc.o \
 	$(BLD_DIR)/gui.o \
 	$(BLD_DIR)/sbgc.o \
 	$(BLD_DIR)/ceres_bridge.o
 
 TESTS := \
-	test_proto \
+	test_xyz \
 	test_aprilgrid \
 	test_sbgc \
 	test_ubx
@@ -120,11 +120,11 @@ $(BLD_DIR)/ceres_bridge.o: ceres_bridge.cpp Makefile
 	@echo "CXX [ceres_bridge.c]"
 	@g++ -Wall -O3 -c ceres_bridge.cpp -o $(BLD_DIR)/ceres_bridge.o -I/usr/include/eigen3
 
-$(BLD_DIR)/libproto.a: $(LIBPROTO_OBJS)
-	@echo "AR [libproto.a]"
+$(BLD_DIR)/libxyz.a: $(LIBXYZ_OBJS)
+	@echo "AR [libxyz.a]"
 	@$(AR) $(ARFLAGS) \
-		$(BLD_DIR)/libproto.a \
-		$(LIBPROTO_OBJS) \
+		$(BLD_DIR)/libxyz.a \
+		$(LIBXYZ_OBJS) \
 		> /dev/null 2>&1
 
 setup:
@@ -135,35 +135,35 @@ setup:
 clean:  ## Clean build directory
 	@rm -rf $(BLD_DIR)
 
-libproto: setup $(BLD_DIR)/libproto.a  ## Compile libproto
+libxyz: setup $(BLD_DIR)/libxyz.a  ## Compile libxyz
 
-install: ## Install libproto
+install: ## Install libxyz
 	mkdir -p $(PREFIX)
 	mkdir -p $(PREFIX)/lib
 	mkdir -p $(PREFIX)/include
-	ln -sf $(CUR_DIR)/build/libproto.a $(PREFIX)/lib/libproto.a
+	ln -sf $(CUR_DIR)/build/libxyz.a $(PREFIX)/lib/libxyz.a
 	ln -sf $(CUR_DIR)/aprilgrid.h $(PREFIX)/include/aprilgrid.h
 	ln -sf $(CUR_DIR)/ceres_bridge.h $(PREFIX)/include/ceres_bridge.h
 	ln -sf $(CUR_DIR)/euroc.h $(PREFIX)/include/euroc.h
 	ln -sf $(CUR_DIR)/gui.h $(PREFIX)/include/gui.h
 	ln -sf $(CUR_DIR)/http.h $(PREFIX)/include/http.h
-	ln -sf $(CUR_DIR)/proto.h $(PREFIX)/include/proto.h
+	ln -sf $(CUR_DIR)/xyz.h $(PREFIX)/include/xyz.h
 	ln -sf $(CUR_DIR)/sbgc.h $(PREFIX)/include/sbgc.h
 	ln -sf $(CUR_DIR)/stb_image.h $(PREFIX)/include/stb_image.h
-	ln -sf $(CUR_DIR)/proto.py $(PYTHON3_PATH)/proto.py
+	ln -sf $(CUR_DIR)/xyz.py $(PYTHON3_PATH)/xyz.py
 
-uninstall: ## Uninstall libproto
-	rm $(PREFIX)/lib/libproto.a
+uninstall: ## Uninstall libxyz
+	rm $(PREFIX)/lib/libxyz.a
 	rm $(PREFIX)/include/aprilgrid.h
 	rm $(PREFIX)/include/euroc.h
 	rm $(PREFIX)/include/gui.h
 	rm $(PREFIX)/include/http.h
-	rm $(PREFIX)/include/proto.h
+	rm $(PREFIX)/include/xyz.h
 	rm $(PREFIX)/include/sbgc.h
 	rm $(PREFIX)/include/stb_image.h
-	rm $(PYTHON3_PATH)/proto.py
+	rm $(PYTHON3_PATH)/xyz.py
 
-avs: $(BLD_DIR)/libproto.a
+avs: $(BLD_DIR)/libxyz.a
 	@g++ \
 		-std=c++17 \
 		-fsanitize=address -static-libasan \
@@ -175,13 +175,13 @@ avs: $(BLD_DIR)/libproto.a
 		avs.cpp \
 		-o $(BLD_DIR)/avs \
 		$(LDFLAGS) \
-		-lproto \
+		-lxyz \
 		$(shell pkg-config opencv4 --libs)
 
-test_proto: libproto  ## Compile test_proto
+test_xyz: libxyz  ## Compile test_xyz
 	@echo "CC [$@]"
-	@$(CC) $(CFLAGS) test_proto.c -o $(BLD_DIR)/test_proto $(LDFLAGS)
-	./build/test_proto --target $(TEST_TARGET)
+	@$(CC) $(CFLAGS) test_xyz.c -o $(BLD_DIR)/test_xyz $(LDFLAGS)
+	./build/test_xyz --target $(TEST_TARGET)
 
 test_aprilgrid:  ## Compile test_aprilgrid
 	@echo "CC [$@]"
@@ -212,8 +212,8 @@ test_http:  ## Compile test_http
 	@$(CC) $(CFLAGS) test_http.c -o $(BLD_DIR)/test_http $(LDFLAGS)
 	@./build/test_http
 
-tests: test_proto test_aprilgrid test_sbgc test_ubx
+tests: test_xyz test_aprilgrid test_sbgc test_ubx
 
 ci:
-	@$(CC) $(CFLAGS) -DMU_REDIRECT_STREAMS=1 test_proto.c -o $(BLD_DIR)/test_proto $(LDFLAGS)
-	@./build/test_proto
+	@$(CC) $(CFLAGS) -DMU_REDIRECT_STREAMS=1 test_xyz.c -o $(BLD_DIR)/test_xyz $(LDFLAGS)
+	@./build/test_xyz

proto/aprilgrid.c xyz/aprilgrid.c

@@ -264,7 +264,7 @@ void gl_camera_zoom(gl_camera_t *camera,
  * GL-PRIMITIVES
  *****************************************************************************/
 
-void gl_triangle_setup(gl_entity_t *entity, GLfloat pos[3]);
+void gl_triangle_setup(gl_entity_t *entity);
 void gl_triangle_cleanup(const gl_entity_t *entity);
 void gl_triangle_draw(const gl_entity_t *entity, const gl_camera_t *camera);
 
@@ -398,9 +398,6 @@ void gui_loop(gui_t *gui);
 #include "stb_image_write.h"
 #endif
 
-// #define GLAD_GL_IMPLEMENTATION
-// #include "glad.h"
-
 // /**
 //  * Tic, start timer.
 //  * @returns A timespec encapsulating the time instance when tic() is called
@@ -1132,6 +1129,64 @@ void gl_camera_zoom(gl_camera_t *camera,
  * GL-PRIMITIVES
  *****************************************************************************/
 
+void gl_triangle_setup(gl_entity_t *entity) {
+  // Entity transform
+  gl_eye(entity->T, 4, 4);
+
+  // Shader program
+  char *vs = load_file("./shaders/triangle.vert");
+  char *fs = load_file("./shaders/triangle.frag");
+  entity->program_id = gl_prog_setup(vs, fs, NULL);
+  free(vs);
+  free(fs);
+  if (entity->program_id == GL_FALSE) {
+    FATAL("Failed to create shaders to draw cube!");
+  }
+
+  // Vertices
+  const float vertices[] = {-0.5f, -0.5f, 0.0f, 0.5f, -0.5f, 0.0f, 0.0f, 0.5f, 0.0f};
+  const int num_vertices = 3;
+  const size_t vertex_buffer_size = sizeof(float) * 3 * num_vertices;
+
+  // VAO
+  glGenVertexArrays(1, &entity->vao);
+  glBindVertexArray(entity->vao);
+
+  // VBO
+  glGenBuffers(1, &entity->vbo);
+  glBindBuffer(GL_ARRAY_BUFFER, entity->vbo);
+  glBufferData(GL_ARRAY_BUFFER, vertex_buffer_size, vertices, GL_STATIC_DRAW);
+  // -- Position attribute
+  size_t vertex_size = 3 * sizeof(float);
+  void *pos_offset = (void *) 0;
+  glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, vertex_size, pos_offset);
+  glEnableVertexAttribArray(0);
+  // -- Color attribute
+  // void *color_offset = (void *) (3 * sizeof(float));
+  // glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, vertex_size, color_offset);
+  // glEnableVertexAttribArray(1);
+
+  // Clean up
+  glBindBuffer(GL_ARRAY_BUFFER, 0); // Unbind VBO
+  glBindVertexArray(0);             // Unbind VAO
+}
+
+void gl_triangle_cleanup(const gl_entity_t *entity) {
+  glDeleteVertexArrays(1, &entity->vao);
+  glDeleteBuffers(1, &entity->vbo);
+}
+
+void gl_triangle_draw(const gl_entity_t *entity, const gl_camera_t *camera) {
+  glUseProgram(entity->program_id);
+  // gl_prog_set_mat4f(entity->program_id, "projection", camera->P);
+  // gl_prog_set_mat4f(entity->program_id, "view", camera->V);
+  // gl_prog_set_mat4f(entity->program_id, "model", entity->T);
+
+  glBindVertexArray(entity->vao);
+  glDrawArrays(GL_TRIANGLES, 0, 3);
+  glBindVertexArray(0);
+}
+
 void gl_cube_setup(gl_entity_t *entity, GLfloat pos[3]) {
   // Entity transform
   gl_eye(entity->T, 4, 4);
@@ -2206,6 +2261,9 @@ void gui_loop(gui_t *gui) {
   gl_entity_t cf;
   gl_camera_frame_setup(&cf);
 
+  gl_entity_t triangle;
+  gl_triangle_setup(&triangle);
+
   // gl_entity_t frame;
   // gl_axis_frame_setup(&frame);
 
@@ -2229,6 +2287,7 @@ void gui_loop(gui_t *gui) {
     // gl_camera_frame_draw(&cf, &gui->camera);
     // gl_axis_frame_draw(&frame, &gui->camera);
     gl_grid_draw(&grid, &gui->camera);
+    gl_triangle_draw(&triangle, &gui->camera);
     // gl_model_draw(model, &gui->camera);
 
     glBegin(GL_QUADS);

proto/aprilgrid.h xyz/aprilgrid.h

@@ -31,7 +31,7 @@ static char *test_target_name = NULL;
 #endif
 
 #ifndef MU_ENABLE_PRINT
-#define MU_ENABLE_PRINT 0
+#define MU_ENABLE_PRINT 1
 #endif
 
 /* TERMINAL COLORS */

proto/avs.cpp xyz/avs.cpp

@@ -1,4 +1,4 @@
-#include "proto.h"
+#include "xyz.h"
 #include "munit.h"
 #include "stb_ds.h"
 
@@ -4506,7 +4506,7 @@ static void free_imu_test_data(imu_test_data_t *test_data) {
 int test_imu_propagate() {
   // Setup test data
   imu_test_data_t test_data;
-  setup_imu_test_data(&test_data, 5.0, 1.0);
+  setup_imu_test_data(&test_data, 1.0, 0.1);
 
   // Setup IMU buffer
   const int n = 100;
@@ -4717,7 +4717,7 @@ int test_imu_factor() {
   setup_imu_test_data(&test_data, circle_r, circle_v);
 
   // Setup IMU buffer
-  int buf_size = 20;
+  int buf_size = 100;
   imu_buffer_t imu_buf;
   imu_buffer_setup(&imu_buf);
   for (int k = 0; k < buf_size; k++) {
@@ -4760,6 +4760,10 @@ int test_imu_factor() {
   imu_params.sigma_gw = 2.0e-6;
   imu_params.g = 9.81;
 
+  pose_j.data[0] += 0.01;
+  pose_j.data[1] += 0.02;
+  pose_j.data[2] += 0.03;
+
   imu_factor_t factor;
   imu_factor_setup(&factor,
                    &imu_params,
@@ -4774,13 +4778,25 @@ int test_imu_factor() {
 
   // print_vector("pose_i", pose_i.data, 7);
   // print_vector("pose_j", pose_j.data, 7);
+  // print_vector("vel_i", vel_i.data, 3);
+  // print_vector("vel_j", vel_j.data, 3);
   // print_vector("dr", factor.dr, 3);
   // print_vector("dv", factor.dv, 3);
   // print_vector("dq", factor.dq, 4);
   // printf("dt: %f\n", factor.Dt);
   // print_vector("r", factor.r, 15);
-  mat_save("/tmp/F_.csv", factor.F, 15, 15);
-  // mat_save("/tmp/P_.csv", factor.P, 15, 15);
+  mat_save("/tmp/F.csv", factor.F, 15, 15);
+  mat_save("/tmp/P.csv", factor.P, 15, 15);
+  mat_save("/tmp/r.csv", factor.r, 15, 1);
+  mat_save("/tmp/pose_i.csv", factor.params[0], 7, 1);
+  mat_save("/tmp/vel_i.csv", factor.params[1], 3, 1);
+  mat_save("/tmp/pose_j.csv", factor.params[3], 7, 1);
+  mat_save("/tmp/vel_j.csv", factor.params[4], 3, 1);
+
+  for (int i =0; i < 15; i++) {
+    printf("%.4e\n", factor.r[i]);
+  }
+  printf("\n");
 
   /*
   const char *cmd = "\
@@ -4810,14 +4826,14 @@ ginput();\
   system(syscmd);
   */
 
-  MU_ASSERT(factor.pose_i == &pose_i);
-  MU_ASSERT(factor.vel_i == &vel_i);
-  MU_ASSERT(factor.biases_i == &biases_i);
-  MU_ASSERT(factor.pose_i == &pose_i);
-  MU_ASSERT(factor.vel_j == &vel_j);
-  MU_ASSERT(factor.biases_j == &biases_j);
-
-  // Check Jacobians
+  // MU_ASSERT(factor.pose_i == &pose_i);
+  // MU_ASSERT(factor.vel_i == &vel_i);
+  // MU_ASSERT(factor.biases_i == &biases_i);
+  // MU_ASSERT(factor.pose_i == &pose_i);
+  // MU_ASSERT(factor.vel_j == &vel_j);
+  // MU_ASSERT(factor.biases_j == &biases_j);
+  //
+  // // Check Jacobians
   // const double tol = 1e-4;
   // const double step_size = 1e-8;
   // eye(factor.sqrt_info, 15, 15);
@@ -5587,7 +5603,7 @@ int test_visual_odometry_batch() {
 int test_inertial_odometry_batch() {
   // Setup test data
   imu_test_data_t test_data;
-  setup_imu_test_data(&test_data, 1.0, 1.0);
+  setup_imu_test_data(&test_data, 1.0, 0.1);
 
   // Inertial Odometry
   const int num_partitions = test_data.num_measurements / 20.0;
@@ -8372,7 +8388,7 @@ void test_suite() {
   MU_ADD_TEST(test_calib_gimbal_factor);
   MU_ADD_TEST(test_marg);
   // MU_ADD_TEST(test_visual_odometry_batch);
-  // MU_ADD_TEST(test_inertial_odometry_batch);
+  MU_ADD_TEST(test_inertial_odometry_batch);
   // MU_ADD_TEST(test_visual_inertial_odometry_batch);
   // MU_ADD_TEST(test_tsf);
 #ifdef USE_CERES