├── .gitignore ├── imgs ├── example.png ├── ba_after.png └── ba_before.png ├── ba ├── data_gnd │ ├── target_pose.csv │ ├── camera.csv │ ├── points.csv │ ├── point_ids.csv │ ├── camera_poses.csv │ └── keypoints.csv ├── data_noisy │ ├── target_pose.csv │ ├── camera.csv │ ├── points.csv │ ├── point_ids.csv │ ├── camera_poses.csv │ └── keypoints.csv ├── c │ ├── ba_solver.c │ ├── Makefile │ ├── munit.h │ ├── test_ba.c │ ├── util.h │ └── ba.h └── cpp │ ├── ba_solver.cpp │ ├── Makefile │ ├── ba.hpp │ ├── munit.hpp │ ├── test_ba.cpp │ ├── ba.cpp │ └── util.hpp ├── scripts ├── run.sh ├── sim_plot.m └── sim_data.m ├── .github └── workflows │ └── ccpp.yml ├── Makefile └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | bin 2 | build 3 | -------------------------------------------------------------------------------- /imgs/example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/ba/HEAD/imgs/example.png -------------------------------------------------------------------------------- /imgs/ba_after.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/ba/HEAD/imgs/ba_after.png -------------------------------------------------------------------------------- /imgs/ba_before.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chutsu/ba/HEAD/imgs/ba_before.png -------------------------------------------------------------------------------- /ba/data_gnd/target_pose.csv: -------------------------------------------------------------------------------- 1 | #qw,qx,qy,qz,rx,ry,rz 2 | 0.500000,0.500000,-0.500000,-0.500000,1.000000,0.000000,0.000000 3 | -------------------------------------------------------------------------------- /ba/data_noisy/target_pose.csv: -------------------------------------------------------------------------------- 1 | #qw,qx,qy,qz,rx,ry,rz 2 | 0.500000,0.500000,-0.500000,-0.500000,1.000000,0.000000,0.000000 3 | -------------------------------------------------------------------------------- /ba/data_gnd/camera.csv: -------------------------------------------------------------------------------- 1 | #camera_K 2 | 320.000000,0.000000,320.000000 3 | 0.000000,240.000000,240.000000 4 | 0.000000,0.000000,1.000000 5 | -------------------------------------------------------------------------------- /ba/data_noisy/camera.csv: -------------------------------------------------------------------------------- 1 | #camera_K 2 | 320.000000,0.000000,320.000000 3 | 0.000000,240.000000,240.000000 4 | 0.000000,0.000000,1.000000 5 | -------------------------------------------------------------------------------- /scripts/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | # ctags -R . 4 | 5 | # make clean 6 | make ba 7 | # make run 8 | # make runc 9 | make unittest 10 | 11 | # cd ba/c/bin; ./test_ba; cd - 12 | # cd ba; octave-cli sim_data.m; cd - 13 | # cd bin; octave-cli sim_plot.m; cd - 14 | -------------------------------------------------------------------------------- /.github/workflows/ccpp.yml: -------------------------------------------------------------------------------- 1 | name: ci 2 | on: [push] 3 | jobs: 4 | ci: 5 | runs-on: ubuntu-latest 6 | steps: 7 | - uses: actions/checkout@master 8 | 9 | - name: install deps 10 | run: make deps 11 | 12 | - name: build 13 | run: make ba 14 | 15 | - name: run 16 | run: make run 17 | 18 | - name: unittest 19 | run: make unittest 20 | -------------------------------------------------------------------------------- /ba/c/ba_solver.c: -------------------------------------------------------------------------------- 1 | #include "ba.h" 2 | 3 | void print_usage() { 4 | printf("Usage: ba_solver \n"); 5 | printf("Example: ba_solver ./data\n"); 6 | } 7 | 8 | int main(int argc, char **argv) { 9 | /* const char *save_path = "./data_est"; */ 10 | if (argc != 2) { 11 | print_usage(); 12 | return -1; 13 | } 14 | 15 | ba_data_t *data = ba_load_data(argv[1]); 16 | printf("Solving BA problem:\n"); 17 | struct timespec t_start = tic(); 18 | ba_solve(data); 19 | printf("total time taken: %.4fs\n", toc(&t_start)); 20 | printf("nb_frames: %d\n", data->nb_frames); 21 | printf("nb_points: %d\n", data->nb_points); 22 | ba_data_free(data); 23 | 24 | return 0; 25 | } 26 | -------------------------------------------------------------------------------- /ba/cpp/ba_solver.cpp: -------------------------------------------------------------------------------- 1 | #include "ba.hpp" 2 | 3 | void print_usage() { 4 | printf("Usage: ba_solver \n"); 5 | printf("Example: ba_solver ./data\n"); 6 | } 7 | 8 | int main(int argc, char **argv) { 9 | std::string save_path = "./data_est"; 10 | if (argc != 2) { 11 | print_usage(); 12 | return -1; 13 | } 14 | 15 | struct timespec t_start = tic(); 16 | ba_data_t data{std::string{argv[1]}}; 17 | printf("Solving BA problem:\n"); 18 | ba_solve(data); 19 | printf("total time taken: %.4fs\n", toc(&t_start)); 20 | printf("nb_frames: %d\n", data.nb_frames); 21 | printf("nb_points: %d\n", data.nb_points); 22 | printf("\n"); 23 | ba_save(data, save_path); 24 | 25 | return 0; 26 | } 27 | -------------------------------------------------------------------------------- /ba/data_gnd/points.csv: -------------------------------------------------------------------------------- 1 | #x,y,z 2 | 1.000000,0.000000,0.000000 3 | 1.000000,-0.200000,0.000000 4 | 1.000000,-0.400000,0.000000 5 | 1.000000,-0.600000,0.000000 6 | 1.000000,-0.800000,0.000000 7 | 1.000000,0.000000,0.200000 8 | 1.000000,-0.200000,0.200000 9 | 1.000000,-0.400000,0.200000 10 | 1.000000,-0.600000,0.200000 11 | 1.000000,-0.800000,0.200000 12 | 1.000000,0.000000,0.400000 13 | 1.000000,-0.200000,0.400000 14 | 1.000000,-0.400000,0.400000 15 | 1.000000,-0.600000,0.400000 16 | 1.000000,-0.800000,0.400000 17 | 1.000000,0.000000,0.600000 18 | 1.000000,-0.200000,0.600000 19 | 1.000000,-0.400000,0.600000 20 | 1.000000,-0.600000,0.600000 21 | 1.000000,-0.800000,0.600000 22 | 1.000000,0.000000,0.800000 23 | 1.000000,-0.200000,0.800000 24 | 1.000000,-0.400000,0.800000 25 | 1.000000,-0.600000,0.800000 26 | 1.000000,-0.800000,0.800000 27 | -------------------------------------------------------------------------------- /ba/data_noisy/points.csv: -------------------------------------------------------------------------------- 1 | #x,y,z 2 | 1.000000,0.000000,0.000000 3 | 1.000000,-0.200000,0.000000 4 | 1.000000,-0.400000,0.000000 5 | 1.000000,-0.600000,0.000000 6 | 1.000000,-0.800000,0.000000 7 | 1.000000,0.000000,0.200000 8 | 1.000000,-0.200000,0.200000 9 | 1.000000,-0.400000,0.200000 10 | 1.000000,-0.600000,0.200000 11 | 1.000000,-0.800000,0.200000 12 | 1.000000,0.000000,0.400000 13 | 1.000000,-0.200000,0.400000 14 | 1.000000,-0.400000,0.400000 15 | 1.000000,-0.600000,0.400000 16 | 1.000000,-0.800000,0.400000 17 | 1.000000,0.000000,0.600000 18 | 1.000000,-0.200000,0.600000 19 | 1.000000,-0.400000,0.600000 20 | 1.000000,-0.600000,0.600000 21 | 1.000000,-0.800000,0.600000 22 | 1.000000,0.000000,0.800000 23 | 1.000000,-0.200000,0.800000 24 | 1.000000,-0.400000,0.800000 25 | 1.000000,-0.600000,0.800000 26 | 1.000000,-0.800000,0.800000 27 | -------------------------------------------------------------------------------- /ba/cpp/Makefile: -------------------------------------------------------------------------------- 1 | MKFPATH:=$(abspath $(lastword $(MAKEFILE_LIST))) 2 | MKFDIR:=$(dir $(MKFPATH)) 3 | 4 | BLD_DIR=$(MKFDIR)/build 5 | BIN_DIR=$(MKFDIR)/bin 6 | 7 | CXX=g++ 8 | CXXFLAGS=-std=c++11 -Wall -O2 -I/usr/include/eigen3 9 | LIBS=-L$(BLD_DIR) -lba 10 | OBJS=$(BLD_DIR)/util.o $(BLD_DIR)/ba.o 11 | 12 | LIBBA=$(BLD_DIR)/libba.a 13 | BA_SOLVER=$(BIN_DIR)/ba_solver 14 | TEST_BA=$(BIN_DIR)/test_ba 15 | 16 | default: dirs $(LIBBA) $(TEST_BA) $(BA_SOLVER) 17 | .PHONY: dirs 18 | 19 | dirs: 20 | @mkdir -p $(BIN_DIR) 21 | @mkdir -p $(BLD_DIR) 22 | 23 | clean: 24 | @rm -rf $(BIN_DIR) 25 | @rm -rf $(BLD_DIR) 26 | 27 | $(BLD_DIR)/%.o: %.cpp %.hpp 28 | @echo "CXX [$<]" 29 | @$(CXX) $(CXXFLAGS) -c $< -o $@ 30 | 31 | $(LIBBA): $(OBJS) 32 | @echo "AR [libba.a]" 33 | @ar rvs $@ $^ 34 | 35 | $(TEST_BA): test_ba.cpp ba.cpp 36 | @echo "TEST [test_ba]" 37 | $(CXX) $^ -o $@ $(LIBS) $(CXXFLAGS) 38 | 39 | $(BA_SOLVER): ba_solver.cpp $(OBJS) 40 | @echo "EXE [ba_solver]" 41 | @$(CXX) $^ -o $@ $(LIBS) $(CXXFLAGS) 42 | @cd $(BIN_DIR); ln -sf $(MKFDIR)/../../scripts/sim_plot.m . 43 | @cd $(BIN_DIR); ln -sf $(MKFDIR)/../data_gnd . 44 | @cd $(BIN_DIR); ln -sf $(MKFDIR)/../data_noisy . 45 | -------------------------------------------------------------------------------- /ba/c/Makefile: -------------------------------------------------------------------------------- 1 | MKFPATH:=$(abspath $(lastword $(MAKEFILE_LIST))) 2 | MKFDIR:=$(dir $(MKFPATH)) 3 | 4 | BLD_DIR=build 5 | BIN_DIR=bin 6 | 7 | CC=gcc 8 | CFLAGS= -O2 -Wall -I$(INC_DIR) -pedantic 9 | LIBS=-L$(BLD_DIR) \ 10 | -lba \ 11 | -lblas \ 12 | -llapack \ 13 | -lpthread \ 14 | -lm 15 | OBJS=$(BLD_DIR)/util.o 16 | 17 | LIBBA=$(BLD_DIR)/libba.a 18 | BA_SOLVER=$(BIN_DIR)/ba_solver 19 | TEST_BA=$(BIN_DIR)/test_ba 20 | 21 | default: dirs $(OBJS) $(LIBBA) $(BA_SOLVER) $(TEST_BA) 22 | .PHONY: dirs 23 | 24 | dirs: 25 | @mkdir -p $(BIN_DIR) 26 | @mkdir -p $(BLD_DIR) 27 | 28 | clean: 29 | @rm -rf $(BIN_DIR) 30 | @rm -rf $(BLD_DIR) 31 | 32 | $(BLD_DIR)/%.o : %.c %.h 33 | @echo "CC [$<]"; \ 34 | $(CC) $(CFLAGS) -c $< -o $@ 35 | 36 | $(LIBBA): $(OBJS) 37 | @echo "AR [libba.a]" 38 | @ar rvs $@ $^ 39 | 40 | $(TEST_BA): test_ba.c ba.h 41 | @echo "TEST [test_ba]" 42 | $(CC) $^ -o $@ $(LIBS) $(CFLAGS) 43 | 44 | $(BA_SOLVER): ba_solver.c $(OBJS) 45 | @echo "EXE [ba_solver]" 46 | @$(CC) $^ -o $@ $(LIBS) $(CFLAGS) 47 | @cd $(BIN_DIR); ln -sf $(MKFDIR)/../../scripts/sim_plot.m . 48 | @cd $(BIN_DIR); ln -sf $(MKFDIR)/../data_gnd . 49 | @cd $(BIN_DIR); ln -sf $(MKFDIR)/../data_noisy . 50 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | define usage 2 | [MAKE TARGETS]: 3 | deps: 4 | Install dependencies 5 | 6 | ba: 7 | Build repo 8 | 9 | unittest: 10 | Build and run unittest 11 | 12 | run: 13 | Run bundle adjustment solver 14 | 15 | plot: 16 | Plot results 17 | 18 | clean: 19 | Clean build and bin directories 20 | endef 21 | export usage 22 | 23 | .PHONY: ba deps unittest run clean 24 | 25 | default: 26 | @echo "$$usage" 27 | 28 | deps: 29 | @echo "Installing dependencies ..." 30 | @sudo apt-get update 31 | @sudo apt-get install -qqq \ 32 | libblas-dev \ 33 | liblapack-dev \ 34 | liblapacke-dev \ 35 | libeigen3-dev \ 36 | octave 37 | 38 | ba: 39 | @make -s -C ba/c 40 | @echo "" 41 | @make -s -C ba/cpp 42 | 43 | unittest: ba 44 | @echo "Running unittests" 45 | @cd ba/c/bin && ./test_ba 46 | @cd ba/cpp/bin && ./test_ba 47 | 48 | run: ba 49 | @cd ba/cpp/bin && ./ba_solver ./data_noisy 50 | 51 | plot: ba run 52 | @cd ba/cpp/bin && octave-cli sim_plot.m 53 | 54 | runc: ba 55 | @cd ba/c/bin && ./ba_solver ./data_noisy 56 | 57 | clean: 58 | @echo "Cleaning repo..." 59 | @make -s -C ba/c clean 60 | @make -s -C ba/cpp clean 61 | @echo "Done!" 62 | @echo "" 63 | -------------------------------------------------------------------------------- /ba/cpp/ba.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "util.hpp" 3 | 4 | struct ba_data_t { 5 | mat3_t cam_K; 6 | 7 | poses_t cam_poses; 8 | pose_t target_pose; 9 | int nb_frames; 10 | 11 | std::vector keypoints; 12 | int **point_ids; 13 | int nb_ids; 14 | 15 | real_t **points; 16 | int nb_points; 17 | 18 | ba_data_t(const std::string &data_path) { 19 | cam_K = load_camera(data_path); 20 | cam_poses = load_camera_poses(data_path); 21 | target_pose = load_target_pose(data_path)[0]; 22 | nb_frames = cam_poses.size(); 23 | keypoints = load_keypoints(data_path); 24 | point_ids = load_point_ids(data_path, &nb_ids); 25 | points = load_points(data_path, &nb_points); 26 | } 27 | 28 | ~ba_data_t() { 29 | // Point IDs 30 | for (int i = 0; i < nb_frames; i++) { 31 | free(point_ids[i]); 32 | } 33 | free(point_ids); 34 | 35 | // Points 36 | for (int i = 0; i < nb_points; i++) { 37 | free(points[i]); 38 | } 39 | free(points); 40 | } 41 | }; 42 | 43 | int ba_residual_size(ba_data_t &data); 44 | vecx_t ba_residuals(ba_data_t &data); 45 | matx_t ba_jacobian(ba_data_t &data); 46 | void ba_update(ba_data_t &data, const vecx_t &e, const matx_t &E); 47 | real_t ba_cost(const vecx_t &e); 48 | void ba_solve(ba_data_t &data); 49 | void ba_save(const ba_data_t &data, std::string &save_dir); 50 | -------------------------------------------------------------------------------- /ba/data_gnd/point_ids.csv: -------------------------------------------------------------------------------- 1 | #size,point_ids 2 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 3 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 4 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 5 | 24,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,21,22,23,24 6 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 7 | 24,0,1,2,3,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 8 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 9 | 23,0,1,2,3,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23 10 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 11 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 12 | 24,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,21,22,23,24 13 | 20,0,1,2,3,5,6,7,8,10,11,12,13,15,16,17,18,20,21,22,23 14 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 15 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 16 | 24,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23 17 | 24,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23 18 | 20,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,16,17,18,19,22 19 | 18,0,1,2,5,6,7,8,10,11,12,13,15,16,17,18,20,21,22 20 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 21 | 22,0,1,2,3,5,6,7,8,9,10,11,12,13,14,15,16,17,18,20,21,22,23 22 | -------------------------------------------------------------------------------- /ba/data_noisy/point_ids.csv: -------------------------------------------------------------------------------- 1 | #size,point_ids 2 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 3 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 4 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 5 | 24,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,21,22,23,24 6 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 7 | 24,0,1,2,3,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 8 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 9 | 23,0,1,2,3,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23 10 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 11 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 12 | 24,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,21,22,23,24 13 | 20,0,1,2,3,5,6,7,8,10,11,12,13,15,16,17,18,20,21,22,23 14 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 15 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 16 | 24,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23 17 | 24,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23 18 | 20,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,16,17,18,19,22 19 | 18,0,1,2,5,6,7,8,10,11,12,13,15,16,17,18,20,21,22 20 | 25,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24 21 | 22,0,1,2,3,5,6,7,8,9,10,11,12,13,14,15,16,17,18,20,21,22,23 22 | -------------------------------------------------------------------------------- /ba/data_gnd/camera_poses.csv: -------------------------------------------------------------------------------- 1 | #qw,qx,qy,qz,rx,ry,rz 2 | 0.587952,-0.264701,0.317377,-0.695354,0.492102,-0.305479,-0.083865 3 | 0.267556,-0.591839,0.627629,-0.429212,0.476897,-0.145820,0.829389 4 | -0.440239,0.651259,-0.541401,0.298223,0.150210,-0.638946,0.722973 5 | 0.211643,-0.496324,0.799202,-0.264850,0.540301,-0.282468,0.970943 6 | 0.583645,-0.243038,0.551915,-0.543766,0.260698,-0.291842,0.045994 7 | 0.611568,-0.468889,0.295922,-0.564410,0.394617,-0.501888,0.156175 8 | 0.665775,-0.242598,0.350610,-0.612342,0.443991,-0.359340,-0.188241 9 | 0.554724,-0.712711,0.295889,-0.311085,0.438487,-0.785390,0.593455 10 | 0.616337,-0.234625,0.352398,-0.663999,0.421875,-0.319408,-0.044631 11 | -0.340855,0.111244,-0.351900,0.864644,0.471665,0.092220,-0.122588 12 | 0.312209,-0.138851,0.751150,-0.564818,0.294847,0.155886,0.395682 13 | -0.471106,0.758349,-0.352554,0.280484,0.580064,-0.608752,0.771097 14 | 0.233689,-0.647879,0.651128,-0.318863,0.428347,-0.259275,0.999103 15 | 0.594645,-0.544489,0.379034,-0.454160,0.282183,-0.589543,0.315672 16 | -0.383166,0.732720,-0.490112,0.275854,0.523586,-0.580148,0.847342 17 | 0.574168,-0.643521,0.473831,-0.178032,0.424807,-0.931541,0.454438 18 | 0.333602,-0.370507,0.801015,-0.331374,0.329482,-0.285650,0.634189 19 | -0.444822,0.659160,-0.404246,0.451915,0.551796,-0.403225,0.599381 20 | 0.563732,-0.383197,0.387413,-0.620707,0.458978,-0.329291,0.044738 21 | -0.499453,0.740990,-0.332193,0.301874,0.521153,-0.712245,0.658399 22 | -------------------------------------------------------------------------------- /ba/data_noisy/camera_poses.csv: -------------------------------------------------------------------------------- 1 | #qw,qx,qy,qz,rx,ry,rz 2 | 0.580352,-0.266816,0.322819,-0.698419,0.409725,-0.234750,-0.160478 3 | 0.270162,-0.596359,0.624046,-0.426539,0.606420,-0.096968,0.785635 4 | -0.439515,0.646242,-0.544145,0.305130,0.018296,-0.638572,0.642128 5 | 0.210948,-0.494092,0.799793,-0.267779,0.642046,-0.203268,0.916526 6 | 0.573518,-0.245350,0.560038,-0.545196,0.018683,-0.342718,0.113301 7 | 0.599264,-0.473678,0.298832,-0.572024,0.520283,-0.625398,0.186603 8 | 0.663167,-0.251911,0.349080,-0.612285,0.448309,-0.254279,-0.043217 9 | 0.553254,-0.713699,0.294922,-0.312355,0.443520,-0.789377,0.715658 10 | 0.617873,-0.232333,0.354620,-0.662193,0.630353,-0.353687,-0.016562 11 | -0.341907,0.111776,-0.358594,0.861404,0.383459,0.041621,-0.066283 12 | 0.313472,-0.143093,0.745267,-0.570821,0.268203,0.173104,0.442695 13 | -0.466699,0.761881,-0.350558,0.280782,0.638107,-0.604493,0.944471 14 | 0.226664,-0.651719,0.649274,-0.319887,0.280779,-0.247064,0.931746 15 | 0.598655,-0.539974,0.383811,-0.450255,0.414750,-0.390507,0.236311 16 | -0.377476,0.738601,-0.488013,0.271704,0.548057,-0.483331,0.882913 17 | 0.573896,-0.645864,0.471276,-0.177205,0.599808,-0.906187,0.430935 18 | 0.333609,-0.365758,0.799789,-0.339505,0.190001,-0.216039,0.491724 19 | -0.441686,0.657457,-0.407418,0.454614,0.576635,-0.460877,0.634458 20 | 0.564942,-0.374841,0.391248,-0.622302,0.483631,-0.214738,-0.134223 21 | -0.485929,0.750520,-0.334114,0.298261,0.574753,-0.709173,0.496708 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ba 2 | 3 | 4 | 5 | 6 | 7 | Minimal Bundle Adjustment Example: 8 | 9 |

10 | BA Demo 11 |

12 | 13 | where red represents the ground truth and blue represents the initial camera 14 | poses and landmark points before and after bundle adjustment. 15 | 16 | Example output on a Lenovo ThinkPad P15 Gen 1 with Intel Core i7: 17 | 18 | Single precision mode: 19 | 20 | ``` 21 | Solving BA problem: 22 | iter: [0] lambda: 1.00e-04 cost: 3.0443e+04 dcost: -1.02e+06 rms reproj error: 11.35 23 | iter: [1] lambda: 1.00e-05 cost: 1.7417e+02 dcost: -3.03e+04 rms reproj error: 0.86 24 | iter: [2] lambda: 1.00e-06 cost: 9.8439e-02 dcost: -1.74e+02 rms reproj error: 0.02 25 | iter: [3] lambda: 1.00e-07 cost: 6.8691e-07 dcost: -9.84e-02 rms reproj error: 0.00 26 | Done! 27 | total time taken: 0.0239s 28 | nb_frames: 20 29 | nb_points: 25 30 | ``` 31 | 32 | Double precision mode: 33 | 34 | ``` 35 | Solving BA problem: 36 | iter: [0] lambda: 1.00e-04 cost: 3.0441e+04 dcost: -1.02e+06 rms reproj error: 11.35 37 | iter: [1] lambda: 1.00e-05 cost: 1.7375e+02 dcost: -3.03e+04 rms reproj error: 0.86 38 | iter: [2] lambda: 1.00e-06 cost: 9.7500e-02 dcost: -1.74e+02 rms reproj error: 0.02 39 | iter: [3] lambda: 1.00e-07 cost: 1.1059e-07 dcost: -9.75e-02 rms reproj error: 0.00 40 | Done! 41 | total time taken: 0.0322s 42 | nb_frames: 20 43 | nb_points: 25 44 | ``` 45 | 46 | An approximate ~34% speed up of single vs double precision. 47 | 48 | ## Dependencies 49 | 50 | LAPACKE (http://www.netlib.org/lapack/lapacke.html) 51 | Eigen3 (http://eigen.tuxfamily.org/) 52 | octave (https://www.gnu.org/software/octave/) 53 | 54 | If youre in Ubuntu, you can obtain the dependencies via: 55 | 56 | sudo apt-get install -qqq \ 57 | libblas-dev \ 58 | liblapack-dev \ 59 | liblapacke-dev \ 60 | libeigen3-dev \ 61 | octave 62 | 63 | 64 | 65 | ## Build and Run 66 | 67 | git clone https://github.com/chutsu/ba 68 | cd ba 69 | make deps 70 | make ba 71 | make run 72 | make plot 73 | 74 | 75 | ## Licence 76 | 77 | ``` 78 | Copyright (c) <2020> 79 | 80 | Permission is hereby granted, free of charge, to any person obtaining a copy 81 | of this software and associated documentation files (the "Software"), to deal 82 | in the Software without restriction, including without limitation the rights 83 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 84 | copies of the Software, and to permit persons to whom the Software is 85 | furnished to do so, subject to the following conditions: 86 | 87 | The above copyright notice and this permission notice shall be included in all 88 | copies or substantial portions of the Software. 89 | 90 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 91 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 92 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 93 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 94 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 95 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 96 | SOFTWARE. 97 | ``` 98 | -------------------------------------------------------------------------------- /ba/c/munit.h: -------------------------------------------------------------------------------- 1 | #ifndef MUNIT_H 2 | #define MUNIT_H 3 | 4 | #include 5 | #include 6 | 7 | /* GLOBAL VARIABLES */ 8 | static int tests = 0; 9 | static int passed = 0; 10 | static int failed = 0; 11 | static char *test_target = NULL; 12 | 13 | /* MACROS */ 14 | #define KNRM "\x1B[1;0m" 15 | #define KRED "\x1B[1;31m" 16 | #define KGRN "\x1B[1;32m" 17 | #define KYEL "\x1B[1;33m" 18 | #define KBLU "\x1B[1;34m" 19 | #define KMAG "\x1B[1;35m" 20 | #define KCYN "\x1B[1;36m" 21 | #define KWHT "\x1B[1;37m" 22 | 23 | /* MUNIT */ 24 | #define MU_CHECK(TEST) \ 25 | do { \ 26 | if ((TEST) == 0) { \ 27 | printf("%sERROR!%s [%s:%d] %s %sFAILED!%s\n", \ 28 | KRED, \ 29 | KNRM, \ 30 | __func__, \ 31 | __LINE__, \ 32 | #TEST, \ 33 | KRED, \ 34 | KNRM); \ 35 | return -1; \ 36 | } \ 37 | } while (0) 38 | 39 | #define MU_CHECK_NEAR(expected, actual, tolerance) \ 40 | do { \ 41 | if (!(fabs(expected - actual) < tolerance)) { \ 42 | printf("%sERROR!%s [%s:%d] %sFAILED!%s\n", \ 43 | KRED, \ 44 | KNRM, \ 45 | __func__, \ 46 | __LINE__, \ 47 | KRED, \ 48 | KNRM); \ 49 | return -1; \ 50 | } \ 51 | } while (0) 52 | 53 | #define MU_CHECK_FLOAT(expected, actual) \ 54 | do { \ 55 | if (!(fabs(expected - actual) < 1e-6)) { \ 56 | printf("%sERROR!%s [%s:%d] %sFAILED!%s\n", \ 57 | KRED, \ 58 | KNRM, \ 59 | __func__, \ 60 | __LINE__, \ 61 | KRED, \ 62 | KNRM); \ 63 | printf("%sReason: %f != %f%s\n", \ 64 | KRED, \ 65 | (float) expected, \ 66 | (float) actual, \ 67 | KNRM); \ 68 | return -1; \ 69 | } \ 70 | } while (0) 71 | 72 | #define MU_ADD_TEST(TEST) \ 73 | do { \ 74 | if (test_target != NULL && strcmp(test_target, #TEST) != 0) { \ 75 | continue; \ 76 | } \ 77 | \ 78 | printf("%s-> %s %s\n", KBLU, #TEST, KNRM); \ 79 | fflush(stdout); \ 80 | if (TEST() == -1) { \ 81 | printf("%sTEST FAILED!%s\n\n", KRED, KNRM); \ 82 | failed++; \ 83 | } else { \ 84 | printf("%sTEST PASSED!%s\n\n", KGRN, KNRM); \ 85 | passed++; \ 86 | } \ 87 | tests++; \ 88 | } while (0) 89 | 90 | #define MU_REPORT() \ 91 | do { \ 92 | printf(KBLU); \ 93 | printf("%d tests, ", tests); \ 94 | printf("%d passed, ", passed); \ 95 | printf("%d failed\n", tests - passed); \ 96 | printf("\n"); \ 97 | printf(KNRM); \ 98 | if (failed != 0) \ 99 | return -1; \ 100 | else \ 101 | return 0; \ 102 | } while (0) 103 | 104 | #define MU_RUN_TESTS(TEST_SUITE) \ 105 | int main(int argc, char *argv[]) { \ 106 | if (argc == 3 && strcmp(argv[1], "--target") == 0) { \ 107 | test_target = argv[2]; \ 108 | printf("%sTEST TARGET [%s]%s\n", KYEL, test_target, KNRM); \ 109 | } \ 110 | \ 111 | TEST_SUITE(); \ 112 | MU_REPORT(); \ 113 | return 0; \ 114 | } 115 | 116 | #define PYTHON_SCRIPT(A) \ 117 | if (system("python3 " A) != 0) { \ 118 | printf("Python script [%s] failed !", A); \ 119 | return -1; \ 120 | } 121 | 122 | #define OCTAVE_SCRIPT(A) \ 123 | if (system("octave " A) != 0) { \ 124 | printf("Octave script [%s] failed !", A); \ 125 | exit(-1); \ 126 | } 127 | 128 | #endif // MUNIT_H 129 | -------------------------------------------------------------------------------- /ba/cpp/munit.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MUNIT_HPP 2 | #define MUNIT_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | /* GLOBAL VARIABLES */ 9 | static int tests = 0; 10 | static int passed = 0; 11 | static int failed = 0; 12 | static std::string test_target = ""; 13 | 14 | /* MACROS */ 15 | #define KNRM "\x1B[1;0m" 16 | #define KRED "\x1B[1;31m" 17 | #define KGRN "\x1B[1;32m" 18 | #define KYEL "\x1B[1;33m" 19 | #define KBLU "\x1B[1;34m" 20 | #define KMAG "\x1B[1;35m" 21 | #define KCYN "\x1B[1;36m" 22 | #define KWHT "\x1B[1;37m" 23 | 24 | /* MUNIT */ 25 | #define MU_CHECK(TEST) \ 26 | do { \ 27 | if ((TEST) == false) { \ 28 | printf("%sERROR!%s [%s:%d] %s %sFAILED!%s\n", \ 29 | KRED, \ 30 | KNRM, \ 31 | __func__, \ 32 | __LINE__, \ 33 | #TEST, \ 34 | KRED, \ 35 | KNRM); \ 36 | return -1; \ 37 | } \ 38 | } while (0) 39 | 40 | #define MU_CHECK_NEAR(expected, actual, tolerance) \ 41 | do { \ 42 | if (!(fabs(expected - actual) < tolerance)) { \ 43 | printf("%sERROR!%s [%s:%d] %sFAILED!%s\n", \ 44 | KRED, \ 45 | KNRM, \ 46 | __func__, \ 47 | __LINE__, \ 48 | KRED, \ 49 | KNRM); \ 50 | return -1; \ 51 | } \ 52 | } while (0) 53 | 54 | #define MU_CHECK_FLOAT(expected, actual) \ 55 | do { \ 56 | if (!(fabs(expected - actual) < 1e-6)) { \ 57 | printf("%sERROR!%s [%s:%d] %sFAILED!%s\n", \ 58 | KRED, \ 59 | KNRM, \ 60 | __func__, \ 61 | __LINE__, \ 62 | KRED, \ 63 | KNRM); \ 64 | printf("%sReason: %f != %f%s\n", \ 65 | KRED, \ 66 | (float) expected, \ 67 | (float) actual, \ 68 | KNRM); \ 69 | return -1; \ 70 | } \ 71 | } while (0) 72 | 73 | #define MU_ADD_TEST(TEST) \ 74 | do { \ 75 | if (test_target != "" && test_target != #TEST) { \ 76 | continue; \ 77 | } \ 78 | \ 79 | printf("%s-> %s %s\n", KBLU, #TEST, KNRM); \ 80 | fflush(stdout); \ 81 | if (TEST() == -1) { \ 82 | printf("%sTEST FAILED!%s\n\n", KRED, KNRM); \ 83 | failed++; \ 84 | } else { \ 85 | printf("%sTEST PASSED!%s\n\n", KGRN, KNRM); \ 86 | passed++; \ 87 | } \ 88 | tests++; \ 89 | } while (0) 90 | 91 | #define MU_REPORT() \ 92 | do { \ 93 | printf(KBLU); \ 94 | printf("%d tests, ", tests); \ 95 | printf("%d passed, ", passed); \ 96 | printf("%d failed\n", tests - passed); \ 97 | printf("\n"); \ 98 | printf(KNRM); \ 99 | if (failed != 0) { \ 100 | return -1; \ 101 | } else { \ 102 | return 0; \ 103 | } \ 104 | } while (0) 105 | 106 | #define MU_RUN_TESTS(TEST_SUITE) \ 107 | int main(int argc, char *argv[]) { \ 108 | if (argc == 3 && strcmp(argv[1], "--target") == 0) { \ 109 | test_target = std::string{argv[2]}; \ 110 | printf("%sTEST TARGET [%s]%s\n", KYEL, test_target.c_str(), KNRM); \ 111 | } \ 112 | \ 113 | TEST_SUITE(); \ 114 | MU_REPORT(); \ 115 | return 0; \ 116 | } 117 | 118 | #define PYTHON_SCRIPT(A) \ 119 | if (system("python3 " A) != 0) { \ 120 | printf("Python script [%s] failed !", A); \ 121 | return -1; \ 122 | } 123 | 124 | #define OCTAVE_SCRIPT(A) \ 125 | if (system("octave " A) != 0) { \ 126 | printf("Octave script [%s] failed !", A); \ 127 | exit(-1); \ 128 | } 129 | 130 | #endif // MUNIT_HPP 131 | -------------------------------------------------------------------------------- /scripts/sim_plot.m: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env octave-cli 2 | graphics_toolkit("fltk"); 3 | save_figs = false; 4 | show_figs = true; 5 | data_gnd_dir = "./data_gnd"; % Ground truth data 6 | data_noisy_dir = "./data_noisy"; % Noisy data 7 | data_est_dir = "./data_est"; % Estimated data 8 | 9 | function R = quat2rot(q) 10 | qw = q(1); 11 | qx = q(2); 12 | qy = q(3); 13 | qz = q(4); 14 | 15 | qx2 = qx**2; 16 | qy2 = qy**2; 17 | qz2 = qz**2; 18 | qw2 = qw**2; 19 | 20 | % Homogeneous form 21 | R11 = qw2 + qx2 - qy2 - qz2; 22 | R12 = 2 * (qx * qy - qw * qz); 23 | R13 = 2 * (qx * qz + qw * qy); 24 | 25 | R21 = 2 * (qx * qy + qw * qz); 26 | R22 = qw2 - qx2 + qy2 - qz2; 27 | R23 = 2 * (qy * qz - qw * qx); 28 | 29 | R31 = 2 * (qx * qz - qw * qy); 30 | R32 = 2 * (qy * qz + qw * qx); 31 | R33 = qw2 - qx2 - qy2 + qz2; 32 | 33 | R = [R11, R12, R13; R21, R22, R23; R31, R32, R33;]; 34 | endfunction 35 | 36 | function euler = quat2euler(q) 37 | qw = q(1); 38 | qx = q(2); 39 | qy = q(3); 40 | qz = q(4); 41 | 42 | qw2 = qw**2; 43 | qx2 = qx**2; 44 | qy2 = qy**2; 45 | qz2 = qz**2; 46 | 47 | t1 = atan2(2 * (qx * qw + qz * qy), (qw2 - qx2 - qy2 + qz2)); 48 | t2 = asin(2 * (qy * qw - qx * qz)); 49 | t3 = atan2(2 * (qx * qy + qz * qw), (qw2 + qx2 - qy2 - qz2)); 50 | euler = [t1; t2; t3]; 51 | endfunction 52 | 53 | function T = tf(rot, trans) 54 | assert(size(rot) == [3, 3] || size(rot) == [4, 1]); 55 | assert(size(trans) == [3, 1]); 56 | 57 | C = rot; 58 | if size(rot) == [4, 1] 59 | C = quat2rot(rot); 60 | endif 61 | 62 | T = eye(4, 4); 63 | T(1:3, 1:3) = C; 64 | T(1:3, 4) = trans; 65 | endfunction 66 | 67 | function C = tf_rot(tf) 68 | C = tf(1:3, 1:3); 69 | endfunction 70 | 71 | function r = tf_trans(T) 72 | r = T(1:3, 4); 73 | endfunction 74 | 75 | function hp = homogeneous(p) 76 | hp = [p; ones(1, columns(p))]; 77 | endfunction 78 | 79 | function cam_K = load_cam_K(data_dir) 80 | cam_K = csvread(strcat(data_dir, "/camera.csv"), 1, 0); 81 | endfunction 82 | 83 | function cam_poses = load_cam_poses(data_dir) 84 | csv = csvread(strcat(data_dir, "/camera_poses.csv"), 1, 0); 85 | cam_poses = {}; 86 | for i = 1:rows(csv) 87 | cam_poses{i}.q_WC = csv(i, 1:4)'; 88 | cam_poses{i}.r_WC = csv(i, 5:7)'; 89 | endfor 90 | endfunction 91 | 92 | function keypoints = load_keypoints(data_dir) 93 | csv = csvread(strcat(data_dir, "/keypoints.csv"), 1, 0); 94 | keypoints = {}; 95 | for i = 1:rows(csv) 96 | keypoints.frame{i} = {}; 97 | nb_keypoints = csv(i, 1); 98 | for j = 1:nb_keypoints 99 | keypoints.frame{i}{j} = csv(i, 1 + j); 100 | endfor 101 | endfor 102 | endfunction 103 | 104 | function point_ids = load_point_ids(data_dir) 105 | csv = csvread(strcat(data_dir, "/point_ids.csv"), 1, 0); 106 | point_ids = {}; 107 | for i = 1:rows(csv) 108 | point_ids.frame{i} = {}; 109 | nb_point_ids = csv(i, 1); 110 | for j = 1:nb_point_ids 111 | point_ids.frame{i}{j} = csv(i, 1 + j); 112 | endfor 113 | endfor 114 | endfunction 115 | 116 | function points = load_points(data_dir) 117 | points = csvread(strcat(data_dir, "/points.csv"), 1, 0); 118 | endfunction 119 | 120 | function T_WT = load_target_pose(data_dir) 121 | csv = csvread(strcat(data_dir, "/target_pose.csv"), 1, 0); 122 | q_WT = csv(1, 1:4)'; 123 | r_WT = csv(1, 5:7)'; 124 | T_WT = tf(q_WT, r_WT); 125 | endfunction 126 | 127 | function data = load_data(data_dir) 128 | data = {}; 129 | data.cam_K = load_cam_K(data_dir); 130 | data.cam_poses = load_cam_poses(data_dir); 131 | data.keypoints = load_keypoints(data_dir); 132 | data.point_ids = load_point_ids(data_dir); 133 | data.points = load_points(data_dir); 134 | data.T_WT = load_target_pose(data_dir); 135 | endfunction 136 | 137 | function draw_frame(T_WB, scale=1.1) 138 | r_WB = tf_trans(T_WB); 139 | origin = r_WB; 140 | 141 | x_axis = T_WB * homogeneous(scale * [1; 0; 0]); 142 | y_axis = T_WB * homogeneous(scale * [0; 1; 0]); 143 | z_axis = T_WB * homogeneous(scale * [0; 0; 1]); 144 | 145 | % Draw x-axis 146 | plot3([origin(1), x_axis(1)], ... 147 | [origin(2), x_axis(2)], ... 148 | [origin(3), x_axis(3)], 'r', 149 | "linewidth", 5) 150 | 151 | % Draw y-axis 152 | plot3([origin(1), y_axis(1)], ... 153 | [origin(2), y_axis(2)], ... 154 | [origin(3), y_axis(3)], 'g', 155 | "linewidth", 5) 156 | 157 | % Draw z-axis 158 | plot3([origin(1), z_axis(1)], ... 159 | [origin(2), z_axis(2)], ... 160 | [origin(3), z_axis(3)], 'b', 161 | "linewidth", 5) 162 | endfunction 163 | 164 | function draw_camera(T_WC, scale=0.05, style="b-") 165 | fov = deg2rad(60.0); 166 | 167 | % Form the camera fov frame 168 | fov_hwidth = scale; 169 | fov_corners = zeros(3, 4); 170 | fov_corners(1:3, 1) = [-fov_hwidth; fov_hwidth; 0.0]; % Bottom left 171 | fov_corners(1:3, 2) = [-fov_hwidth; -fov_hwidth; 0.0]; % Top left 172 | fov_corners(1:3, 3) = [fov_hwidth; -fov_hwidth; 0.0]; % Top right 173 | fov_corners(1:3, 4) = [fov_hwidth; fov_hwidth; 0.0]; % Bottom right 174 | 175 | % Calculate the distance from camera origin to fov frame given fov 176 | dist = fov_hwidth / tan(fov / 2.0); 177 | fov_corners(3, :) = dist; 178 | 179 | % Transform fov_corners to world frame 180 | fov_corners = T_WC * [fov_corners; ones(1, 4)]; 181 | fov_corners = fov_corners(1:3, :); 182 | 183 | % Transform camera_origin to world frame 184 | cam_origin = [0; 0; 0]; 185 | cam_origin = T_WC * [cam_origin; 1.0]; 186 | cam_origin = cam_origin(1:3, :); 187 | 188 | % Draw fov frame 189 | frame_x = [fov_corners(1, :), fov_corners(1, 1)]; 190 | frame_y = [fov_corners(2, :), fov_corners(2, 1)]; 191 | frame_z = [fov_corners(3, :), fov_corners(3, 1)]; 192 | plot3(frame_x, frame_y, frame_z, style); 193 | 194 | % Draw from camera origin to fov frame 195 | for i = 1:4 196 | x = [cam_origin(1), fov_corners(1, i)]; 197 | y = [cam_origin(2), fov_corners(2, i)]; 198 | z = [cam_origin(3), fov_corners(3, i)]; 199 | plot3(x, y, z, style); 200 | endfor 201 | endfunction 202 | 203 | function plot_data(data) 204 | figure(); 205 | clf(); 206 | hold on; 207 | 208 | % Draw camera poses 209 | for i = 1:length(data.cam_poses) 210 | q_WC = data.cam_poses{i}.q_WC; 211 | r_WC = data.cam_poses{i}.r_WC; 212 | T_WC = tf(q_WC, r_WC); 213 | 214 | draw_camera(T_WC); 215 | draw_frame(T_WC, 0.05); 216 | endfor 217 | 218 | % 3D landmark points 219 | scatter3(data.points(:, 1), 220 | data.points(:, 2), 221 | data.points(:, 3), 'r'); 222 | 223 | xlabel("x [m]"); 224 | ylabel("y [m]"); 225 | zlabel("z [m]"); 226 | view(3); 227 | axis 'equal'; 228 | ginput; 229 | endfunction 230 | 231 | function plot_compare_data(title_name, data0, data1, save_figs) 232 | figure(); 233 | clf(); 234 | hold on; 235 | 236 | for i = 1:length(data0.cam_poses) 237 | q_WC = data0.cam_poses{i}.q_WC; 238 | r_WC = data0.cam_poses{i}.r_WC; 239 | T_WC = tf(q_WC, r_WC); 240 | 241 | draw_camera(T_WC, 0.05, 'r-'); 242 | % draw_frame(T_WC, 0.05); 243 | endfor 244 | 245 | for i = 1:length(data1.cam_poses) 246 | q_WC = data1.cam_poses{i}.q_WC; 247 | r_WC = data1.cam_poses{i}.r_WC; 248 | T_WC = tf(q_WC, r_WC); 249 | 250 | draw_camera(T_WC, 0.05, 'b-'); 251 | % draw_frame(T_WC, 0.05); 252 | endfor 253 | 254 | scatter3(data0.points(:, 1), 255 | data0.points(:, 2), 256 | data0.points(:, 3), 'r'); 257 | 258 | scatter3(data1.points(:, 1), 259 | data1.points(:, 2), 260 | data1.points(:, 3), 'b'); 261 | 262 | % Plot settings 263 | if save_figs 264 | title(title_name, "fontsize", 40); 265 | else 266 | title(title_name, "fontsize", 20); 267 | end 268 | xlabel("x [m]"); 269 | ylabel("y [m]"); 270 | zlabel("z [m]"); 271 | view(3); 272 | axis 'equal'; 273 | endfunction 274 | 275 | % Main 276 | data_gnd = load_data(data_gnd_dir); 277 | data_noisy = load_data(data_noisy_dir); 278 | data_est = load_data(data_est_dir); 279 | 280 | plot_compare_data("Before Optimization", data_gnd, data_noisy, save_figs); 281 | if save_figs 282 | print('-dpng', '-S1200,1200', 'ba_before.png') 283 | endif 284 | 285 | plot_compare_data("After Optimization", data_gnd, data_est, save_figs); 286 | if save_figs 287 | print('-dpng', '-S1200,1200', 'ba_after.png') 288 | endif 289 | 290 | if show_figs 291 | ginput(); 292 | endif 293 | -------------------------------------------------------------------------------- /ba/data_gnd/keypoints.csv: -------------------------------------------------------------------------------- 1 | #size,keypoints 2 | 50,3.982364,383.329805,174.570727,386.712990,325.164074,389.699625,459.083269,392.355571,578.952358,394.732868,82.826967,278.300508,208.817350,285.845436,323.417892,292.708284,428.106302,298.977545,524.115265,304.727037,129.986561,215.478972,229.827021,223.965054,322.321812,231.826780,408.252847,239.130610,488.294864,245.933894,161.365210,173.679315,244.031276,182.128857,321.569762,190.054294,394.443393,197.502923,463.060847,204.516515,183.748688,143.862170,254.275824,151.955298,321.021740,159.614523,384.282583,166.873829,444.324364,173.763741 3 | 50,175.985524,367.052161,255.937620,364.336701,328.024371,361.888377,393.352002,359.669617,452.828581,357.649579,171.733642,327.059478,262.197894,326.329938,342.683189,325.680873,414.754361,325.099662,479.665317,324.576194,166.139391,274.440628,270.302246,277.127587,361.388997,279.477239,441.716489,281.549347,513.084751,283.390345,158.447907,202.095467,281.206224,210.928424,386.086124,218.474957,476.728199,224.997021,555.847418,230.689969,147.207783,96.372218,296.663393,117.086367,420.201898,134.208475,524.026848,148.598336,612.508235,160.861620 4 | 50,226.310204,290.643771,275.688432,293.915188,329.527427,297.482142,388.460254,301.386574,453.245718,305.678755,214.025001,256.234849,266.830697,258.163805,324.812315,260.281833,388.770073,262.618166,459.678111,265.208388,199.677949,216.051011,256.410774,216.107150,319.221083,216.169303,389.140717,216.238491,467.449111,216.315979,182.702434,168.505261,243.975422,165.915871,312.484534,163.020683,389.592049,159.762128,477.025099,156.067217,162.303290,111.370467,228.877299,104.977176,304.210597,97.742706,390.153636,89.489351,489.117579,79.985563 5 | 48,195.638288,340.976947,258.479824,319.194807,316.212220,299.183598,369.434212,280.735786,418.654498,263.675046,166.028429,322.638498,241.705108,297.838368,310.170467,275.401471,372.408392,255.005378,429.231570,236.383760,122.510533,295.686303,217.532264,267.063130,301.617470,241.734340,376.551516,219.162133,443.751215,198.919726,52.277736,252.188611,179.682564,218.875440,288.576781,190.402329,382.721280,165.785900,464.922752,144.292274,111.969448,132.667658,266.260269,102.557930,392.886148,77.846992,498.675547,57.202276 6 | 50,97.352552,407.836260,211.443508,368.842901,304.701729,336.969636,382.355856,310.429449,448.019888,287.987169,77.756775,324.817889,184.572510,296.606961,273.305953,273.171717,348.190378,253.394114,412.233302,236.479843,61.323105,255.195923,161.666222,235.029150,246.177053,218.044322,318.328667,203.543425,380.647520,191.018699,47.343296,195.969850,141.907464,181.912685,222.500535,169.932354,292.005701,159.600263,352.563882,150.598160,35.305920,144.972976,124.689195,135.625685,201.656807,127.576766,268.627632,120.573265,327.430521,114.423930 7 | 48,107.460621,381.340498,201.288301,400.295491,312.955944,422.854500,448.086082,450.153383,147.485656,300.865698,232.866737,311.367938,332.328812,323.602202,449.668274,338.035462,590.181008,355.319134,179.328790,236.841522,257.551846,241.852578,347.164849,247.593286,450.850847,254.235526,572.207399,262.009762,205.266188,184.691488,277.378653,186.018608,358.890692,187.518714,451.768224,189.227984,558.566488,191.193444,226.801373,141.392595,293.652915,140.188906,368.391572,138.843206,452.500619,137.328789,547.860138,135.611802 8 | 50,77.495749,365.457615,206.808761,348.358085,326.099475,332.583839,436.489579,317.986553,538.939396,304.439241,109.065487,283.257888,212.220159,272.151544,308.884907,261.743950,399.653518,251.971175,485.049486,242.776855,130.073326,228.558714,215.870364,220.747175,297.124667,213.349237,374.187720,206.332898,447.375657,199.669376,145.060037,189.537055,218.498840,183.731409,288.581009,178.191119,355.531533,172.898399,419.555733,167.837017,156.289758,160.297661,220.481907,155.804675,282.093027,151.492342,341.275718,147.349982,398.170780,143.367740 9 | 46,221.120473,327.019827,262.244287,349.876412,316.153617,379.839177,389.908546,420.832110,224.767837,282.887533,268.223294,300.555137,325.884543,323.998365,406.078559,356.602698,525.220703,405.042099,228.709571,235.193435,274.755781,246.668156,336.671494,262.097559,424.368736,283.951722,558.184717,317.298680,232.982809,183.488205,281.922333,187.550723,348.696239,193.093691,445.225313,201.106664,597.100100,213.713940,237.631207,127.243620,289.819976,122.402468,362.184798,115.689739,469.229208,105.760051 10 | 50,6.210473,446.388820,175.371628,428.674923,319.029844,413.631595,442.548134,400.697241,549.884295,389.457417,67.218135,328.094592,194.242251,321.851343,306.193495,316.348926,405.604462,311.462866,494.470458,307.095093,104.484362,255.835151,206.146969,254.460633,297.863775,253.220585,381.026235,252.096196,456.777809,251.072005,129.610450,207.115515,214.341895,208.070467,292.021782,208.945945,363.495217,209.751475,429.477450,210.495116,147.697768,172.044096,220.327349,174.187765,287.697849,176.176213,350.360522,178.025708,408.792180,179.750325 11 | 50,26.144139,393.222159,182.602066,411.466910,281.558696,423.006364,349.792481,430.963189,399.688514,436.781627,103.688450,285.560151,209.999378,318.051898,285.077879,340.998097,340.924537,358.066479,384.089908,371.259086,146.921044,225.536305,226.963876,260.209041,287.420923,286.397659,334.697257,306.876693,372.679634,323.329795,174.489626,187.260268,238.501595,220.869554,289.093020,247.432425,330.084006,268.954615,363.970465,286.746595,193.601575,160.725365,246.857436,192.379134,290.346270,218.227660,326.529270,239.733806,357.104778,257.907018 12 | 48,151.627687,401.450797,230.718188,349.915541,283.918808,315.250094,322.153914,290.336155,350.959616,271.566400,108.435464,334.819095,199.903129,294.598308,260.314986,268.033593,303.192107,249.179372,335.201240,235.104097,60.084929,260.229775,166.195743,234.088952,234.886336,217.166750,282.984089,205.317676,318.542196,196.557794,5.593335,176.166778,129.168763,167.620472,207.412729,162.209227,261.402830,158.475346,300.902994,155.743569,88.306407,94.266970,177.637151,102.646897,238.303428,108.337872,282.194511,112.455201 13 | 40,214.897388,342.791928,263.021611,373.125700,324.707172,412.007485,406.626570,463.643110,212.089858,301.427628,264.900356,329.310273,334.408311,366.008765,430.019728,416.489244,208.708702,251.611886,267.221531,275.176650,346.805335,307.227290,461.341229,353.354121,204.558113,190.459823,270.162277,206.593614,363.203338,229.474859,505.444028,264.455587,199.341726,113.604990,274.008865,116.884855,385.911910,121.800362,572.157001,129.981453 14 | 50,201.490738,309.490521,262.207581,309.943394,320.470463,310.377964,376.425208,310.795318,430.206310,311.196459,191.465116,282.712781,261.179647,283.862985,327.679371,284.960149,391.181648,286.007859,451.884674,287.009385,177.962442,246.648074,259.805041,248.986928,337.254237,251.200229,410.654547,253.297824,480.315385,255.288554,158.794392,195.451543,257.872820,199.963225,350.588478,204.185170,437.535219,208.144418,519.235224,211.864748,129.448803,117.071509,254.957354,125.992966,370.436980,134.201543,477.043561,141.779401,575.761921,148.796546 15 | 50,179.385066,357.514144,248.503190,367.251325,329.628890,378.680102,426.188849,392.283217,543.050680,408.746407,186.699169,297.738940,254.269850,302.547143,333.190072,308.162957,426.581494,314.808515,538.826139,322.795633,193.609475,241.263799,259.694212,241.683687,336.522685,242.171838,426.946709,242.746371,534.925382,243.432443,200.148525,187.822779,264.805873,184.328846,339.648051,180.284549,427.287270,175.548729,531.312560,169.927445,206.345457,137.177763,269.631116,130.187727,342.584919,122.129824,427.605592,112.739110,527.956926,101.655091 16 | 48,196.885841,318.978153,251.865896,335.204519,314.010729,353.545439,384.818483,374.443063,466.236273,398.472046,183.749846,284.303393,245.066853,300.267712,315.505948,318.607032,397.266350,339.893934,493.315989,364.901142,166.920075,239.878264,236.203491,254.723430,317.494482,272.141426,414.210782,292.864560,531.203140,317.932190,144.583634,180.917313,224.167394,192.876084,320.268702,207.316890,438.625299,225.101919,587.980442,247.544991,113.509224,98.890965,206.883614,104.063585,324.408892,110.574082,476.846550,119.018605 17 | 48,222.738099,301.036475,264.259682,302.817547,316.619365,305.063521,384.697599,307.983744,476.822603,311.935455,198.321967,262.639066,239.326456,259.829665,291.759275,256.237258,361.171807,251.481494,457.405581,244.888086,171.173533,219.944770,211.240218,211.405608,263.285481,200.313527,333.625228,185.322457,433.962376,163.938229,140.806971,172.189568,179.362510,156.444602,230.348286,135.623487,300.930756,106.799649,405.095877,64.261629,106.614025,118.416900,142.870024,93.527164,191.808947,59.930604,261.496601,12.090044 18 | 40,164.496726,357.083406,236.008196,320.808834,298.595012,289.061341,353.829744,261.043231,402.935999,236.133823,112.116119,321.586258,198.406420,282.567820,272.421453,249.099969,336.606617,220.076953,392.798218,194.668437,39.324854,272.257271,147.473562,230.769093,237.735842,196.142535,314.210069,166.805372,379.831364,141.631627,74.592652,156.649193,189.579480,122.618171,283.897187,94.704296,362.659176,71.394226,118.213921,13.658378 19 | 36,118.389678,391.230527,204.629316,425.104674,304.827002,464.461416,130.507852,324.463531,223.844907,355.390083,333.455339,391.708699,464.001499,434.964217,144.331952,248.297470,245.999047,275.014373,366.875262,306.779204,512.972658,345.171860,160.249477,160.597352,271.821610,181.329540,406.399606,206.336647,571.917019,237.092877,178.774902,58.528596,302.306006,70.731475,453.869005,85.703453 20 | 50,63.514473,364.164995,204.820713,368.148427,340.356370,371.969185,470.467855,375.637036,595.474394,379.160978,106.656637,264.451802,226.342958,269.619101,341.850309,274.605979,453.393803,279.421722,561.174040,284.074992,138.528326,190.787718,242.329779,196.431074,342.965872,201.902340,440.579220,207.209268,535.303995,212.359154,163.035764,134.144408,254.673372,139.921825,343.830651,145.542866,430.606957,151.013795,515.096411,156.340546,182.466984,89.233611,264.491729,94.973167,344.520666,100.573071,422.625764,106.038356,498.875570,111.373820 21 | 44,202.372761,321.456716,246.716331,347.344877,303.795807,380.668370,380.018066,425.167587,202.255287,277.232893,250.136664,299.197433,313.045082,328.055302,399.371324,367.655589,525.181792,425.368405,202.119975,226.293462,254.152978,242.660471,324.184753,264.689003,423.507253,295.930948,575.355777,343.694983,201.962425,166.983034,258.935911,175.331949,337.860335,186.897563,454.448671,203.982461,201.776668,97.053448,264.728114,93.796097,355.048991,89.122546,495.546345,81.852670 22 | -------------------------------------------------------------------------------- /ba/data_noisy/keypoints.csv: -------------------------------------------------------------------------------- 1 | #size,keypoints 2 | 50,3.982364,383.329805,174.570727,386.712990,325.164074,389.699625,459.083269,392.355571,578.952358,394.732868,82.826967,278.300508,208.817350,285.845436,323.417892,292.708284,428.106302,298.977545,524.115265,304.727037,129.986561,215.478972,229.827021,223.965054,322.321812,231.826780,408.252847,239.130610,488.294864,245.933894,161.365210,173.679315,244.031276,182.128857,321.569762,190.054294,394.443393,197.502923,463.060847,204.516515,183.748688,143.862170,254.275824,151.955298,321.021740,159.614523,384.282583,166.873829,444.324364,173.763741 3 | 50,175.985524,367.052161,255.937620,364.336701,328.024371,361.888377,393.352002,359.669617,452.828581,357.649579,171.733642,327.059478,262.197894,326.329938,342.683189,325.680873,414.754361,325.099662,479.665317,324.576194,166.139391,274.440628,270.302246,277.127587,361.388997,279.477239,441.716489,281.549347,513.084751,283.390345,158.447907,202.095467,281.206224,210.928424,386.086124,218.474957,476.728199,224.997021,555.847418,230.689969,147.207783,96.372218,296.663393,117.086367,420.201898,134.208475,524.026848,148.598336,612.508235,160.861620 4 | 50,226.310204,290.643771,275.688432,293.915188,329.527427,297.482142,388.460254,301.386574,453.245718,305.678755,214.025001,256.234849,266.830697,258.163805,324.812315,260.281833,388.770073,262.618166,459.678111,265.208388,199.677949,216.051011,256.410774,216.107150,319.221083,216.169303,389.140717,216.238491,467.449111,216.315979,182.702434,168.505261,243.975422,165.915871,312.484534,163.020683,389.592049,159.762128,477.025099,156.067217,162.303290,111.370467,228.877299,104.977176,304.210597,97.742706,390.153636,89.489351,489.117579,79.985563 5 | 48,195.638288,340.976947,258.479824,319.194807,316.212220,299.183598,369.434212,280.735786,418.654498,263.675046,166.028429,322.638498,241.705108,297.838368,310.170467,275.401471,372.408392,255.005378,429.231570,236.383760,122.510533,295.686303,217.532264,267.063130,301.617470,241.734340,376.551516,219.162133,443.751215,198.919726,52.277736,252.188611,179.682564,218.875440,288.576781,190.402329,382.721280,165.785900,464.922752,144.292274,111.969448,132.667658,266.260269,102.557930,392.886148,77.846992,498.675547,57.202276 6 | 50,97.352552,407.836260,211.443508,368.842901,304.701729,336.969636,382.355856,310.429449,448.019888,287.987169,77.756775,324.817889,184.572510,296.606961,273.305953,273.171717,348.190378,253.394114,412.233302,236.479843,61.323105,255.195923,161.666222,235.029150,246.177053,218.044322,318.328667,203.543425,380.647520,191.018699,47.343296,195.969850,141.907464,181.912685,222.500535,169.932354,292.005701,159.600263,352.563882,150.598160,35.305920,144.972976,124.689195,135.625685,201.656807,127.576766,268.627632,120.573265,327.430521,114.423930 7 | 48,107.460621,381.340498,201.288301,400.295491,312.955944,422.854500,448.086082,450.153383,147.485656,300.865698,232.866737,311.367938,332.328812,323.602202,449.668274,338.035462,590.181008,355.319134,179.328790,236.841522,257.551846,241.852578,347.164849,247.593286,450.850847,254.235526,572.207399,262.009762,205.266188,184.691488,277.378653,186.018608,358.890692,187.518714,451.768224,189.227984,558.566488,191.193444,226.801373,141.392595,293.652915,140.188906,368.391572,138.843206,452.500619,137.328789,547.860138,135.611802 8 | 50,77.495749,365.457615,206.808761,348.358085,326.099475,332.583839,436.489579,317.986553,538.939396,304.439241,109.065487,283.257888,212.220159,272.151544,308.884907,261.743950,399.653518,251.971175,485.049486,242.776855,130.073326,228.558714,215.870364,220.747175,297.124667,213.349237,374.187720,206.332898,447.375657,199.669376,145.060037,189.537055,218.498840,183.731409,288.581009,178.191119,355.531533,172.898399,419.555733,167.837017,156.289758,160.297661,220.481907,155.804675,282.093027,151.492342,341.275718,147.349982,398.170780,143.367740 9 | 46,221.120473,327.019827,262.244287,349.876412,316.153617,379.839177,389.908546,420.832110,224.767837,282.887533,268.223294,300.555137,325.884543,323.998365,406.078559,356.602698,525.220703,405.042099,228.709571,235.193435,274.755781,246.668156,336.671494,262.097559,424.368736,283.951722,558.184717,317.298680,232.982809,183.488205,281.922333,187.550723,348.696239,193.093691,445.225313,201.106664,597.100100,213.713940,237.631207,127.243620,289.819976,122.402468,362.184798,115.689739,469.229208,105.760051 10 | 50,6.210473,446.388820,175.371628,428.674923,319.029844,413.631595,442.548134,400.697241,549.884295,389.457417,67.218135,328.094592,194.242251,321.851343,306.193495,316.348926,405.604462,311.462866,494.470458,307.095093,104.484362,255.835151,206.146969,254.460633,297.863775,253.220585,381.026235,252.096196,456.777809,251.072005,129.610450,207.115515,214.341895,208.070467,292.021782,208.945945,363.495217,209.751475,429.477450,210.495116,147.697768,172.044096,220.327349,174.187765,287.697849,176.176213,350.360522,178.025708,408.792180,179.750325 11 | 50,26.144139,393.222159,182.602066,411.466910,281.558696,423.006364,349.792481,430.963189,399.688514,436.781627,103.688450,285.560151,209.999378,318.051898,285.077879,340.998097,340.924537,358.066479,384.089908,371.259086,146.921044,225.536305,226.963876,260.209041,287.420923,286.397659,334.697257,306.876693,372.679634,323.329795,174.489626,187.260268,238.501595,220.869554,289.093020,247.432425,330.084006,268.954615,363.970465,286.746595,193.601575,160.725365,246.857436,192.379134,290.346270,218.227660,326.529270,239.733806,357.104778,257.907018 12 | 48,151.627687,401.450797,230.718188,349.915541,283.918808,315.250094,322.153914,290.336155,350.959616,271.566400,108.435464,334.819095,199.903129,294.598308,260.314986,268.033593,303.192107,249.179372,335.201240,235.104097,60.084929,260.229775,166.195743,234.088952,234.886336,217.166750,282.984089,205.317676,318.542196,196.557794,5.593335,176.166778,129.168763,167.620472,207.412729,162.209227,261.402830,158.475346,300.902994,155.743569,88.306407,94.266970,177.637151,102.646897,238.303428,108.337872,282.194511,112.455201 13 | 40,214.897388,342.791928,263.021611,373.125700,324.707172,412.007485,406.626570,463.643110,212.089858,301.427628,264.900356,329.310273,334.408311,366.008765,430.019728,416.489244,208.708702,251.611886,267.221531,275.176650,346.805335,307.227290,461.341229,353.354121,204.558113,190.459823,270.162277,206.593614,363.203338,229.474859,505.444028,264.455587,199.341726,113.604990,274.008865,116.884855,385.911910,121.800362,572.157001,129.981453 14 | 50,201.490738,309.490521,262.207581,309.943394,320.470463,310.377964,376.425208,310.795318,430.206310,311.196459,191.465116,282.712781,261.179647,283.862985,327.679371,284.960149,391.181648,286.007859,451.884674,287.009385,177.962442,246.648074,259.805041,248.986928,337.254237,251.200229,410.654547,253.297824,480.315385,255.288554,158.794392,195.451543,257.872820,199.963225,350.588478,204.185170,437.535219,208.144418,519.235224,211.864748,129.448803,117.071509,254.957354,125.992966,370.436980,134.201543,477.043561,141.779401,575.761921,148.796546 15 | 50,179.385066,357.514144,248.503190,367.251325,329.628890,378.680102,426.188849,392.283217,543.050680,408.746407,186.699169,297.738940,254.269850,302.547143,333.190072,308.162957,426.581494,314.808515,538.826139,322.795633,193.609475,241.263799,259.694212,241.683687,336.522685,242.171838,426.946709,242.746371,534.925382,243.432443,200.148525,187.822779,264.805873,184.328846,339.648051,180.284549,427.287270,175.548729,531.312560,169.927445,206.345457,137.177763,269.631116,130.187727,342.584919,122.129824,427.605592,112.739110,527.956926,101.655091 16 | 48,196.885841,318.978153,251.865896,335.204519,314.010729,353.545439,384.818483,374.443063,466.236273,398.472046,183.749846,284.303393,245.066853,300.267712,315.505948,318.607032,397.266350,339.893934,493.315989,364.901142,166.920075,239.878264,236.203491,254.723430,317.494482,272.141426,414.210782,292.864560,531.203140,317.932190,144.583634,180.917313,224.167394,192.876084,320.268702,207.316890,438.625299,225.101919,587.980442,247.544991,113.509224,98.890965,206.883614,104.063585,324.408892,110.574082,476.846550,119.018605 17 | 48,222.738099,301.036475,264.259682,302.817547,316.619365,305.063521,384.697599,307.983744,476.822603,311.935455,198.321967,262.639066,239.326456,259.829665,291.759275,256.237258,361.171807,251.481494,457.405581,244.888086,171.173533,219.944770,211.240218,211.405608,263.285481,200.313527,333.625228,185.322457,433.962376,163.938229,140.806971,172.189568,179.362510,156.444602,230.348286,135.623487,300.930756,106.799649,405.095877,64.261629,106.614025,118.416900,142.870024,93.527164,191.808947,59.930604,261.496601,12.090044 18 | 40,164.496726,357.083406,236.008196,320.808834,298.595012,289.061341,353.829744,261.043231,402.935999,236.133823,112.116119,321.586258,198.406420,282.567820,272.421453,249.099969,336.606617,220.076953,392.798218,194.668437,39.324854,272.257271,147.473562,230.769093,237.735842,196.142535,314.210069,166.805372,379.831364,141.631627,74.592652,156.649193,189.579480,122.618171,283.897187,94.704296,362.659176,71.394226,118.213921,13.658378 19 | 36,118.389678,391.230527,204.629316,425.104674,304.827002,464.461416,130.507852,324.463531,223.844907,355.390083,333.455339,391.708699,464.001499,434.964217,144.331952,248.297470,245.999047,275.014373,366.875262,306.779204,512.972658,345.171860,160.249477,160.597352,271.821610,181.329540,406.399606,206.336647,571.917019,237.092877,178.774902,58.528596,302.306006,70.731475,453.869005,85.703453 20 | 50,63.514473,364.164995,204.820713,368.148427,340.356370,371.969185,470.467855,375.637036,595.474394,379.160978,106.656637,264.451802,226.342958,269.619101,341.850309,274.605979,453.393803,279.421722,561.174040,284.074992,138.528326,190.787718,242.329779,196.431074,342.965872,201.902340,440.579220,207.209268,535.303995,212.359154,163.035764,134.144408,254.673372,139.921825,343.830651,145.542866,430.606957,151.013795,515.096411,156.340546,182.466984,89.233611,264.491729,94.973167,344.520666,100.573071,422.625764,106.038356,498.875570,111.373820 21 | 44,202.372761,321.456716,246.716331,347.344877,303.795807,380.668370,380.018066,425.167587,202.255287,277.232893,250.136664,299.197433,313.045082,328.055302,399.371324,367.655589,525.181792,425.368405,202.119975,226.293462,254.152978,242.660471,324.184753,264.689003,423.507253,295.930948,575.355777,343.694983,201.962425,166.983034,258.935911,175.331949,337.860335,186.897563,454.448671,203.982461,201.776668,97.053448,264.728114,93.796097,355.048991,89.122546,495.546345,81.852670 22 | -------------------------------------------------------------------------------- /ba/cpp/test_ba.cpp: -------------------------------------------------------------------------------- 1 | #include "munit.hpp" 2 | #include "ba.hpp" 3 | 4 | #define TEST_DATA "./data_noisy" 5 | 6 | // static int check_J_cam_pose(const mat3_t &cam_K, 7 | // const mat4_t &T_WC, 8 | // const vec3_t &p_W, 9 | // const mat_t<2, 6> &J_cam_pose, 10 | // const real_t step_size = 1e-3, 11 | // const real_t threshold = 1e-2) { 12 | // const vec2_t z{0.0, 0.0}; 13 | // const vec4_t hp_W = p_W.homogeneous(); 14 | // 15 | // 16 | // // Perturb rotation 17 | // matx_t fdiff = zeros(2, 6); 18 | // for (int i = 0; i < 3; i++) { 19 | // // Forward difference 20 | // const mat4_t T_WC_fd = tf_perturb_rot(T_WC, step_size, i); 21 | // const mat4_t T_CW_fd = T_WC_fd.inverse(); 22 | // const vec3_t p_C_fd = (T_CW_fd * hp_W).head(3); 23 | // const vec3_t x_fd{p_C_fd(0) / p_C_fd(2), p_C_fd(1) / p_C_fd(2), 1.0}; 24 | // const vec2_t z_fd = (cam_K * x_fd).head(2); 25 | // const vec2_t e_fd = z - z_fd; 26 | // 27 | // // Backward difference 28 | // const mat4_t T_WC_bd = tf_perturb_rot(T_WC, -step_size, i); 29 | // const mat4_t T_CW_bd = T_WC_bd.inverse(); 30 | // const vec3_t p_C_bd = (T_CW_bd * hp_W).head(3); 31 | // const vec3_t x_bd{p_C_bd(0) / p_C_bd(2), p_C_bd(1) / p_C_bd(2), 1.0}; 32 | // const vec2_t z_bd = (cam_K * x_bd).head(2); 33 | // const vec2_t e_bd = z - z_bd; 34 | // 35 | // // Calculate central finite difference 36 | // fdiff.block(0, i, 2, 1) = (e_fd - e_bd) / (2 * step_size); 37 | // } 38 | // 39 | // // Perturb translation 40 | // for (int i = 0; i < 3; i++) { 41 | // // Forward difference 42 | // const mat4_t T_WC_fd = tf_perturb_trans(T_WC, step_size, i); 43 | // const mat4_t T_CW_fd = T_WC_fd.inverse(); 44 | // const vec3_t p_C_fd = (T_CW_fd * hp_W).head(3); 45 | // const vec3_t x_fd{p_C_fd(0) / p_C_fd(2), p_C_fd(1) / p_C_fd(2), 1.0}; 46 | // const vec2_t z_fd = (cam_K * x_fd).head(2); 47 | // const vec2_t e_fd = z - z_fd; 48 | // 49 | // // Backward difference 50 | // const mat4_t T_WC_bd = tf_perturb_trans(T_WC, -step_size, i); 51 | // const mat4_t T_CW_bd = T_WC_bd.inverse(); 52 | // const vec3_t p_C_bd = (T_CW_bd * hp_W).head(3); 53 | // const vec3_t x_bd{p_C_bd(0) / p_C_bd(2), p_C_bd(1) / p_C_bd(2), 1.0}; 54 | // const vec2_t z_bd = (cam_K * x_bd).head(2); 55 | // const vec2_t e_bd = z - z_bd; 56 | // 57 | // // Calculate central finite difference 58 | // fdiff.block(0, i + 3, 2, 1) = (e_fd - e_bd) / (2 * step_size); 59 | // } 60 | // 61 | // return check_jacobian("J_cam_pose", fdiff, J_cam_pose, threshold, true); 62 | // } 63 | // 64 | // static int check_J_point(const mat3_t &cam_K, 65 | // const mat4_t &T_WC, 66 | // const vec3_t &p_W, 67 | // const mat_t<2, 3> &J_point, 68 | // const real_t step_size = 1e-10, 69 | // const real_t threshold = 1e-2) { 70 | // const vec2_t z{0.0, 0.0}; 71 | // const mat4_t T_CW = T_WC.inverse(); 72 | // matx_t fdiff = zeros(2, 3); 73 | // mat3_t dr = I(3) * step_size; 74 | // 75 | // // Perturb landmark 76 | // for (int i = 0; i < 3; i++) { 77 | // // Forward difference 78 | // const vec3_t p_W_fd = p_W + dr.col(i); 79 | // const vec4_t hp_W_fd = p_W_fd.homogeneous(); 80 | // const vec3_t p_C_fd = (T_CW * hp_W_fd).head(3); 81 | // const vec3_t x_fd{p_C_fd(0) / p_C_fd(2), p_C_fd(1) / p_C_fd(2), 1.0}; 82 | // const vec2_t z_fd = (cam_K * x_fd).head(2); 83 | // const vec2_t e_fd = z - z_fd; 84 | // 85 | // // Backward difference 86 | // const vec3_t p_W_bd = p_W - dr.col(i); 87 | // const vec4_t hp_W_bd = p_W_bd.homogeneous(); 88 | // const vec3_t p_C_bd = (T_CW * hp_W_bd).head(3); 89 | // const vec3_t x_bd{p_C_bd(0) / p_C_bd(2), p_C_bd(1) / p_C_bd(2), 1.0}; 90 | // const vec2_t z_bd = (cam_K * x_bd).head(2); 91 | // const vec2_t e_bd = z - z_bd; 92 | // 93 | // // Calculate central finite difference 94 | // fdiff.block(0, i, 2, 1) = (e_fd - e_bd) / (2 * step_size); 95 | // } 96 | // 97 | // return check_jacobian("J_point", fdiff, J_point, threshold, true); 98 | // } 99 | 100 | // int check_J_cam_pose(const mat3_t &cam_K, 101 | // const mat4_t &T_WC, 102 | // const vec3_t &p_W, 103 | // const mat_t<2, 6> &J_cam_pose, 104 | // const double step_size = 1e-3, 105 | // const double threshold = 1e-2) { 106 | // const vec2_t z{0.0, 0.0}; 107 | // const vec4_t hp_W = p_W.homogeneous(); 108 | // 109 | // // Perturb rotation 110 | // matx_t fdiff = zeros(2, 6); 111 | // for (int i = 0; i < 3; i++) { 112 | // // Forward difference 113 | // const mat4_t T_WC_fd = tf_perturb_rot(T_WC, step_size, i); 114 | // const mat4_t T_CW_fd = T_WC_fd.inverse(); 115 | // const vec3_t p_C_fd = (T_CW_fd * hp_W).head(3); 116 | // const vec3_t x_fd{p_C_fd(0) / p_C_fd(2), p_C_fd(1) / p_C_fd(2), 1.0}; 117 | // const vec2_t z_fd = (cam_K * x_fd).head(2); 118 | // const vec2_t e_fd = z - z_fd; 119 | // 120 | // // Backward difference 121 | // const mat4_t T_WC_bd = tf_perturb_rot(T_WC, -step_size, i); 122 | // const mat4_t T_CW_bd = T_WC_bd.inverse(); 123 | // const vec3_t p_C_bd = (T_CW_bd * hp_W).head(3); 124 | // const vec3_t x_bd{p_C_bd(0) / p_C_bd(2), p_C_bd(1) / p_C_bd(2), 1.0}; 125 | // const vec2_t z_bd = (cam_K * x_bd).head(2); 126 | // const vec2_t e_bd = z - z_bd; 127 | // 128 | // fdiff.block(0, i, 2, 1) = (e_fd - e_bd) / (2 * step_size); 129 | // } 130 | // 131 | // // Perturb translation 132 | // for (int i = 0; i < 3; i++) { 133 | // // Forward difference 134 | // const mat4_t T_WC_fd = tf_perturb_trans(T_WC, step_size, i); 135 | // const mat4_t T_CW_fd = T_WC_fd.inverse(); 136 | // const vec3_t p_C_fd = (T_CW_fd * hp_W).head(3); 137 | // const vec3_t x_fd{p_C_fd(0) / p_C_fd(2), p_C_fd(1) / p_C_fd(2), 1.0}; 138 | // const vec2_t z_fd = (cam_K * x_fd).head(2); 139 | // const vec2_t e_fd = z - z_fd; 140 | // 141 | // // Backward difference 142 | // const mat4_t T_WC_bd = tf_perturb_trans(T_WC, -step_size, i); 143 | // const mat4_t T_CW_bd = T_WC_bd.inverse(); 144 | // const vec3_t p_C_bd = (T_CW_bd * hp_W).head(3); 145 | // const vec3_t x_bd{p_C_bd(0) / p_C_bd(2), p_C_bd(1) / p_C_bd(2), 1.0}; 146 | // const vec2_t z_bd = (cam_K * x_bd).head(2); 147 | // const vec2_t e_bd = z - z_bd; 148 | // 149 | // fdiff.block(0, i + 3, 2, 1) = (e_fd - e_bd) / (2 * step_size); 150 | // } 151 | // 152 | // return check_jacobian("J_cam_pose", fdiff, J_cam_pose, threshold, true); 153 | // } 154 | // 155 | // int check_J_point(const mat3_t &cam_K, 156 | // const mat4_t &T_WC, 157 | // const vec3_t &p_W, 158 | // const mat_t<2, 3> &J_point, 159 | // const double step_size = 1e-10, 160 | // const double threshold = 1e-2) { 161 | // const vec2_t z{0.0, 0.0}; 162 | // const mat4_t T_CW = T_WC.inverse(); 163 | // matx_t fdiff = zeros(2, 3); 164 | // mat3_t dr = I(3) * step_size; 165 | // 166 | // // Perturb landmark 167 | // for (int i = 0; i < 3; i++) { 168 | // // Forward difference 169 | // const vec3_t p_W_fd = p_W + dr.col(i); 170 | // const vec4_t hp_W_fd = p_W_fd.homogeneous(); 171 | // const vec3_t p_C_fd = (T_CW * hp_W_fd).head(3); 172 | // const vec3_t x_fd{p_C_fd(0) / p_C_fd(2), p_C_fd(1) / p_C_fd(2), 1.0}; 173 | // const vec2_t z_fd = (cam_K * x_fd).head(2); 174 | // const vec2_t e_fd = z - z_fd; 175 | // 176 | // // Backward difference 177 | // const vec3_t p_W_bd = p_W - dr.col(i); 178 | // const vec4_t hp_W_bd = p_W_bd.homogeneous(); 179 | // const vec3_t p_C_bd = (T_CW * hp_W_bd).head(3); 180 | // const vec3_t x_bd{p_C_bd(0) / p_C_bd(2), p_C_bd(1) / p_C_bd(2), 1.0}; 181 | // const vec2_t z_bd = (cam_K * x_bd).head(2); 182 | // const vec2_t e_bd = z - z_bd; 183 | // 184 | // fdiff.block(0, i, 2, 1) = (e_fd - e_bd) / (2 * step_size); 185 | // } 186 | // 187 | // return check_jacobian("J_point", fdiff, J_point, threshold, true); 188 | // } 189 | 190 | int test_parse_keypoints_line() { 191 | std::string line = "4,1,2,3,4\n"; 192 | keypoints_t keypoints = parse_keypoints_line(line.c_str()); 193 | 194 | keypoints_print(keypoints); 195 | MU_CHECK(keypoints.size() == 2); 196 | MU_CHECK(fltcmp(keypoints[0](0), 1.0) == 0); 197 | MU_CHECK(fltcmp(keypoints[0](1), 2.0) == 0); 198 | MU_CHECK(fltcmp(keypoints[1](0), 3.0) == 0); 199 | MU_CHECK(fltcmp(keypoints[1](1), 4.0) == 0); 200 | 201 | return 0; 202 | } 203 | 204 | int test_load_keypoints() { 205 | std::vector keypoints = load_keypoints(TEST_DATA); 206 | 207 | for (const auto &kp : keypoints) { 208 | MU_CHECK(kp.size() > 0); 209 | /* printf("frame[%d]\n", i); */ 210 | /* keypoints_print(keypoints[i]); */ 211 | } 212 | 213 | MU_CHECK(keypoints.size() == 20); 214 | 215 | return 0; 216 | } 217 | 218 | int test_ba_residuals() { 219 | ba_data_t data{TEST_DATA}; 220 | 221 | vecx_t r = ba_residuals(data); 222 | // for (int i = 0; i < r.rows(); i++) { 223 | // MU_CHECK(r[i] < 0.01); 224 | // } 225 | 226 | const double cost = ba_cost(r); 227 | printf("Cost: %e\n", cost); 228 | 229 | return 0; 230 | } 231 | 232 | int test_ba_jacobian() { 233 | ba_data_t data{TEST_DATA}; 234 | // for (const auto pose : data.cam_poses) { 235 | // std::cout << pose.q.w() << ", "; 236 | // std::cout << pose.q.x() << ", "; 237 | // std::cout << pose.q.y() << ", "; 238 | // std::cout << pose.q.z() << std::endl; 239 | // } 240 | 241 | matx_t J = ba_jacobian(data); 242 | mat2csv("/tmp/J1.csv", J); 243 | 244 | return 0; 245 | } 246 | 247 | int test_ba_update() { 248 | ba_data_t data{TEST_DATA}; 249 | const vecx_t e = ba_residuals(data); 250 | const matx_t E = ba_jacobian(data); 251 | printf("cost [before update]: %f\n", ba_cost(e)); 252 | ba_update(data, e, E); 253 | 254 | { 255 | const vecx_t e = ba_residuals(data); 256 | printf("cost [after update]: %f\n", ba_cost(e)); 257 | } 258 | 259 | return 0; 260 | } 261 | 262 | int test_ba_cost() { 263 | vecx_t e{5}; 264 | e << 1.0, 2.0, 3.0, 4.0, 5.0; 265 | const double cost = ba_cost(e); 266 | 267 | printf("Cost: %f\n", cost); 268 | MU_CHECK(fltcmp(cost, 27.50) == 0); 269 | 270 | return 0; 271 | } 272 | 273 | int test_ba_solve() { 274 | ba_data_t data{TEST_DATA}; 275 | struct timespec t_start = tic(); 276 | ba_solve(data); 277 | printf("time taken: %fs\n", toc(&t_start)); 278 | printf("nb_frames: %d\n", data.nb_frames); 279 | printf("nb_points: %d\n", data.nb_points); 280 | 281 | return 0; 282 | } 283 | 284 | void test_suite() { 285 | MU_ADD_TEST(test_parse_keypoints_line); 286 | MU_ADD_TEST(test_load_keypoints); 287 | MU_ADD_TEST(test_ba_residuals); 288 | MU_ADD_TEST(test_ba_jacobian); 289 | MU_ADD_TEST(test_ba_update); 290 | MU_ADD_TEST(test_ba_cost); 291 | MU_ADD_TEST(test_ba_solve); 292 | } 293 | 294 | MU_RUN_TESTS(test_suite); 295 | -------------------------------------------------------------------------------- /ba/cpp/ba.cpp: -------------------------------------------------------------------------------- 1 | #include "ba.hpp" 2 | 3 | static mat2_t J_intrinsics(const mat3_t &K) { 4 | // J = [K[0, 0], 0.0, 5 | // 0.0, K[1, 1]]; 6 | mat2_t J = zeros(2, 2); 7 | J(0, 0) = K(0, 0); 8 | J(1, 1) = K(1, 1); 9 | return J; 10 | } 11 | 12 | static matx_t J_project(const vec3_t &p_C) { 13 | const real_t x = p_C(0); 14 | const real_t y = p_C(1); 15 | const real_t z = p_C(2); 16 | 17 | // J = [1 / z, 0, -x / z^2, 18 | // 0, 1 / z, -y / z^2]; 19 | matx_t J = zeros(2, 3); 20 | J(0, 0) = 1.0 / z; 21 | J(1, 1) = 1.0 / z; 22 | J(0, 2) = -x / (z * z); 23 | J(1, 2) = -y / (z * z); 24 | return J; 25 | } 26 | 27 | static mat3_t J_camera_rotation(const quat_t &q_WC, 28 | const vec3_t &r_WC, 29 | const vec3_t &p_W) { 30 | const mat3_t C_WC = q_WC.toRotationMatrix(); 31 | const mat3_t C_CW = C_WC.transpose(); 32 | return C_CW * skew(p_W - r_WC); 33 | } 34 | 35 | static mat3_t J_camera_translation(const quat_t &q_WC) { 36 | const mat3_t C_WC = q_WC.toRotationMatrix(); 37 | const mat3_t C_CW = C_WC.transpose(); 38 | return -C_CW; 39 | } 40 | 41 | static mat3_t J_target_point(const quat_t &q_WC) { 42 | const mat3_t C_WC = q_WC.toRotationMatrix(); 43 | const mat3_t C_CW = C_WC.transpose(); 44 | return C_CW; 45 | } 46 | 47 | int ba_residual_size(ba_data_t &data) { 48 | // Calculate residual size 49 | int r_size = 0; 50 | for (int k = 0; k < data.nb_frames; k++) { 51 | r_size += data.point_ids[k][0]; 52 | } 53 | r_size = r_size * 2; 54 | // ^ Scale 2 because each pixel error are size 2 55 | 56 | return r_size; 57 | } 58 | 59 | vecx_t ba_residuals(ba_data_t &data) { 60 | // Initialize memory for residuals 61 | int r_size = ba_residual_size(data); 62 | vecx_t r{r_size}; 63 | 64 | // Loop over time 65 | int res_idx = 0; // Residual index 66 | for (int k = 0; k < data.nb_frames; k++) { 67 | // Form camera pose and its inverse 68 | const mat4_t T_WC = data.cam_poses[k].T(); 69 | const mat4_t T_CW = T_WC.inverse(); 70 | 71 | // Get point ids and measurements at time step k 72 | const int nb_ids = data.point_ids[k][0]; 73 | const int *point_ids = &data.point_ids[k][1]; 74 | 75 | for (int i = 0; i < nb_ids; i++) { 76 | // Get point in world frame and transform to camera frame 77 | const int id = point_ids[i]; 78 | const vec3_t p_W{data.points[id]}; 79 | const vec3_t p_C = tf_point(T_CW, p_W); 80 | 81 | // Project point in camera frame down to image plane 82 | const vec3_t x{p_C(0) / p_C(2), p_C(1) / p_C(2), 1.0}; 83 | const vec2_t z_hat = (data.cam_K * x).head(2); 84 | 85 | // Calculate reprojection error 86 | const vec2_t z = data.keypoints[k][i]; 87 | r.block(res_idx, 0, 2, 1) = z - z_hat; 88 | res_idx += 2; 89 | } 90 | } 91 | 92 | return r; 93 | } 94 | 95 | matx_t ba_jacobian(ba_data_t &data) { 96 | // Initialize memory for jacobian 97 | int J_rows = ba_residual_size(data); 98 | int J_cols = (data.nb_frames * 6) + (data.nb_points * 3); 99 | matx_t J = zeros(J_rows, J_cols); 100 | 101 | // Loop over camera poses 102 | int pose_idx = 0; 103 | int meas_idx = 0; 104 | 105 | for (int k = 0; k < data.nb_frames; k++) { 106 | // Form camera pose 107 | const mat4_t T_WC = data.cam_poses[k].T(); 108 | const quat_t q_WC = tf_quat(T_WC); 109 | const vec3_t r_WC = tf_trans(T_WC); 110 | const mat4_t T_CW = T_WC.inverse(); 111 | 112 | // Get point ids and measurements at time step k 113 | const int nb_ids = data.point_ids[k][0]; 114 | const int *point_ids = &data.point_ids[k][1]; 115 | 116 | // Loop over observations at time k 117 | for (int i = 0; i < nb_ids; i++) { 118 | // Get point in world frame and transform to camera frame 119 | const int id = point_ids[i]; 120 | const vec3_t p_W{data.points[id]}; 121 | const vec3_t p_C = tf_point(T_CW, p_W); 122 | 123 | // Camera pose jacobian 124 | const int rs = meas_idx * 2; 125 | int cs = pose_idx * 6; 126 | const mat2_t J_K = J_intrinsics(data.cam_K); 127 | const matx_t J_P = J_project(p_C); 128 | const matx_t J_h = J_K * J_P; 129 | const mat3_t J_C = J_camera_rotation(q_WC, r_WC, p_W); 130 | const mat3_t J_r = J_camera_translation(q_WC); 131 | const matx_t J_cam_rot = -1 * J_h * J_C; 132 | const matx_t J_cam_pos = -1 * J_h * J_r; 133 | J.block(rs, cs, 2, 3) = J_cam_rot; 134 | J.block(rs, cs + 3, 2, 3) = J_cam_pos; 135 | 136 | // Point jacobian 137 | cs = (data.nb_frames * 6) + point_ids[i] * 3; 138 | const matx_t J_point = -1 * J_h * J_target_point(q_WC); 139 | J.block(rs, cs, 2, 3) = J_point; 140 | 141 | meas_idx++; 142 | } 143 | pose_idx++; 144 | } 145 | 146 | return J; 147 | } 148 | 149 | void ba_update(ba_data_t &data, const vecx_t &e, const matx_t &E) { 150 | const real_t lambda = 1e-4; // LM damping term 151 | 152 | // Solve Gauss-Newton system [H dx = g]: Solve for dx 153 | matx_t H = E.transpose() * E; // Hessian approx: H = J^t J 154 | matx_t H_diag = (H.diagonal().asDiagonal()); 155 | H = H + lambda * H_diag; // R. Fletcher trust region mod 156 | const vecx_t g = -E.transpose() * e; 157 | const vecx_t dx = H.ldlt().solve(g); // Cholesky decomp 158 | 159 | // Update camera poses 160 | for (int k = 0; k < data.nb_frames; k++) { 161 | const int s = k * 6; 162 | 163 | // Update camera rotation 164 | const vec3_t dalpha{dx(s), dx(s + 1), dx(s + 2)}; 165 | const quat_t q = data.cam_poses[k].rot(); 166 | const quat_t dq = quat_delta(dalpha); 167 | data.cam_poses[k].set_rot(dq * q); 168 | 169 | // Update camera position 170 | const vec3_t r_WC = data.cam_poses[k].trans(); 171 | const vec3_t dr_WC{dx(s + 3), dx(s + 4), dx(s + 5)}; 172 | data.cam_poses[k].set_trans(r_WC + dr_WC); 173 | } 174 | 175 | // Update points 176 | for (int i = 0; i < data.nb_points; i++) { 177 | const int s = (data.nb_frames * 6) + (i * 3); 178 | const vec3_t dp_W{dx(s), dx(s + 1), dx(s + 2)}; 179 | data.points[i][0] += dp_W(0); 180 | data.points[i][1] += dp_W(1); 181 | data.points[i][2] += dp_W(2); 182 | } 183 | } 184 | 185 | real_t ba_cost(const vecx_t &e) { 186 | return 0.5 * e.transpose() * e; 187 | } 188 | 189 | void ba_solve(ba_data_t &data) { 190 | int max_iter = 10; 191 | real_t cost_prev = 0.0; 192 | 193 | for (int iter = 0; iter < max_iter; iter++) { 194 | // Update 195 | struct timespec t_start = tic(); 196 | const vecx_t e = ba_residuals(data); 197 | const matx_t E = ba_jacobian(data); 198 | ba_update(data, e, E); 199 | 200 | // Calculate reprojection error 201 | double sse = 0.0; 202 | int N = 0; 203 | for (int i = 0; i < e.size(); i += 2) { 204 | const real_t dx = e(i); 205 | const real_t dy = e(i + 1); 206 | const real_t reproj_error = sqrt(dx * dx + dy * dy); 207 | sse += reproj_error * reproj_error; 208 | N++; 209 | } 210 | const double rmse_reproj_error = sqrt(sse / N); 211 | 212 | // Print cost 213 | const real_t cost = ba_cost(e); 214 | printf(" - iter[%d] ", iter); 215 | printf("cost: %.2e ", cost); 216 | printf("time: %.3fs ", toc(&t_start)); 217 | printf("rmse_reproj_error: %.2fpx\n", rmse_reproj_error); 218 | 219 | // Termination criteria 220 | real_t cost_diff = fabs(cost - cost_prev); 221 | if (cost_diff < 1.0e-1) { 222 | printf("Done!\n"); 223 | break; 224 | } 225 | cost_prev = cost; 226 | } 227 | } 228 | 229 | void ba_save(const ba_data_t &data, std::string &save_dir) { 230 | // Create save directory 231 | const char last_char = save_dir[save_dir.length() - 1]; 232 | const std::string postfix = (last_char == '/') ? "" : "/"; 233 | save_dir += postfix; 234 | const std::string cmd = "mkdir -p " + save_dir; 235 | const int retval = system(cmd.c_str()); 236 | if (retval != 0) { 237 | printf("Error! Failed to save results to [%s]", save_dir.c_str()); 238 | } 239 | 240 | // Save camera matrix 241 | { 242 | const std::string csv_path = save_dir + "camera.csv"; 243 | FILE *camera_csv = fopen(csv_path.c_str(), "w"); 244 | fprintf(camera_csv, "#camera_K\n"); 245 | fprintf(camera_csv, "%f, 0.0000, %f\n", data.cam_K(0, 0), data.cam_K(0, 2)); 246 | fprintf(camera_csv, "0.0000, %f, %f\n", data.cam_K(1, 1), data.cam_K(1, 2)); 247 | fprintf(camera_csv, "0.0000, 0.0000, 1.0000\n"); 248 | fclose(camera_csv); 249 | } 250 | 251 | // Save camera poses 252 | { 253 | const std::string csv_path = save_dir + "camera_poses.csv"; 254 | FILE *poses_csv = fopen(csv_path.c_str(), "w"); 255 | fprintf(poses_csv, "#qw,qx,qy,qz,rx,ry,rz\n"); 256 | for (const auto &pose : data.cam_poses) { 257 | const auto &q = pose.rot(); 258 | const auto &r = pose.trans(); 259 | fprintf(poses_csv, "%f,%f,%f,%f,", q.w(), q.x(), q.y(), q.z()); 260 | fprintf(poses_csv, "%f,%f,%f", r(0), r(1), r(2)); 261 | fprintf(poses_csv, "\n"); 262 | } 263 | fclose(poses_csv); 264 | } 265 | 266 | // Save target pose 267 | { 268 | const std::string csv_path = save_dir + "target_pose.csv"; 269 | FILE *target_csv = fopen(csv_path.c_str(), "w"); 270 | fprintf(target_csv, "#qw,qx,qy,qz,rx,ry,rz\n"); 271 | { 272 | const auto &q = data.target_pose.rot(); 273 | const auto &r = data.target_pose.trans(); 274 | fprintf(target_csv, "%f,%f,%f,%f,", q.w(), q.x(), q.y(), q.z()); 275 | fprintf(target_csv, "%f,%f,%f", r(0), r(1), r(2)); 276 | fprintf(target_csv, "\n"); 277 | } 278 | fclose(target_csv); 279 | } 280 | 281 | // Save keypoints 282 | { 283 | const std::string csv_path = save_dir + "keypoints.csv"; 284 | FILE *keypoints_csv = fopen(csv_path.c_str(), "w"); 285 | fprintf(keypoints_csv, "#size,keypoints\n"); 286 | for (const auto &kps : data.keypoints) { 287 | int nb_kps = kps.size(); 288 | fprintf(keypoints_csv, "%d,", nb_kps * 2); 289 | for (int j = 0; j < nb_kps; j++) { 290 | fprintf(keypoints_csv, "%f,", kps[j](0)); 291 | fprintf(keypoints_csv, "%f", kps[j](1)); 292 | if (j + 1 < nb_kps) { 293 | fprintf(keypoints_csv, ","); 294 | } 295 | } 296 | fprintf(keypoints_csv, "\n"); 297 | } 298 | fclose(keypoints_csv); 299 | } 300 | 301 | // Save point ids 302 | { 303 | const std::string csv_path = save_dir + "point_ids.csv"; 304 | FILE *point_ids_csv = fopen(csv_path.c_str(), "w"); 305 | fprintf(point_ids_csv, "#size,point_ids\n"); 306 | for (int i = 0; i < data.nb_ids; i++) { 307 | int nb_ids = data.point_ids[i][0]; 308 | fprintf(point_ids_csv, "%d,", data.point_ids[i][0]); 309 | for (int j = 1; j < (nb_ids + 1); j++) { 310 | fprintf(point_ids_csv, "%d", data.point_ids[i][j]); 311 | if (j + 1 < (nb_ids + 1)) { 312 | fprintf(point_ids_csv, ","); 313 | } 314 | } 315 | fprintf(point_ids_csv, "\n"); 316 | } 317 | fclose(point_ids_csv); 318 | } 319 | 320 | // Save points 321 | { 322 | const std::string csv_path = save_dir + "points.csv"; 323 | FILE *points_csv = fopen(csv_path.c_str(), "w"); 324 | fprintf(points_csv, "#x,y,z\n"); 325 | for (int i = 0; i < data.nb_points; i++) { 326 | fprintf(points_csv, "%f,", data.points[i][0]); 327 | fprintf(points_csv, "%f,", data.points[i][1]); 328 | fprintf(points_csv, "%f\n", data.points[i][2]); 329 | } 330 | fclose(points_csv); 331 | } 332 | } 333 | -------------------------------------------------------------------------------- /ba/c/test_ba.c: -------------------------------------------------------------------------------- 1 | #include "munit.h" 2 | #include "ba.h" 3 | 4 | #define TEST_BA_DATA_GT "./data_gnd" 5 | #define TEST_BA_DATA "./data_noisy" 6 | #define TEST_BA_JAC "./ba_jacobian.csv" 7 | #define TEST_BA_UPDATE_DX "./ba_update_dx.csv" 8 | 9 | #define STEP_SIZE 1e-8 10 | #define THRESHOLD 1e-3 11 | 12 | static void 13 | project(const real_t *cam_params, 14 | const real_t *T_WC, 15 | const real_t *p_W, 16 | real_t *z) { 17 | /* Invert camera pose to T_CW */ 18 | real_t T_CW[4 * 4] = {0}; 19 | tf_inv(T_WC, T_CW); 20 | 21 | /* Transform point from world frame to camera frame */ 22 | real_t p_C[3] = {0}; 23 | tf_point(T_CW, p_W, p_C); 24 | 25 | /* Project */ 26 | const real_t x = p_C[0] / p_C[2]; 27 | const real_t y = p_C[1] / p_C[2]; 28 | 29 | /* Scale and center */ 30 | const real_t fx = cam_params[0]; 31 | const real_t fy = cam_params[1]; 32 | const real_t cx = cam_params[2]; 33 | const real_t cy = cam_params[3]; 34 | z[0] = fx * x + cx; 35 | z[1] = fy * y + cy; 36 | } 37 | 38 | static int check_J_cam_pose(const real_t *cam_params, 39 | const real_t *T_WC, 40 | const real_t *p_W, 41 | const real_t *J_cam_pose) { 42 | #if PRECISION == 1 43 | const real_t step_size = 1e-4; 44 | const real_t threshold = 1e-1; 45 | #else 46 | const real_t step_size = 1e-8; 47 | const real_t threshold = 1e-4; 48 | #endif 49 | const real_t z[2] = {0.0, 0.0}; 50 | real_t fdiff[2 * 6] = {0.0}; 51 | 52 | // Perturb rotation 53 | for (int i = 0; i < 3; i++) { 54 | // Forward difference 55 | // -- Perturb rotation 56 | real_t T_WC_fd[4 * 4] = {0}; 57 | mat_copy(T_WC, 4, 4, T_WC_fd); 58 | tf_perturb_rot(T_WC_fd, step_size, i); 59 | // -- Project landmark to image plane 60 | real_t z_fd[2] = {0}; 61 | project(cam_params, T_WC_fd, p_W, z_fd); 62 | // -- Calculate reprojection error 63 | real_t e_fd[2] = {0}; 64 | e_fd[0] = z[0] - z_fd[0]; 65 | e_fd[1] = z[1] - z_fd[1]; 66 | 67 | // Backward difference 68 | // -- Perturb rotation 69 | real_t T_WC_bd[4 * 4] = {0}; 70 | mat_copy(T_WC, 4, 4, T_WC_bd); 71 | tf_perturb_rot(T_WC_bd, -step_size, i); 72 | // -- Project landmark to image plane 73 | real_t z_bd[2] = {0}; 74 | project(cam_params, T_WC_bd, p_W, z_bd); 75 | // -- Calculate reprojection error 76 | real_t e_bd[2] = {0}; 77 | e_bd[0] = z[0] - z_bd[0]; 78 | e_bd[1] = z[1] - z_bd[1]; 79 | 80 | // Set finite difference 81 | fdiff[i] = (e_fd[0] - e_bd[0]) / (2 * step_size); 82 | fdiff[i + 6] = (e_fd[1] - e_bd[1]) / (2 * step_size); 83 | } 84 | 85 | // Perturb translation 86 | for (int i = 0; i < 3; i++) { 87 | // Forward difference 88 | // -- Perturb translation 89 | real_t T_WC_fd[4 * 4] = {0}; 90 | mat_copy(T_WC, 4, 4, T_WC_fd); 91 | tf_perturb_trans(T_WC_fd, step_size, i); 92 | // -- Project landmark to image plane 93 | real_t z_fd[2] = {0}; 94 | project(cam_params, T_WC_fd, p_W, z_fd); 95 | // -- Calculate reprojection error 96 | real_t e_fd[2] = {z[0] - z_fd[0], z[1] - z_fd[1]}; 97 | 98 | // Backward difference 99 | // -- Perturb translation 100 | real_t T_WC_bd[4 * 4] = {0}; 101 | mat_copy(T_WC, 4, 4, T_WC_bd); 102 | tf_perturb_trans(T_WC_bd, -step_size, i); 103 | // -- Project landmark to image plane 104 | real_t z_bd[2] = {0}; 105 | project(cam_params, T_WC_bd, p_W, z_bd); 106 | // -- Calculate reprojection error 107 | real_t e_bd[2] = {z[0] - z_bd[0], z[1] - z_bd[1]}; 108 | 109 | // Set finite difference 110 | fdiff[i + 3] = (e_fd[0] - e_bd[0]) / (2 * step_size); 111 | fdiff[i + 9] = (e_fd[1] - e_bd[1]) / (2 * step_size); 112 | } 113 | 114 | return check_jacobian("J_cam_pose", fdiff, J_cam_pose, 2, 6, threshold, 1); 115 | } 116 | 117 | static int check_J_landmark(const real_t *cam_params, 118 | const real_t *T_WC, 119 | const real_t *p_W, 120 | const real_t *J_landmark) { 121 | #if PRECISION == 1 122 | const real_t step_size = 1e-4; 123 | const real_t threshold = 1e-1; 124 | #else 125 | const real_t step_size = 1e-8; 126 | const real_t threshold = 1e-2; 127 | #endif 128 | const real_t z[2] = {0.0, 0.0}; 129 | real_t fdiff[2 * 6] = {0.0}; 130 | 131 | // Perturb landmark 132 | for (int i = 0; i < 3; i++) { 133 | // Forward difference 134 | // -- Perturb landmark 135 | real_t p_W_fd[3] = {p_W[0], p_W[1], p_W[2]}; 136 | p_W_fd[i] += step_size; 137 | // -- Project landmark to image plane 138 | real_t z_fd[2] = {0}; 139 | project(cam_params, T_WC, p_W_fd, z_fd); 140 | // -- Calculate reprojection error 141 | real_t e_fd[2] = {z[0] - z_fd[0], z[1] - z_fd[1]}; 142 | 143 | // Backward difference 144 | // -- Perturb landmark 145 | real_t p_W_bd[3] = {p_W[0], p_W[1], p_W[2]}; 146 | p_W_bd[i] -= step_size; 147 | // -- Project landmark to image plane 148 | real_t z_bd[2] = {0}; 149 | project(cam_params, T_WC, p_W_bd, z_bd); 150 | // -- Calculate reprojection error 151 | real_t e_bd[2] = {z[0] - z_bd[0], z[1] - z_bd[1]}; 152 | 153 | // Set finite difference 154 | fdiff[i] = (e_fd[0] - e_bd[0]) / (2 * step_size); 155 | fdiff[i + 3] = (e_fd[1] - e_bd[1]) / (2 * step_size); 156 | } 157 | 158 | return check_jacobian("J_landmark", fdiff, J_landmark, 2, 3, threshold, 1); 159 | } 160 | 161 | int test_load_camera() { 162 | real_t K[3 * 3] = {0}; 163 | load_camera(TEST_BA_DATA, K); 164 | print_matrix("K", K, 3, 3); 165 | 166 | return 0; 167 | } 168 | 169 | int test_parse_keypoints_line() { 170 | keypoints_t *keypoints = parse_keypoints_line("4,1,2,3,4\n"); 171 | 172 | /* keypoints_print(keypoints); */ 173 | MU_CHECK(keypoints->size == 2); 174 | MU_CHECK(fltcmp(keypoints->data[0][0], 1.0) == 0); 175 | MU_CHECK(fltcmp(keypoints->data[0][1], 2.0) == 0); 176 | MU_CHECK(fltcmp(keypoints->data[1][0], 3.0) == 0); 177 | MU_CHECK(fltcmp(keypoints->data[1][1], 4.0) == 0); 178 | 179 | keypoints_free(keypoints); 180 | 181 | return 0; 182 | } 183 | 184 | int test_load_keypoints() { 185 | int nb_frames = 0; 186 | keypoints_t **keypoints = load_keypoints(TEST_BA_DATA, &nb_frames); 187 | 188 | for (int i = 0; i < nb_frames; i++) { 189 | printf("frame: %d ", i); 190 | printf("nb_keypoints: %d\n", keypoints[i]->size); 191 | MU_CHECK(keypoints[i] != NULL); 192 | MU_CHECK(keypoints[i]->size > 0); 193 | /* printf("frame[%d]\n", i); */ 194 | /* keypoints_print(keypoints[i]); */ 195 | keypoints_free(keypoints[i]); 196 | } 197 | free(keypoints); 198 | 199 | return 0; 200 | } 201 | 202 | int test_ba_load_data() { 203 | ba_data_t *data = ba_load_data(TEST_BA_DATA); 204 | ba_data_free(data); 205 | return 0; 206 | } 207 | 208 | int test_ba_residuals() { 209 | ba_data_t *data = ba_load_data(TEST_BA_DATA); 210 | 211 | int r_size = 0; 212 | real_t *r = ba_residuals(data, &r_size); 213 | /* for (int i = 0; i < r_size; i++) { */ 214 | /* MU_CHECK(r[i] < 0.01); */ 215 | /* } */ 216 | 217 | const real_t cost = ba_cost(r, r_size); 218 | printf("Cost: %e\n", cost); 219 | 220 | ba_data_free(data); 221 | free(r); 222 | return 0; 223 | } 224 | 225 | int test_J_cam_pose() { 226 | /* Setup camera intrinsics */ 227 | real_t cam_params[4] = {640.0, 320.0, 480.0, 240.0}; 228 | 229 | /* Setup camera pose */ 230 | /* -- Rotation -- */ 231 | const real_t roll = deg2rad(-90.0); 232 | const real_t pitch = deg2rad(0.0); 233 | const real_t yaw = deg2rad(-90.0); 234 | const real_t rpy[3] = {roll, pitch, yaw}; 235 | real_t C_WC[3 * 3] = {0}; 236 | real_t q_WC[4] = {0}; 237 | euler321(rpy, C_WC); 238 | rot2quat(C_WC, q_WC); 239 | /* -- Translation -- */ 240 | real_t r_WC[3] = {0.1, 0.2, 0.3}; 241 | /* -- Transform -- */ 242 | real_t T_WC[4 * 4] = {0}; 243 | tf_rot_set(T_WC, C_WC); 244 | tf_trans_set(T_WC, r_WC); 245 | mat_set(T_WC, 4, 3, 3, 1.0); 246 | 247 | /* Landmark in world frame */ 248 | real_t p_W[3] = {10.0, 0.0, 0.0}; 249 | 250 | /* Transform point in world frame to camera frame */ 251 | real_t T_CW[4 * 4] = {0}; 252 | real_t p_C[3] = {0}; 253 | tf_inv(T_WC, T_CW); 254 | tf_point(T_CW, p_W, p_C); 255 | 256 | /* -- Form jacobians */ 257 | real_t J_K[2 * 2] = {0}; 258 | real_t J_P[2 * 3] = {0}; 259 | real_t J_h[2 * 3] = {0}; 260 | J_intrinsics_point(cam_params, J_K); 261 | J_project(p_C, J_P); 262 | dot(J_K, 2, 2, J_P, 2, 3, J_h); 263 | 264 | /* J_cam_rot = -1 * J_K * J_P * J_C; */ 265 | real_t J_C[3 * 3] = {0}; 266 | real_t J_cam_rot[2 * 3] = {0}; 267 | J_camera_rotation(q_WC, r_WC, p_W, J_C); 268 | dot(J_h, 2, 3, J_C, 3, 3, J_cam_rot); 269 | mat_scale(J_cam_rot, 2, 3, -1); 270 | 271 | /* J_cam_pos = -1 * J_K * J_P * J_r; */ 272 | real_t J_r[3 * 3] = {0}; 273 | real_t J_cam_pos[2 * 3] = {0}; 274 | J_camera_translation(q_WC, J_r); 275 | dot(J_h, 2, 3, J_r, 3, 3, J_cam_pos); 276 | mat_scale(J_cam_pos, 2, 3, -1); 277 | 278 | /* Form J_cam_pose */ 279 | real_t J_cam_pose[2 * 6] = {0}; 280 | mat_block_set(J_cam_pose, 6, 0, 0, 1, 2, J_cam_rot); 281 | mat_block_set(J_cam_pose, 6, 0, 3, 1, 5, J_cam_pos); 282 | 283 | /* Check jacobians */ 284 | int retval = check_J_cam_pose(cam_params, T_WC, p_W, J_cam_pose); 285 | MU_CHECK(retval == 0); 286 | 287 | return 0; 288 | } 289 | 290 | int test_J_landmark() { 291 | /* Setup camera intrinsics */ 292 | real_t cam_params[4] = {640.0, 320.0, 480.0, 240.0}; 293 | 294 | /* Setup camera pose */ 295 | /* -- Rotation -- */ 296 | const real_t roll = deg2rad(-90.0); 297 | const real_t pitch = deg2rad(0.0); 298 | const real_t yaw = deg2rad(-90.0); 299 | const real_t rpy[3] = {roll, pitch, yaw}; 300 | real_t C_WC[3 * 3] = {0}; 301 | real_t q_WC[4] = {0}; 302 | euler321(rpy, C_WC); 303 | rot2quat(C_WC, q_WC); 304 | /* -- Translation -- */ 305 | real_t r_WC[3] = {0.1, 0.2, 0.3}; 306 | /* -- Transform -- */ 307 | real_t T_WC[4 * 4] = {0}; 308 | tf_rot_set(T_WC, C_WC); 309 | tf_trans_set(T_WC, r_WC); 310 | mat_set(T_WC, 4, 3, 3, 1.0); 311 | 312 | /* Landmark in world frame */ 313 | real_t p_W[3] = {10.0, 0.0, 0.0}; 314 | 315 | /* Transform point in world frame to camera frame */ 316 | real_t T_CW[4 * 4] = {0}; 317 | real_t p_C[3] = {0}; 318 | tf_inv(T_WC, T_CW); 319 | tf_point(T_CW, p_W, p_C); 320 | 321 | /* -- Form jacobians */ 322 | real_t J_K[2 * 2] = {0}; 323 | real_t J_P[2 * 3] = {0}; 324 | real_t J_KP[2 * 3] = {0}; 325 | real_t J_L[3 * 3] = {0}; 326 | real_t J_landmark[2 * 3] = {0}; 327 | J_intrinsics_point(cam_params, J_K); 328 | J_project(p_C, J_P); 329 | J_target_point(q_WC, J_L); 330 | dot(J_K, 2, 2, J_P, 2, 3, J_KP); 331 | dot(J_KP, 2, 3, J_L, 3, 3, J_landmark); 332 | mat_scale(J_landmark, 2, 3, -1); 333 | 334 | /* Check jacobians */ 335 | int retval = check_J_landmark(cam_params, T_WC, p_W, J_landmark); 336 | MU_CHECK(retval == 0); 337 | 338 | return 0; 339 | } 340 | 341 | int test_ba_cost() { 342 | const real_t e[5] = {1.0, 2.0, 3.0, 4.0, 5.0}; 343 | const real_t cost = ba_cost(e, 5); 344 | 345 | printf("Cost: %f\n", cost); 346 | MU_CHECK(fltcmp(cost, 27.50) == 0); 347 | 348 | return 0; 349 | } 350 | 351 | int test_ba_solve() { 352 | ba_data_t *data = ba_load_data(TEST_BA_DATA); 353 | struct timespec t_start = tic(); 354 | ba_solve(data); 355 | printf("time taken: %f\n", toc(&t_start)); 356 | ba_data_free(data); 357 | 358 | return 0; 359 | } 360 | 361 | void test_suite() { 362 | MU_ADD_TEST(test_load_camera); 363 | MU_ADD_TEST(test_parse_keypoints_line); 364 | MU_ADD_TEST(test_load_keypoints); 365 | MU_ADD_TEST(test_ba_load_data); 366 | MU_ADD_TEST(test_ba_residuals); 367 | MU_ADD_TEST(test_J_cam_pose); 368 | MU_ADD_TEST(test_J_landmark); 369 | MU_ADD_TEST(test_ba_cost); 370 | MU_ADD_TEST(test_ba_solve); 371 | } 372 | 373 | MU_RUN_TESTS(test_suite) 374 | -------------------------------------------------------------------------------- /ba/c/util.h: -------------------------------------------------------------------------------- 1 | #ifndef UTIL_H 2 | #define UTIL_H 3 | 4 | #define PRECISION 2 5 | #define MAX_LINE_LENGTH 9046 6 | #define USE_CBLAS 7 | #define USE_LAPACK 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #ifdef USE_CBLAS 19 | #include 20 | #endif 21 | 22 | /****************************************************************************** 23 | * LOGGING 24 | ******************************************************************************/ 25 | 26 | /* DEBUG */ 27 | #ifdef NDEBUG 28 | #define DEBUG(M, ...) 29 | #else 30 | #define DEBUG(M, ...) \ 31 | fprintf(stderr, "[DEBUG] %s:%d: " M "\n", __func__, __LINE__, ##__VA_ARGS__) 32 | #endif 33 | 34 | /* LOG */ 35 | #define LOG_ERROR(M, ...) \ 36 | fprintf(stderr, "[ERROR] [%s] " M "\n", __func__, ##__VA_ARGS__) 37 | #define LOG_WARN(M, ...) fprintf(stderr, "[WARN] " M "\n", ##__VA_ARGS__) 38 | #define LOG_INFO(M, ...) fprintf(stderr, "[INFO] " M "\n", ##__VA_ARGS__) 39 | 40 | /* FATAL */ 41 | #define FATAL(M, ...) \ 42 | fprintf(stderr, "[FATAL] " M "\n", ##__VA_ARGS__); \ 43 | exit(-1); 44 | 45 | /* CHECK */ 46 | #define CHECK(A, M, ...) \ 47 | if (!(A)) { \ 48 | log_err(M, ##__VA_ARGS__); \ 49 | goto error; \ 50 | } 51 | 52 | /****************************************************************************** 53 | * DATA 54 | ******************************************************************************/ 55 | 56 | #if PRECISION == 1 57 | typedef float real_t; 58 | #elif PRECISION == 2 59 | typedef double real_t; 60 | #else 61 | #error "Precision not defined!" 62 | #endif 63 | 64 | char *malloc_string(const char *s); 65 | int dsv_rows(const char *fp); 66 | int dsv_cols(const char *fp, const char delim); 67 | char **dsv_fields(const char *fp, const char delim, int *nb_fields); 68 | real_t **dsv_data(const char *fp, const char delim, int *nb_rows, int *nb_cols); 69 | real_t **csv_data(const char *fp, int *nb_rows, int *nb_cols); 70 | int **load_iarrays(const char *csv_path, int *nb_arrays); 71 | real_t **load_darrays(const char *csv_path, int *nb_arrays); 72 | real_t *load_vector(const char *file_path); 73 | 74 | /****************************************************************************** 75 | * MATHS 76 | ******************************************************************************/ 77 | 78 | #define UNUSED(expr) \ 79 | do { \ 80 | (void)(expr); \ 81 | } while (0) 82 | 83 | #ifndef M_PI 84 | #define M_PI (3.14159265358979323846) 85 | #endif 86 | 87 | #define MIN(x, y) ((x) < (y) ? (x) : (y)) 88 | #define MAX(x, y) ((x) > (y) ? (x) : (y)) 89 | #define SIGN(a, b) ((b) >= 0.0 ? fabs(a) : -fabs(a)) 90 | 91 | float randf(float a, float b); 92 | real_t deg2rad(const real_t d); 93 | real_t rad2deg(const real_t r); 94 | int fltcmp(const real_t x, const real_t y); 95 | real_t pythag(const real_t a, const real_t b); 96 | real_t lerp(const real_t a, const real_t b, const real_t t); 97 | void lerp3(const real_t *a, const real_t *b, const real_t t, real_t *x); 98 | real_t sinc(const real_t x); 99 | 100 | /****************************************************************************** 101 | * LINEAR ALGEBRA 102 | ******************************************************************************/ 103 | 104 | void print_matrix(const char *prefix, const real_t *data, const size_t m, 105 | const size_t n); 106 | void print_vector(const char *prefix, const real_t *data, const size_t length); 107 | 108 | void eye(real_t *A, const size_t m, const size_t n); 109 | void ones(real_t *A, const size_t m, const size_t n); 110 | void zeros(real_t *A, const size_t m, const size_t n); 111 | 112 | real_t *mat_new(const size_t m, const size_t n); 113 | int mat_cmp(const real_t *A, const real_t *B, const size_t m, const size_t n); 114 | int mat_equals(const real_t *A, const real_t *B, const size_t m, const size_t n, 115 | const real_t tol); 116 | int mat_save(const char *save_path, const real_t *A, const int m, const int n); 117 | real_t *mat_load(const char *save_path, int *nb_rows, int *nb_cols); 118 | void mat_set(real_t *A, const size_t stride, const size_t i, const size_t j, 119 | const real_t val); 120 | real_t mat_val(const real_t *A, const size_t stride, const size_t i, 121 | const size_t j); 122 | void mat_copy(const real_t *src, const int m, const int n, real_t *dest); 123 | void mat_block_get(const real_t *A, const size_t stride, const size_t rs, 124 | const size_t cs, const size_t re, const size_t ce, 125 | real_t *block); 126 | void mat_block_set(real_t *A, const size_t stride, const size_t rs, 127 | const size_t cs, const size_t re, const size_t ce, 128 | const real_t *block); 129 | void mat_diag_get(const real_t *A, const int m, const int n, real_t *d); 130 | void mat_diag_set(real_t *A, const int m, const int n, const real_t *d); 131 | void mat_triu(const real_t *A, const size_t n, real_t *U); 132 | void mat_tril(const real_t *A, const size_t n, real_t *L); 133 | real_t mat_trace(const real_t *A, const size_t m, const size_t n); 134 | void mat_transpose(const real_t *A, size_t m, size_t n, real_t *A_t); 135 | void mat_add(const real_t *A, const real_t *B, real_t *C, size_t m, size_t n); 136 | void mat_sub(const real_t *A, const real_t *B, real_t *C, size_t m, size_t n); 137 | void mat_scale(real_t *A, const size_t m, const size_t n, const real_t scale); 138 | 139 | real_t *vec_new(const size_t length); 140 | void vec_copy(const real_t *src, const size_t length, real_t *dest); 141 | int vec_equals(const real_t *x, const real_t *y, const size_t length); 142 | void vec_add(const real_t *x, const real_t *y, real_t *z, size_t length); 143 | void vec_sub(const real_t *x, const real_t *y, real_t *z, size_t length); 144 | void vec_scale(real_t *x, const size_t length, const real_t scale); 145 | real_t vec_norm(const real_t *x, const size_t length); 146 | 147 | void dot(const real_t *A, const size_t A_m, const size_t A_n, const real_t *B, 148 | const size_t B_m, const size_t B_n, real_t *C); 149 | void skew(const real_t x[3], real_t A[3 * 3]); 150 | void fwdsubs(const real_t *L, const real_t *b, real_t *y, const size_t n); 151 | void bwdsubs(const real_t *U, const real_t *y, real_t *x, const size_t n); 152 | int check_jacobian(const char *jac_name, const real_t *fdiff, const real_t *jac, 153 | const size_t m, const size_t n, const real_t tol, 154 | const int print); 155 | 156 | #ifdef USE_CBLAS 157 | void cblas_dot(const real_t *A, const size_t A_m, const size_t A_n, 158 | const real_t *B, const size_t B_m, const size_t B_n, real_t *C); 159 | #endif 160 | 161 | /****************************************************************************** 162 | * SVD 163 | ******************************************************************************/ 164 | 165 | int svd(real_t *A, int m, int n, real_t *U, real_t *s, real_t *V_t); 166 | int svdcomp(real_t *A, int m, int n, real_t *w, real_t *V); 167 | int pinv(real_t *A, const int m, const int n, real_t *A_inv); 168 | 169 | #ifdef USE_LAPACK 170 | 171 | #endif 172 | 173 | /****************************************************************************** 174 | * CHOL 175 | ******************************************************************************/ 176 | 177 | void chol(const real_t *A, const size_t n, real_t *L); 178 | void chol_solve(const real_t *A, const real_t *b, real_t *x, const size_t n); 179 | 180 | #ifdef USE_LAPACK 181 | void lapack_chol_solve(const real_t *A, const real_t *b, real_t *x, 182 | const size_t n); 183 | #endif 184 | 185 | /****************************************************************************** 186 | * TIME 187 | ******************************************************************************/ 188 | 189 | typedef uint64_t timestamp_t; 190 | 191 | struct timespec tic(); 192 | float toc(struct timespec *tic); 193 | float mtoc(struct timespec *tic); 194 | float time_now(); 195 | 196 | /****************************************************************************** 197 | * TRANSFORMS 198 | ******************************************************************************/ 199 | 200 | void tf(const real_t params[7], real_t T[4 * 4]); 201 | void tf_params(const real_t T[4 * 4], real_t params[7]); 202 | void tf_rot_set(real_t T[4 * 4], const real_t C[3 * 3]); 203 | void tf_trans_set(real_t T[4 * 4], const real_t r[3]); 204 | void tf_trans_get(const real_t T[4 * 4], real_t r[3]); 205 | void tf_rot_get(const real_t T[4 * 4], real_t C[3 * 3]); 206 | void tf_quat_get(const real_t T[4 * 4], real_t q[4]); 207 | void tf_inv(const real_t T[4 * 4], real_t T_inv[4 * 4]); 208 | void tf_point(const real_t T[4 * 4], const real_t p[3], real_t retval[3]); 209 | void tf_hpoint(const real_t T[4 * 4], const real_t p[4], real_t retval[4]); 210 | void tf_perturb_rot(real_t T[4 * 4], const real_t step_size, const int i); 211 | void tf_perturb_trans(real_t T[4 * 4], const real_t step_size, const int i); 212 | void rvec2rot(const real_t *rvec, const real_t eps, real_t *R); 213 | void euler321(const real_t euler[3], real_t C[3 * 3]); 214 | void rot2quat(const real_t C[3 * 3], real_t q[4]); 215 | void quat2euler(const real_t q[4], real_t euler[3]); 216 | void quat2rot(const real_t q[4], real_t C[3 * 3]); 217 | void quat_lmul(const real_t p[4], const real_t q[4], real_t r[4]); 218 | void quat_rmul(const real_t p[4], const real_t q[4], real_t r[4]); 219 | void quat_mul(const real_t p[4], const real_t q[4], real_t r[4]); 220 | void quat_delta(const real_t dalpha[3], real_t dq[4]); 221 | 222 | /****************************************************************************** 223 | * IMAGE 224 | ******************************************************************************/ 225 | 226 | typedef struct image_t { 227 | int width; 228 | int height; 229 | uint8_t *data; 230 | } image_t; 231 | 232 | void image_init(image_t *img, uint8_t *data, int width, int height); 233 | 234 | /****************************************************************************** 235 | * CV 236 | ******************************************************************************/ 237 | 238 | /******************************** RADTAN **************************************/ 239 | 240 | void radtan4_distort(const real_t params[4], const real_t p[2], real_t p_d[2]); 241 | void radtan4_point_jacobian(const real_t params[4], const real_t p[2], 242 | real_t J_point[2 * 2]); 243 | void radtan4_params_jacobian(const real_t params[4], const real_t p[2], 244 | real_t J_param[2 * 4]); 245 | 246 | /********************************* EQUI ***************************************/ 247 | 248 | void equi4_distort(const real_t params[4], const real_t p[2], real_t p_d[2]); 249 | void equi4_point_jacobian(const real_t params[4], const real_t p[2], 250 | real_t J_point[2 * 2]); 251 | void equi4_params_jacobian(const real_t params[4], const real_t p[2], 252 | real_t J_param[2 * 4]); 253 | 254 | /******************************** PINHOLE *************************************/ 255 | 256 | real_t pinhole_focal(const int image_width, const real_t fov); 257 | int pinhole_project(const real_t params[4], const real_t p_C[3], real_t x[2]); 258 | 259 | void pinhole_point_jacobian(const real_t params[4], real_t J_point[2 * 3]); 260 | void pinhole_params_jacobian(const real_t params[4], const real_t x[2], 261 | real_t J[2 * 4]); 262 | 263 | /**************************** PINHOLE-RADTAN4 *********************************/ 264 | 265 | void pinhole_radtan4_project(const real_t params[8], const real_t p_C[3], 266 | real_t x[2]); 267 | void pinhole_radtan4_project_jacobian(const real_t params[8], 268 | const real_t p_C[3], real_t J[2 * 3]); 269 | void pinhole_radtan4_params_jacobian(const real_t params[8], 270 | const real_t p_C[3], real_t J[2 * 8]); 271 | 272 | /***************************** PINHOLE-EQUI4 **********************************/ 273 | 274 | void pinhole_equi4_project(const real_t params[8], const real_t p_C[3], 275 | real_t x[2]); 276 | void pinhole_equi4_project_jacobian(const real_t params[8], const real_t p_C[3], 277 | real_t J[2 * 3]); 278 | void pinhole_equi4_params_jacobian(const real_t params[8], const real_t p_C[3], 279 | real_t J[2 * 8]); 280 | 281 | #endif // ZERO_H 282 | -------------------------------------------------------------------------------- /scripts/sim_data.m: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env octave-cli 2 | data_gnd_path = "./data_gnd"; 3 | data_noisy_path = "./data_noisy"; 4 | 5 | function q = rot2quat(R) 6 | m00 = R(1, 1); 7 | m01 = R(1, 2); 8 | m02 = R(1, 3); 9 | 10 | m10 = R(2, 1); 11 | m11 = R(2, 2); 12 | m12 = R(2, 3); 13 | 14 | m20 = R(3, 1); 15 | m21 = R(3, 2); 16 | m22 = R(3, 3); 17 | 18 | tr = m00 + m11 + m22; 19 | 20 | if (tr > 0) 21 | S = sqrt(tr+1.0) * 2; % S=4*qw 22 | qw = 0.25 * S; 23 | qx = (m21 - m12) / S; 24 | qy = (m02 - m20) / S; 25 | qz = (m10 - m01) / S; 26 | elseif ((m00 > m11) && (m00 > m22)) 27 | S = sqrt(1.0 + m00 - m11 - m22) * 2; % S=4*qx 28 | qw = (m21 - m12) / S; 29 | qx = 0.25 * S; 30 | qy = (m01 + m10) / S; 31 | qz = (m02 + m20) / S; 32 | elseif (m11 > m22) 33 | S = sqrt(1.0 + m11 - m00 - m22) * 2; % S=4*qy 34 | qw = (m02 - m20) / S; 35 | qx = (m01 + m10) / S; 36 | qy = 0.25 * S; 37 | qz = (m12 + m21) / S; 38 | else 39 | S = sqrt(1.0 + m22 - m00 - m11) * 2; % S=4*qz 40 | qw = (m10 - m01) / S; 41 | qx = (m02 + m20) / S; 42 | qy = (m12 + m21) / S; 43 | qz = 0.25 * S; 44 | endif 45 | 46 | q = [qw; qx; qy; qz]; 47 | endfunction 48 | 49 | function R = quat2rot(q) 50 | qw = q(1); 51 | qx = q(2); 52 | qy = q(3); 53 | qz = q(4); 54 | 55 | qx2 = qx**2; 56 | qy2 = qy**2; 57 | qz2 = qz**2; 58 | qw2 = qw**2; 59 | 60 | % Homogeneous form 61 | R11 = qw2 + qx2 - qy2 - qz2; 62 | R12 = 2 * (qx * qy - qw * qz); 63 | R13 = 2 * (qx * qz + qw * qy); 64 | 65 | R21 = 2 * (qx * qy + qw * qz); 66 | R22 = qw2 - qx2 + qy2 - qz2; 67 | R23 = 2 * (qy * qz - qw * qx); 68 | 69 | R31 = 2 * (qx * qz - qw * qy); 70 | R32 = 2 * (qy * qz + qw * qx); 71 | R33 = qw2 - qx2 - qy2 + qz2; 72 | 73 | R = [R11, R12, R13; R21, R22, R23; R31, R32, R33;]; 74 | endfunction 75 | 76 | function euler = quat2euler(q) 77 | qw = q(1); 78 | qx = q(2); 79 | qy = q(3); 80 | qz = q(4); 81 | 82 | qw2 = qw**2; 83 | qx2 = qx**2; 84 | qy2 = qy**2; 85 | qz2 = qz**2; 86 | 87 | t1 = atan2(2 * (qx * qw + qz * qy), (qw2 - qx2 - qy2 + qz2)); 88 | t2 = asin(2 * (qy * qw - qx * qz)); 89 | t3 = atan2(2 * (qx * qy + qz * qw), (qw2 + qx2 - qy2 - qz2)); 90 | euler = [t1; t2; t3]; 91 | endfunction 92 | 93 | function R = euler321(rpy) 94 | phi = rpy(1); 95 | theta = rpy(2); 96 | psi = rpy(3); 97 | 98 | R11 = cos(psi) * cos(theta); 99 | R21 = sin(psi) * cos(theta); 100 | R31 = -sin(theta); 101 | 102 | R12 = cos(psi) * sin(theta) * sin(phi) - sin(psi) * cos(phi); 103 | R22 = sin(psi) * sin(theta) * sin(phi) + cos(psi) * cos(phi); 104 | R32 = cos(theta) * sin(phi); 105 | 106 | R13 = cos(psi) * sin(theta) * cos(phi) + sin(psi) * sin(phi); 107 | R23 = sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi); 108 | R33 = cos(theta) * cos(phi); 109 | 110 | R = [R11, R12, R13; R21, R22, R23; R31, R32, R33;]; 111 | endfunction 112 | 113 | function dq = quat_delta(dalpha) 114 | half_norm = 0.5 * norm(dalpha); 115 | vector = sinc(half_norm) * 0.5 * dalpha; 116 | scalar = cos(half_norm); 117 | dq = [scalar; vector]; 118 | endfunction 119 | 120 | function result = quat_mul(p, q) 121 | % p_w = p(1); 122 | % p_x = p(2); 123 | % p_y = p(3); 124 | % p_z = p(4); 125 | % 126 | % q_w = q(1); 127 | % q_x = q(2); 128 | % q_y = q(3); 129 | % q_z = q(4); 130 | % 131 | % w = p_w * q_w - p_x * q_x - p_y * q_y - p_z * q_z; 132 | % x = p_w * q_x + q_x * p_w + p_y * q_z - p_z * q_y; 133 | % y = p_w * q_y - p_y * q_w + p_z * q_x + p_x * q_z; 134 | % z = p_w * q_z + p_z * q_w - p_x * q_y + p_y * q_x; 135 | 136 | % result = [w; x; y; z]; 137 | 138 | p_w = p(1); 139 | q_w = q(1); 140 | p_v = [p(2); p(3); p(4)]; 141 | q_v = [q(2); q(3); q(4)]; 142 | 143 | result = [p_w * q_w - p_v' * q_v; 144 | p_w * q_v + q_w * p_v + cross(p_v, q_v);]; 145 | endfunction 146 | 147 | function r = quatmul(p, q) 148 | r = quatlmul(p, q); 149 | endfunction 150 | 151 | function r = quatrmul(p, q) 152 | assert(size(p) == [4, 1]); 153 | assert(size(q) == [4, 1]); 154 | 155 | qw = q(1); 156 | qx = q(2); 157 | qy = q(3); 158 | qz = q(4); 159 | 160 | rprod = [ 161 | qw, -qx, -qy, -qz; 162 | qx, qw, qz, -qy; 163 | qy, -qz, qw, qx; 164 | qz, qy, -qx, qw; 165 | ]; 166 | 167 | r = rprod * p; 168 | endfunction 169 | 170 | function T = tf(rot, trans) 171 | assert(size(rot) == [3, 3] || size(rot) == [4, 1]); 172 | assert(size(trans) == [3, 1]); 173 | 174 | C = rot; 175 | if size(rot) == [4, 1] 176 | C = quat2rot(rot); 177 | endif 178 | 179 | T = eye(4, 4); 180 | T(1:3, 1:3) = C; 181 | T(1:3, 4) = trans; 182 | endfunction 183 | 184 | function C = tf_rot(tf) 185 | C = tf(1:3, 1:3); 186 | endfunction 187 | 188 | function r = tf_trans(T) 189 | r = T(1:3, 4); 190 | endfunction 191 | 192 | function hp = homogeneous(p) 193 | hp = [p; ones(1, columns(p))]; 194 | endfunction 195 | 196 | function p = dehomogeneous(hp) 197 | p = hp(1:3, :); 198 | endfunction 199 | 200 | function y = normalize(x) 201 | y = x / norm(x); 202 | endfunction 203 | 204 | function retval = randf(bounds, sz=1) 205 | val_min = bounds(1); 206 | val_max = bounds(2); 207 | retval = (val_max - val_min) * rand(sz, 1) + val_min; 208 | endfunction 209 | 210 | function T_target_camera = lookat(cam_pos, target, up_axis=[0.0; -1.0; 0.0]) 211 | assert(size(cam_pos) == [3, 1]); 212 | assert(size(target) == [3, 1]); 213 | assert(size(up_axis) == [3, 1]); 214 | 215 | % Note: If we were using OpenGL the cam_dir would be the opposite direction, 216 | % since in OpenGL the camera forward is -z. In robotics however our camera is 217 | % +z forward. 218 | cam_dir = normalize((target - cam_pos)); 219 | cam_right = normalize(cross(up_axis, cam_dir)); 220 | cam_up = cross(cam_dir, cam_right); 221 | 222 | A = [cam_right(1), cam_right(2), cam_right(3), 0.0; 223 | cam_up(1), cam_up(2), cam_up(3), 0.0; 224 | cam_dir(1), cam_dir(2), cam_dir(3), 0.0; 225 | 0.0, 0.0, 0.0, 1.0;]; 226 | 227 | B = [1.0, 0.0, 0.0, -cam_pos(1); 228 | 0.0, 1.0, 0.0, -cam_pos(2); 229 | 0.0, 0.0, 1.0, -cam_pos(3); 230 | 0.0, 0.0, 0.0, 1.0;]; 231 | 232 | T_camera_target = A * B; 233 | T_target_camera = inv(T_camera_target); 234 | endfunction 235 | 236 | function fx = focal_length(image_width, fov) 237 | fx = (image_width / 2.0) / tan(deg2rad(fov / 2.0)); 238 | endfunction 239 | 240 | function K = pinhole_K(intrinsics) 241 | fx = intrinsics(1); 242 | fy = intrinsics(2); 243 | cx = intrinsics(3); 244 | cy = intrinsics(4); 245 | 246 | K = eye(3); 247 | K(1, 1) = fx; 248 | K(2, 2) = fy; 249 | K(1, 3) = cx; 250 | K(2, 3) = cy; 251 | endfunction 252 | 253 | function camera = camera_init(resolution, fov, D=zeros(4, 1)) 254 | fx = focal_length(resolution(1), fov); 255 | fy = focal_length(resolution(2), fov); 256 | cx = resolution(1) / 2.0; 257 | cy = resolution(2) / 2.0; 258 | intrinsics = [fx, fy, cx, cy]; 259 | 260 | camera.resolution = resolution; 261 | camera.K = pinhole_K(intrinsics); 262 | camera.D = D; 263 | endfunction 264 | 265 | function [z, point_ids] = camera_measurements(K, resolution, T_WC, points_W) 266 | assert(size(K) == [3, 3]); 267 | assert(length(resolution) == 2); 268 | assert(size(T_WC) == [4, 4]); 269 | assert(rows(points_W) == 3); 270 | assert(columns(points_W) > 0); 271 | 272 | # Form projection matrix 273 | T_CW = inv(T_WC); 274 | R_CW = T_CW(1:3, 1:3); 275 | t_CW = T_CW(1:3, 4); 276 | P = K * [R_CW, t_CW]; 277 | 278 | # Setup 279 | image_width = resolution(1); 280 | image_height = resolution(2); 281 | nb_points = columns(points_W); 282 | 283 | # Check points 284 | z = []; 285 | point_ids = []; 286 | 287 | for i = 1:nb_points 288 | # Project point to image plane 289 | hp_W = homogeneous(points_W(:, i)); 290 | x = P * hp_W; 291 | 292 | # Check to see if point is infront of camera 293 | if x(3) < 1e-4 294 | continue; 295 | endif 296 | 297 | # Normalize projected ray 298 | x(1) = x(1) / x(3); 299 | x(2) = x(2) / x(3); 300 | x(3) = x(3) / x(3); 301 | z_hat = [x(1); x(2)]; 302 | 303 | # Check to see if ray is within image plane 304 | x_ok = (x(1) < image_width) && (x(1) > 0.0); 305 | y_ok = (x(2) < image_height) && (x(2) > 0.0); 306 | if x_ok && y_ok 307 | z = [z, z_hat]; 308 | point_ids = [point_ids, i]; 309 | endif 310 | endfor 311 | assert(columns(z) == length(point_ids)); 312 | endfunction 313 | 314 | function [poses, calib_center] = calib_generate_random_poses(calib_target, 315 | nb_poses) 316 | calib_width = (calib_target.nb_rows - 1.0) * calib_target.tag_size; 317 | calib_height = (calib_target.nb_cols - 1.0) * calib_target.tag_size; 318 | calib_center = [calib_width / 2.0; calib_height / 2.0; 0.0]; 319 | 320 | % Settings 321 | angle_range = [-20.0, 20.0]; 322 | x_range = [-0.5, 0.5]; 323 | y_range = [-0.5, 0.5]; 324 | z_range = [0.5, 0.7]; 325 | 326 | % For each position create a camera pose that "looks at" the AprilGrid 327 | % center in the target frame, T_TC. 328 | poses = {}; 329 | pose_idx = 1; 330 | for i = 1:nb_poses 331 | % Generate random pose 332 | x = unifrnd(x_range(1), x_range(2)); 333 | y = unifrnd(y_range(1), y_range(2)); 334 | z = unifrnd(z_range(1), z_range(2)); 335 | r_TC = calib_center + [x; y; z] ; 336 | T_TC = lookat(r_TC, calib_center); 337 | 338 | % Perturb the pose a little so it doesn't look at the center directly 339 | p = randf(angle_range); 340 | q = randf(angle_range); 341 | r = randf(angle_range); 342 | C_perturb = euler321(deg2rad([p, q, r])); 343 | r_perturb = zeros(3, 1); 344 | T_perturb = tf(C_perturb, r_perturb); 345 | 346 | poses{pose_idx} = T_perturb * T_TC; 347 | pose_idx++; 348 | endfor 349 | endfunction 350 | 351 | function calib_target = calib_target_init(nb_rows=6, nb_cols=7, tag_size=0.2) 352 | # Create calib_target grid 353 | nb_corners = nb_rows * nb_cols; 354 | object_points = zeros(3, nb_corners); 355 | idx = 1; 356 | for i = 1:nb_rows 357 | for j = 1:nb_cols 358 | object_points(1:2, idx) = [j - 1; i - 1]; 359 | idx += 1; 360 | endfor 361 | endfor 362 | 363 | # Chessboard struct 364 | calib_target.nb_rows = nb_rows; 365 | calib_target.nb_cols = nb_cols; 366 | calib_target.nb_corners = nb_corners; 367 | calib_target.tag_size = tag_size; 368 | calib_target.width = calib_target.nb_cols * calib_target.tag_size; 369 | calib_target.height = calib_target.nb_rows * calib_target.tag_size; 370 | calib_target.center = [((nb_cols - 1.0) / 2.0) * tag_size, 371 | ((nb_rows - 1.0) / 2.0) * tag_size]; 372 | calib_target.object_points = tag_size * object_points; 373 | endfunction 374 | 375 | function data = calib_sim(calib_target, T_WT, camera, nb_poses, debug=false) 376 | % Generate camera poses 377 | [poses, calib_center] = calib_generate_random_poses(calib_target, nb_poses); 378 | nb_poses = length(poses); 379 | 380 | % Transform calibration grid points from target to world frame 381 | p_data = []; 382 | hp_T = homogeneous(calib_target.object_points); 383 | hp_W = T_WT * hp_T; 384 | p_data = dehomogeneous(hp_W); 385 | assert(rows(calib_target.object_points) == 3); 386 | assert(rows(p_data) == 3); 387 | 388 | % Decompose target pose to quaternion and translation vector 389 | C_WT = tf_rot(T_WT); 390 | r_WT = tf_trans(T_WT); 391 | q_WT = rot2quat(C_WT); 392 | assert(norm(quat2rot(q_WT) - C_WT) < 1e-4); 393 | 394 | % Simulation calibration data 395 | point_ids_data = {}; 396 | z_data = {}; 397 | q_WC = {}; 398 | r_WC = {}; 399 | cam_poses = {}; 400 | 401 | for i = 1:nb_poses 402 | % Transform camera pose in target frame to world frame 403 | T_TC = poses{i}; 404 | T_WC = T_WT * T_TC; 405 | cam_poses{i} = T_WC; 406 | 407 | % Decompose camera pose to quaternion and translation vector 408 | C_WC = tf_rot(T_WC); 409 | q_WC{i} = rot2quat(C_WC); 410 | r_WC{i} = tf_trans(T_WC); 411 | assert(norm(quat2rot(q_WC{i}) - C_WC) < 1e-4); 412 | 413 | % Project world points to camera 414 | K = camera.K; 415 | image_size = camera.resolution; 416 | [z, point_ids] = camera_measurements(K, image_size, T_WC, p_data); 417 | 418 | z_data{i} = z; 419 | point_ids_data{i} = point_ids; 420 | endfor 421 | 422 | % Form simulation data struct 423 | % -- Time, camera and calib_target data 424 | data.time = 1:nb_poses; 425 | data.camera = camera; 426 | data.target = calib_target; 427 | % -- Calibration target 428 | data.q_WT = q_WT; 429 | data.r_WT = r_WT; 430 | % -- Camera poses 431 | data.q_WC = q_WC; 432 | data.r_WC = r_WC; 433 | % -- Image measurements and corresponding landmark points 434 | data.z_data = z_data; 435 | data.point_ids_data = point_ids_data; 436 | data.p_data = p_data; 437 | 438 | % Visualize 439 | if debug 440 | figure() 441 | hold on; 442 | for i = 1:nb_poses 443 | calib_target_draw(calib_target, T_WT); 444 | T_WC = cam_poses{i}; 445 | draw_camera(T_WC); 446 | draw_frame(T_WC, 0.05); 447 | endfor 448 | view(3); 449 | axis("equal"); 450 | xlabel("x [m]"); 451 | ylabel("y [m]"); 452 | zlabel("z [m]"); 453 | ginput(); 454 | endif 455 | endfunction 456 | 457 | function data_noisy = calib_data_add_noise(data) 458 | nb_poses = length(data.time); 459 | 460 | % Add noise to camera position and rotation 461 | for i = 1:nb_poses 462 | % Position 463 | % data.r_WC{i} += normrnd([0; 0; 0], 1e-2); 464 | data.r_WC{i} += normrnd([0; 0; 0], 1e-1); 465 | 466 | % Rotation 467 | dq = quat_delta(normrnd([0; 0; 0], 1e-2)); 468 | q_WC = data.q_WC{i}; 469 | data.q_WC{i} = quat_mul(dq, q_WC); 470 | endfor 471 | 472 | % Add noise to point data 473 | % data.p_data += normrnd(zeros(3, length(data.p_data)), 1e-2); 474 | 475 | data_noisy = data; 476 | endfunction 477 | 478 | function save_dataset(save_path, data) 479 | % Create directory to save dataset 480 | mkdir(save_path); 481 | 482 | % -- Save camera matrix 483 | cam_file = fopen(strcat(save_path, "camera.csv"), "w"); 484 | K = data.camera.K; 485 | fprintf(cam_file, "#camera_K\n"); 486 | fprintf(cam_file, "%f,%f,%f\n", K(1, 1), K(1, 2), K(1, 3)); 487 | fprintf(cam_file, "%f,%f,%f\n", K(2, 1), K(2, 2), K(2, 3)); 488 | fprintf(cam_file, "%f,%f,%f\n", K(3, 1), K(3, 2), K(3, 3)); 489 | fclose(cam_file); 490 | 491 | % -- Save camera poses 492 | cam_poses_file = fopen(strcat(save_path, "camera_poses.csv"), "w"); 493 | fprintf(cam_poses_file, "#qw,qx,qy,qz,rx,ry,rz\n"); 494 | for k = 1:length(data.q_WC) 495 | q = data.q_WC{k}; 496 | r = data.r_WC{k}; 497 | fprintf(cam_poses_file, "%f,%f,%f,%f,", q(1), q(2), q(3), q(4)); 498 | fprintf(cam_poses_file, "%f,%f,%f", r(1), r(2), r(3)); 499 | fprintf(cam_poses_file, "\n"); 500 | endfor 501 | fclose(cam_poses_file); 502 | 503 | % -- Save target pose 504 | target_pose_file = fopen(strcat(save_path, "target_pose.csv"), "w"); 505 | fprintf(target_pose_file, "#qw,qx,qy,qz,rx,ry,rz\n"); 506 | q = data.q_WT; 507 | r = data.r_WT; 508 | fprintf(target_pose_file, "%f,%f,%f,%f,", q(1), q(2), q(3), q(4)); 509 | fprintf(target_pose_file, "%f,%f,%f", r(1), r(2), r(3)); 510 | fprintf(target_pose_file, "\n"); 511 | fclose(target_pose_file); 512 | 513 | % -- Save measured keypoints 514 | keypoints_file = fopen(strcat(save_path, "keypoints.csv"), "w"); 515 | fprintf(keypoints_file, "#size,keypoints\n"); 516 | for i = 1:length(data.z_data) 517 | z_data = data.z_data{i}; 518 | fprintf(keypoints_file, "%d,", length(z_data) * 2); 519 | for j = 1:length(z_data) 520 | fprintf(keypoints_file, "%f,%f", z_data(1, j), z_data(2, j)); 521 | if j != length(z_data) 522 | fprintf(keypoints_file, ","); 523 | end 524 | endfor 525 | fprintf(keypoints_file, "\n"); 526 | endfor 527 | fclose(keypoints_file); 528 | 529 | % -- Save point ids 530 | point_ids_file = fopen(strcat(save_path, "point_ids.csv"), "w"); 531 | fprintf(point_ids_file, "#size,point_ids\n"); 532 | for i = 1:length(data.point_ids_data) 533 | id = data.point_ids_data{i}; 534 | fprintf(point_ids_file, "%d,", length(id)); 535 | for j = 1:length(id) 536 | fprintf(point_ids_file, "%d", id(j) - 1); # to correct for 0-index 537 | if j != length(id) 538 | fprintf(point_ids_file, ","); 539 | end 540 | endfor 541 | fprintf(point_ids_file, "\n"); 542 | endfor 543 | fclose(point_ids_file); 544 | 545 | % -- Save points 546 | points_file = fopen(strcat(save_path, "points.csv"), "w"); 547 | fprintf(points_file, "#x,y,z\n"); 548 | for i = 1:length(data.p_data) 549 | p = data.p_data(1:3, i); 550 | fprintf(points_file, "%f,%f,%f", p(1), p(2), p(3)); 551 | if j != length(id) 552 | fprintf(points_file, ","); 553 | end 554 | fprintf(points_file, "\n"); 555 | endfor 556 | fclose(points_file); 557 | end 558 | 559 | % Setup data 560 | % -- Create calibration target 561 | calib_target = calib_target_init(5, 5); 562 | C_WT = euler321(deg2rad([90.0, 0.0, -90.0])); 563 | r_WT = [1.0; 0.0; 0.0]; 564 | T_WT = tf(C_WT, r_WT); 565 | % -- Create camera 566 | res = [640; 480]; 567 | fov = 90.0; 568 | camera = camera_init(res, fov); 569 | % -- Create data 570 | nb_poses = 20; 571 | data_gnd = calib_sim(calib_target, T_WT, camera, nb_poses); 572 | data_noisy = calib_data_add_noise(data_gnd); 573 | % -- Save data 574 | save_dataset("./data_gnd/", data_gnd); 575 | save_dataset("./data_noisy/", data_noisy); 576 | -------------------------------------------------------------------------------- /ba/c/ba.h: -------------------------------------------------------------------------------- 1 | #ifndef BA_H 2 | #define BA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "util.h" 8 | 9 | struct keypoints_t { 10 | real_t **data; 11 | int size; 12 | } typedef keypoints_t; 13 | 14 | struct ba_data_t { 15 | /* real_t cam_K[3 * 3]; */ 16 | real_t cam[4]; /* fx, fy, cx, cy */ 17 | 18 | int nb_frames; 19 | real_t **cam_poses; 20 | real_t **target_pose; 21 | 22 | keypoints_t **keypoints; 23 | int **point_ids; 24 | 25 | real_t **points; 26 | int nb_points; 27 | } typedef ba_data_t; 28 | 29 | static void load_camera(const char *data_path, real_t cam[4]) { 30 | /* Setup csv path */ 31 | char cam_csv[1000] = {0}; 32 | strcat(cam_csv, data_path); 33 | strcat(cam_csv, "/camera.csv"); 34 | 35 | /* Parse csv file */ 36 | int nb_rows = 0; 37 | int nb_cols = 0; 38 | real_t **cam_K = csv_data(cam_csv, &nb_rows, &nb_cols); 39 | if (cam_K == NULL) { 40 | FATAL("Failed to load csv file [%s]!", cam_csv); 41 | } 42 | if (nb_rows != 3 || nb_cols != 3) { 43 | LOG_ERROR("Error while parsing camera file [%s]!", cam_csv); 44 | LOG_ERROR("-- Expected 3 rows got %d instead!", nb_rows); 45 | LOG_ERROR("-- Expected 3 cols got %d instead!", nb_cols); 46 | FATAL("Invalid camera file [%s]!", cam_csv); 47 | } 48 | 49 | /* Set cam */ 50 | cam[0] = cam_K[0][0]; 51 | cam[1] = cam_K[1][1]; 52 | cam[2] = cam_K[0][2]; 53 | cam[3] = cam_K[1][2]; 54 | 55 | /* Free cam_K */ 56 | for (int i = 0; i < nb_rows; i++) { 57 | free(cam_K[i]); 58 | } 59 | free(cam_K); 60 | } 61 | 62 | static real_t **load_poses(const char *csv_path, int *nb_poses) { 63 | assert(csv_path != NULL); 64 | assert(nb_poses != NULL); 65 | 66 | FILE *csv_file = fopen(csv_path, "r"); 67 | char line[MAX_LINE_LENGTH] = {0}; 68 | *nb_poses = dsv_rows(csv_path); 69 | real_t **poses = malloc(sizeof(real_t *) * *nb_poses); 70 | 71 | int pose_idx = 0; 72 | while (fgets(line, MAX_LINE_LENGTH, csv_file) != NULL) { 73 | if (line[0] == '#') { 74 | continue; 75 | } 76 | 77 | char entry[MAX_LINE_LENGTH] = {0}; 78 | real_t data[7] = {0}; 79 | int index = 0; 80 | for (size_t i = 0; i < strlen(line); i++) { 81 | char c = line[i]; 82 | if (c == ' ') { 83 | continue; 84 | } 85 | 86 | if (c == ',' || c == '\n') { 87 | data[index] = strtod(entry, NULL); 88 | memset(entry, '\0', sizeof(char) * 100); 89 | index++; 90 | } else { 91 | entry[strlen(entry)] = c; 92 | } 93 | } 94 | 95 | poses[pose_idx] = malloc(sizeof(real_t) * 7); 96 | poses[pose_idx][0] = data[0]; 97 | poses[pose_idx][1] = data[1]; 98 | poses[pose_idx][2] = data[2]; 99 | poses[pose_idx][3] = data[3]; 100 | poses[pose_idx][4] = data[4]; 101 | poses[pose_idx][5] = data[5]; 102 | poses[pose_idx][6] = data[6]; 103 | 104 | pose_idx++; 105 | } 106 | fclose(csv_file); 107 | 108 | return poses; 109 | } 110 | 111 | static real_t **load_camera_poses(const char *data_path, int *nb_cam_poses) { 112 | char cam_poses_csv[1000] = {0}; 113 | strcat(cam_poses_csv, data_path); 114 | strcat(cam_poses_csv, "/camera_poses.csv"); 115 | return load_poses(cam_poses_csv, nb_cam_poses); 116 | } 117 | 118 | static real_t **load_target_pose(const char *data_path) { 119 | char target_pose_csv[1000] = {0}; 120 | strcat(target_pose_csv, data_path); 121 | strcat(target_pose_csv, "/target_pose.csv"); 122 | 123 | int nb_poses = 0; 124 | return load_poses(target_pose_csv, &nb_poses); 125 | } 126 | 127 | void keypoints_free(keypoints_t *keypoints) { 128 | for (int i = 0; i < keypoints->size; i++) { 129 | free(keypoints->data[i]); 130 | } 131 | free(keypoints->data); 132 | free(keypoints); 133 | } 134 | 135 | void keypoints_print(const keypoints_t *keypoints) { 136 | printf("nb_keypoints: %d\n", keypoints->size); 137 | printf("keypoints:\n"); 138 | for (int i = 0; i < keypoints->size; i++) { 139 | printf("-- (%f, %f)\n", keypoints->data[i][0], keypoints->data[i][1]); 140 | } 141 | } 142 | 143 | static keypoints_t *parse_keypoints_line(char *line) { 144 | keypoints_t *keypoints = calloc(1, sizeof(keypoints_t)); 145 | keypoints->data = NULL; 146 | keypoints->size = 0; 147 | 148 | char entry[100] = {0}; 149 | int kp_ready = 0; 150 | real_t kp[2] = {0}; 151 | int kp_index = 0; 152 | 153 | /* Parse line */ 154 | for (size_t i = 0; i < strlen(line); i++) { 155 | char c = line[i]; 156 | if (c == ' ') { 157 | continue; 158 | } 159 | 160 | if (c == ',' || c == '\n') { 161 | /* Initialize keypoints */ 162 | if (keypoints->data == NULL) { 163 | size_t array_size = strtod(entry, NULL); 164 | keypoints->data = calloc(array_size, sizeof(real_t *)); 165 | keypoints->size = array_size / 2.0; 166 | 167 | } else { /* Parse keypoint */ 168 | if (kp_ready == 0) { 169 | kp[0] = strtod(entry, NULL); 170 | kp_ready = 1; 171 | 172 | } else { 173 | kp[1] = strtod(entry, NULL); 174 | keypoints->data[kp_index] = malloc(sizeof(real_t) * 2); 175 | keypoints->data[kp_index][0] = kp[0]; 176 | keypoints->data[kp_index][1] = kp[1]; 177 | 178 | kp_ready = 0; 179 | kp_index++; 180 | } 181 | } 182 | 183 | memset(entry, '\0', sizeof(char) * 100); 184 | } else { 185 | entry[strlen(entry)] = c; 186 | } 187 | } 188 | 189 | return keypoints; 190 | } 191 | 192 | static keypoints_t **load_keypoints(const char *data_path, int *nb_frames) { 193 | char keypoints_csv[1000] = {0}; 194 | strcat(keypoints_csv, data_path); 195 | strcat(keypoints_csv, "/keypoints.csv"); 196 | 197 | FILE *csv_file = fopen(keypoints_csv, "r"); 198 | *nb_frames = dsv_rows(keypoints_csv); 199 | keypoints_t **keypoints = calloc(*nb_frames, sizeof(keypoints_t *)); 200 | 201 | char line[1024] = {0}; 202 | int frame_idx = 0; 203 | while (fgets(line, 1024, csv_file) != NULL) { 204 | if (line[0] == '#') { 205 | continue; 206 | } 207 | 208 | keypoints[frame_idx] = parse_keypoints_line(line); 209 | frame_idx++; 210 | } 211 | fclose(csv_file); 212 | 213 | return keypoints; 214 | } 215 | 216 | static real_t **load_points(const char *data_path, int *nb_points) { 217 | char points_csv[1000] = {0}; 218 | strcat(points_csv, data_path); 219 | strcat(points_csv, "/points.csv"); 220 | 221 | /* Initialize memory for points */ 222 | *nb_points = dsv_rows(points_csv); 223 | real_t **points = malloc(sizeof(real_t *) * *nb_points); 224 | for (int i = 0; i < *nb_points; i++) { 225 | points[i] = malloc(sizeof(real_t) * 3); 226 | } 227 | 228 | /* Load file */ 229 | FILE *infile = fopen(points_csv, "r"); 230 | if (infile == NULL) { 231 | fclose(infile); 232 | return NULL; 233 | } 234 | 235 | /* Loop through data */ 236 | char line[1024] = {0}; 237 | size_t len_max = 1024; 238 | int point_idx = 0; 239 | int col_idx = 0; 240 | 241 | while (fgets(line, len_max, infile) != NULL) { 242 | if (line[0] == '#') { 243 | continue; 244 | } 245 | 246 | char entry[100] = {0}; 247 | for (size_t i = 0; i < strlen(line); i++) { 248 | char c = line[i]; 249 | if (c == ' ') { 250 | continue; 251 | } 252 | 253 | if (c == ',' || c == '\n') { 254 | points[point_idx][col_idx] = strtod(entry, NULL); 255 | memset(entry, '\0', sizeof(char) * 100); 256 | col_idx++; 257 | } else { 258 | entry[strlen(entry)] = c; 259 | } 260 | } 261 | 262 | col_idx = 0; 263 | point_idx++; 264 | } 265 | 266 | /* Cleanup */ 267 | fclose(infile); 268 | 269 | return points; 270 | } 271 | 272 | static int **load_point_ids(const char *data_path, int *nb_points) { 273 | char csv_path[1000] = {0}; 274 | strcat(csv_path, data_path); 275 | strcat(csv_path, "/point_ids.csv"); 276 | return load_iarrays(csv_path, nb_points); 277 | } 278 | 279 | ba_data_t *ba_load_data(const char *data_path) { 280 | ba_data_t *data = malloc(sizeof(ba_data_t)); 281 | 282 | int nb_ids = 0; 283 | load_camera(data_path, data->cam); 284 | data->cam_poses = load_camera_poses(data_path, &data->nb_frames); 285 | data->target_pose = load_target_pose(data_path); 286 | data->keypoints = load_keypoints(data_path, &data->nb_frames); 287 | data->point_ids = load_point_ids(data_path, &nb_ids); 288 | data->points = load_points(data_path, &data->nb_points); 289 | 290 | return data; 291 | } 292 | 293 | void ba_data_free(ba_data_t *data) { 294 | /* Camera poses */ 295 | for (int k = 0; k < data->nb_frames; k++) { 296 | free(data->cam_poses[k]); 297 | } 298 | free(data->cam_poses); 299 | 300 | /* Target pose */ 301 | free(data->target_pose[0]); 302 | free(data->target_pose); 303 | 304 | /* Keypoints */ 305 | for (int i = 0; i < data->nb_frames; i++) { 306 | keypoints_free(data->keypoints[i]); 307 | } 308 | free(data->keypoints); 309 | 310 | /* Point IDs */ 311 | for (int i = 0; i < data->nb_frames; i++) { 312 | free(data->point_ids[i]); 313 | } 314 | free(data->point_ids); 315 | 316 | /* Points */ 317 | for (int i = 0; i < data->nb_points; i++) { 318 | free(data->points[i]); 319 | } 320 | free(data->points); 321 | 322 | free(data); 323 | } 324 | 325 | int ba_residual_size(ba_data_t *data) { 326 | /* Calculate residual size */ 327 | int r_size = 0; 328 | for (int k = 0; k < data->nb_frames; k++) { 329 | r_size += data->point_ids[k][0]; 330 | } 331 | r_size = r_size * 2; 332 | /* ^ Scale 2 because each pixel error are size 2 */ 333 | 334 | return r_size; 335 | } 336 | 337 | real_t *ba_residuals(ba_data_t *data, int *r_size) { 338 | /* Initialize memory for residuals */ 339 | *r_size = ba_residual_size(data); 340 | real_t *r = calloc(*r_size, sizeof(real_t)); 341 | 342 | /* Target pose */ 343 | real_t T_WT[4 * 4] = {0}; 344 | tf(data->target_pose[0], T_WT); 345 | 346 | /* Loop over time */ 347 | int res_idx = 0; /* Residual index */ 348 | for (int k = 0; k < data->nb_frames; k++) { 349 | /* Form camera pose */ 350 | real_t T_WC[4 * 4] = {0}; 351 | tf(data->cam_poses[k], T_WC); 352 | 353 | /* Invert camera pose T_WC to T_CW */ 354 | real_t T_CW[4 * 4] = {0}; 355 | tf_inv(T_WC, T_CW); 356 | 357 | /* Get point ids and measurements at time step k */ 358 | const int nb_ids = data->point_ids[k][0]; 359 | const int *point_ids = &data->point_ids[k][1]; 360 | 361 | for (int i = 0; i < nb_ids; i++) { 362 | /* Get point in world frame */ 363 | const int id = point_ids[i]; 364 | const real_t *p_W = data->points[id]; 365 | 366 | /* Transform point in world frame to camera frame */ 367 | real_t p_C[3] = {0}; 368 | tf_point(T_CW, p_W, p_C); 369 | 370 | /* Project point in camera frame down to image plane */ 371 | real_t z_hat[2] = {0}; 372 | pinhole_project(data->cam, p_C, z_hat); 373 | 374 | /* Calculate reprojection error */ 375 | const real_t *z = data->keypoints[k]->data[i]; 376 | r[res_idx] = z[0] - z_hat[0]; 377 | r[res_idx + 1] = z[1] - z_hat[1]; 378 | res_idx += 2; 379 | } 380 | } 381 | 382 | return r; 383 | } 384 | 385 | static void J_intrinsics_point(const real_t cam[4], real_t J[2 * 2]) { 386 | /* J = [fx, 0, */ 387 | /* 0, fy]; */ 388 | const real_t fx = cam[0]; 389 | const real_t fy = cam[1]; 390 | zeros(J, 2, 2); 391 | J[0] = fx; 392 | J[3] = fy; 393 | } 394 | 395 | static void J_project(const real_t p_C[3], real_t J[2 * 3]) { 396 | const real_t x = p_C[0]; 397 | const real_t y = p_C[1]; 398 | const real_t z = p_C[2]; 399 | 400 | /* J = [1 / z, 0, -x / z^2, */ 401 | /* 0, 1 / z, -y / z^2]; */ 402 | zeros(J, 2, 3); 403 | J[0] = 1.0 / z; 404 | J[2] = -x / (z * z); 405 | J[4] = 1.0 / z; 406 | J[5] = -y / (z * z); 407 | } 408 | 409 | static void J_camera_rotation(const real_t q_WC[4], 410 | const real_t r_WC[3], 411 | const real_t p_W[3], 412 | real_t J[3 * 3]) { 413 | /* Convert quaternion to rotatoin matrix */ 414 | real_t C_WC[3 * 3] = {0}; 415 | quat2rot(q_WC, C_WC); 416 | 417 | /* J = C_WC * skew(p_W - r_WC); */ 418 | real_t C_CW[3 * 3] = {0}; 419 | mat_transpose(C_WC, 3, 3, C_CW); 420 | 421 | real_t x[3] = {0}; 422 | vec_sub(p_W, r_WC, x, 3); 423 | 424 | real_t S[3 * 3] = {0}; 425 | skew(x, S); 426 | 427 | cblas_dot(C_CW, 3, 3, S, 3, 3, J); 428 | } 429 | 430 | static void J_camera_translation(const real_t q_WC[4], real_t J[3 * 3]) { 431 | /* Convert quaternion to rotatoin matrix */ 432 | real_t C_WC[3 * 3] = {0}; 433 | quat2rot(q_WC, C_WC); 434 | 435 | /* J = -C_CW */ 436 | mat_transpose(C_WC, 3, 3, J); 437 | mat_scale(J, 3, 3, -1.0); 438 | } 439 | 440 | static void J_target_point(const real_t q_WC[4], real_t J[3 * 3]) { 441 | /* Convert quaternion to rotatoin matrix */ 442 | real_t C_WC[3 * 3] = {0}; 443 | quat2rot(q_WC, C_WC); 444 | 445 | /* J = C_CW */ 446 | mat_transpose(C_WC, 3, 3, J); 447 | } 448 | 449 | real_t *ba_jacobian(ba_data_t *data, int *J_rows, int *J_cols) { 450 | /* Initialize memory for jacobian */ 451 | *J_rows = ba_residual_size(data); 452 | *J_cols = (data->nb_frames * 6) + (data->nb_points * 3); 453 | real_t *J = calloc(*J_rows * *J_cols, sizeof(real_t)); 454 | zeros(J, *J_rows, *J_cols); 455 | 456 | /* Loop over camera poses */ 457 | int pose_idx = 0; 458 | int meas_idx = 0; 459 | 460 | for (int k = 0; k < data->nb_frames; k++) { 461 | /* Form camera pose */ 462 | real_t T_WC[4 * 4] = {0}; 463 | real_t q_WC[4] = {0}; 464 | real_t r_WC[3] = {0}; 465 | tf(data->cam_poses[k], T_WC); 466 | tf_quat_get(T_WC, q_WC); 467 | tf_trans_get(T_WC, r_WC); 468 | 469 | /* Invert T_WC to T_CW */ 470 | real_t T_CW[4 * 4] = {0}; 471 | tf_inv(T_WC, T_CW); 472 | 473 | /* Get point ids and measurements at time step k */ 474 | const int nb_ids = data->point_ids[k][0]; 475 | const int *point_ids = &data->point_ids[k][1]; 476 | 477 | /* Loop over observations at time k */ 478 | for (int i = 0; i < nb_ids; i++) { 479 | /* Get point in world frame */ 480 | const int id = point_ids[i]; 481 | const real_t *p_W = data->points[id]; 482 | 483 | /* Transform point in world frame to camera frame */ 484 | real_t p_C[3] = {0}; 485 | tf_point(T_CW, p_W, p_C); 486 | 487 | /* Camera pose jacobian */ 488 | /* -- Setup row start, row end, column start and column end */ 489 | const int rs = meas_idx * 2; 490 | const int re = rs + 1; 491 | int cs = pose_idx * 6; 492 | int ce = cs + 5; 493 | 494 | /* -- Form jacobians */ 495 | real_t J_K[2 * 2] = {0}; 496 | real_t J_P[2 * 3] = {0}; 497 | real_t J_h[2 * 3] = {0}; 498 | J_intrinsics_point(data->cam, J_K); 499 | J_project(p_C, J_P); 500 | cblas_dot(J_K, 2, 2, J_P, 2, 3, J_h); 501 | 502 | /* -- J_cam_rot = -1 * J_h * J_C; */ 503 | real_t J_C[3 * 3] = {0}; 504 | real_t J_cam_rot[2 * 3] = {0}; 505 | J_camera_rotation(q_WC, r_WC, p_W, J_C); 506 | cblas_dot(J_h, 2, 3, J_C, 3, 3, J_cam_rot); 507 | mat_scale(J_cam_rot, 2, 3, -1); 508 | 509 | /* -- J_cam_pos = -1 * J_h * J_r; */ 510 | real_t J_r[3 * 3] = {0}; 511 | real_t J_cam_pos[2 * 3] = {0}; 512 | J_camera_translation(q_WC, J_r); 513 | cblas_dot(J_h, 2, 3, J_r, 3, 3, J_cam_pos); 514 | mat_scale(J_cam_pos, 2, 3, -1); 515 | 516 | /* -- Fill in the big jacobian */ 517 | mat_block_set(J, *J_cols, rs, cs, re, cs + 2, J_cam_rot); 518 | mat_block_set(J, *J_cols, rs, cs + 3, re, ce, J_cam_pos); 519 | 520 | /* Point jacobian */ 521 | /* -- Setup row start, row end, column start and column end */ 522 | cs = (data->nb_frames * 6) + point_ids[i] * 3; 523 | ce = cs + 2; 524 | /* -- Form jacobians */ 525 | real_t J_p[3 * 3] = {0}; 526 | J_target_point(q_WC, J_p); 527 | /* -- J_point = -1 * J_h * J_target_point(q_WC); */ 528 | real_t J_point[2 * 3] = {0}; 529 | cblas_dot(J_h, 2, 3, J_p, 3, 3, J_point); 530 | mat_scale(J_point, 2, 3, -1); 531 | /* -- Fill in the big jacobian */ 532 | mat_block_set(J, *J_cols, rs, cs, re, ce, J_point); 533 | 534 | meas_idx++; 535 | } 536 | pose_idx++; 537 | } 538 | 539 | return J; 540 | } 541 | 542 | void ba_update(ba_data_t *data, real_t *dx) { 543 | /* Update camera poses */ 544 | for (int k = 0; k < data->nb_frames; k++) { 545 | const int s = k * 6; 546 | 547 | /* Update camera rotation */ 548 | /* dq = quat_delta(dalpha) */ 549 | /* q_WC_k = quat_mul(dq, q_WC_k) */ 550 | real_t *cam_pose = data->cam_poses[k]; 551 | const real_t dalpha[3] = {dx[s], dx[s + 1], dx[s + 2]}; 552 | real_t dq[4] = {0}; 553 | real_t q_WC[4] = {cam_pose[0], cam_pose[1], cam_pose[2], cam_pose[3]}; 554 | real_t q_new[4] = {0}; 555 | quat_delta(dalpha, dq); 556 | quat_mul(dq, q_WC, q_new); 557 | cam_pose[0] = q_new[0]; 558 | cam_pose[1] = q_new[1]; 559 | cam_pose[2] = q_new[2]; 560 | cam_pose[3] = q_new[3]; 561 | 562 | /* Update camera position */ 563 | /* r_WC_k += dr_WC */ 564 | const real_t dr_WC[3] = {dx[s + 3], dx[s + 4], dx[s + 5]}; 565 | cam_pose[4] += dr_WC[0]; 566 | cam_pose[5] += dr_WC[1]; 567 | cam_pose[6] += dr_WC[2]; 568 | } 569 | 570 | /* Update points */ 571 | for (int i = 0; i < data->nb_points; i++) { 572 | const int s = (data->nb_frames * 6) + (i * 3); 573 | const real_t dp_W[3] = {dx[s], dx[s + 1], dx[s + 2]}; 574 | data->points[i][0] += dp_W[0]; 575 | data->points[i][1] += dp_W[1]; 576 | data->points[i][2] += dp_W[2]; 577 | } 578 | } 579 | 580 | real_t ba_cost(const real_t *e, const int length) { 581 | /* cost = 0.5 * e' * e */ 582 | real_t cost = 0.0; 583 | cblas_dot(e, 1, length, e, length, 1, &cost); 584 | return cost * 0.5; 585 | } 586 | 587 | static real_t calc_reproj_error(const real_t *e, const int length) { 588 | real_t sse = 0; 589 | for (int i = 0; i < length; i += 2) { 590 | real_t err_x = e[i]; 591 | real_t err_y = e[i + 1]; 592 | real_t err_norm = sqrt(err_x * err_x + err_y * err_y); 593 | sse += err_norm * err_norm; 594 | } 595 | 596 | real_t nb_reproj_errors = length / 2.0; 597 | real_t mse = sse / nb_reproj_errors; 598 | real_t rmse = sqrt(mse); 599 | 600 | return rmse; 601 | } 602 | 603 | void ba_solve(ba_data_t *data) { 604 | int max_iter = 10; 605 | real_t lambda = 1e-4; 606 | 607 | int e_size = 0; 608 | real_t *e = ba_residuals(data, &e_size); 609 | real_t cost_prev = ba_cost(e, e_size); 610 | free(e); 611 | 612 | for (int iter = 0; iter < max_iter; iter++) { 613 | /* Jacobians */ 614 | int E_rows = 0; 615 | int E_cols = 0; 616 | real_t *E = ba_jacobian(data, &E_rows, &E_cols); 617 | 618 | /* Solve Gauss-Newton system [H dx = g]: Solve for dx */ 619 | /* -- Calculate L.H.S of Gauss-Newton: H = (E' * W * E); */ 620 | real_t *E_t = mat_new(E_cols, E_rows); 621 | real_t *H = mat_new(E_cols, E_cols); 622 | mat_transpose(E, E_rows, E_cols, E_t); 623 | cblas_dot(E_t, E_cols, E_rows, E, E_rows, E_cols, H); 624 | /* -- Apply Levenberg-Marquardt damping: H = H + lambda * H_diag */ 625 | for (int i = 0; i < E_cols; i++) { 626 | H[(i * E_cols) + i] += lambda * H[(i * E_cols) + i]; 627 | } 628 | /* -- Calculate R.H.S of Gauss-Newton: g = -E' * W * e; */ 629 | real_t *g = vec_new(E_cols); 630 | mat_scale(E_t, E_cols, E_rows, -1.0); 631 | e = ba_residuals(data, &e_size); 632 | cblas_dot(E_t, E_cols, E_rows, e, e_size, 1, g); 633 | free(e); 634 | free(E); 635 | free(E_t); 636 | /* -- Solve linear system: H * dx = g */ 637 | real_t *dx = vec_new(E_cols); 638 | /* chol_solve(H, g, dx, E_cols); */ 639 | lapack_chol_solve(H, g, dx, E_cols); 640 | free(H); 641 | free(g); 642 | 643 | /* Update */ 644 | ba_update(data, dx); 645 | free(dx); 646 | 647 | /* Calculate cost */ 648 | e = ba_residuals(data, &e_size); 649 | real_t cost = ba_cost(e, e_size); 650 | const real_t dcost = cost - cost_prev; 651 | const real_t rms_reproj_error = calc_reproj_error(e, e_size); 652 | free(e); 653 | 654 | printf("iter: [%d] ", iter); 655 | printf("lambda: %.2e ", lambda); 656 | printf("cost: %.4e ", cost); 657 | printf("dcost: %.2e ", dcost); 658 | printf("rms reproj error: %.2f\n", rms_reproj_error); 659 | 660 | /* Termination criteria */ 661 | if (fabs(dcost) < 1.0e-1) { 662 | printf("Done!\n"); 663 | break; 664 | } 665 | 666 | /* Update lambda */ 667 | if (dcost < 0) { 668 | lambda /= 10.0; 669 | cost_prev = cost; 670 | 671 | } else { 672 | lambda *= 10.0; 673 | 674 | /* Restore previous state because update failed */ 675 | for (int i = 0; i < E_cols; i++) 676 | dx[i] *= -1; 677 | ba_update(data, dx); 678 | } 679 | } 680 | } 681 | 682 | #endif // BA_H 683 | -------------------------------------------------------------------------------- /ba/cpp/util.hpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * This file is huge, it contains everything from parsing yaml files, linear 3 | * algebra functions to networking code used for robotics. 4 | * 5 | * Contents: 6 | * - Data Type 7 | * - Macros 8 | * - Data 9 | * - Filesystem 10 | * - Algebra 11 | * - Linear Algebra 12 | * - Geometry 13 | * - Differential Geometry 14 | * - Statistics 15 | * - Transform 16 | * - Time 17 | * 18 | ****************************************************************************/ 19 | #pragma once 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | #include 54 | 55 | /****************************************************************************** 56 | * DATA TYPE 57 | *****************************************************************************/ 58 | 59 | /* PRECISION TYPE */ 60 | #define PRECISION 1 // Single Precision 61 | // #define PRECISION 2 // Double Precision 62 | 63 | #if PRECISION == 1 64 | #define real_t float 65 | #elif PRECISION == 2 66 | #define real_t double 67 | #else 68 | #define real_t double 69 | #endif 70 | 71 | #define col_major_t Eigen::ColMajor 72 | #define row_major_t Eigen::RowMajor 73 | 74 | typedef Eigen::Matrix vec2_t; 75 | typedef Eigen::Matrix vec3_t; 76 | typedef Eigen::Matrix vec4_t; 77 | typedef Eigen::Matrix vec5_t; 78 | typedef Eigen::Matrix vec6_t; 79 | typedef Eigen::Matrix vecx_t; 80 | typedef Eigen::Matrix mat2_t; 81 | typedef Eigen::Matrix mat3_t; 82 | typedef Eigen::Matrix mat4_t; 83 | typedef Eigen::Matrix matx_t; 84 | typedef Eigen::Matrix mat34_t; 85 | typedef Eigen::Quaternion quat_t; 86 | typedef Eigen::AngleAxis angle_axis_t; 87 | typedef Eigen::Matrix row_vector_t; 88 | typedef Eigen::Matrix col_vector_t; 89 | typedef Eigen::Array arrayx_t; 90 | 91 | typedef std::vector> vec2s_t; 92 | typedef std::vector> vec3s_t; 93 | typedef std::vector> vec4s_t; 94 | typedef std::vector> vec5s_t; 95 | typedef std::vector> vec6s_t; 96 | typedef std::vector vecxs_t; 97 | typedef std::vector> mat2s_t; 98 | typedef std::vector> mat3s_t; 99 | typedef std::vector> mat4s_t; 100 | typedef std::vector> matxs_t; 101 | typedef std::vector> quats_t; 102 | 103 | template 104 | using vec_t = Eigen::Matrix; 105 | 106 | template 109 | using mat_t = Eigen::Matrix; 110 | 111 | template 114 | using map_mat_t = Eigen::Map>; 115 | 116 | template 117 | using map_vec_t = Eigen::Map>; 118 | 119 | typedef uint64_t timestamp_t; 120 | typedef std::vector timestamps_t; 121 | 122 | /****************************************************************************** 123 | * MACROS 124 | *****************************************************************************/ 125 | 126 | #define __FILENAME__ \ 127 | (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__) 128 | 129 | #define LOG_ERROR(M, ...) \ 130 | fprintf(stderr, \ 131 | "\033[31m[ERROR] [%s:%d] " M "\033[0m\n", \ 132 | __FILENAME__, \ 133 | __LINE__, \ 134 | ##__VA_ARGS__) 135 | 136 | #define LOG_INFO(M, ...) fprintf(stdout, "[INFO] " M "\n", ##__VA_ARGS__) 137 | #define LOG_WARN(M, ...) \ 138 | fprintf(stdout, "\033[33m[WARN] " M "\033[0m\n", ##__VA_ARGS__) 139 | 140 | #define FATAL(M, ...) \ 141 | fprintf(stdout, \ 142 | "\033[31m[FATAL] [%s:%d] " M "\033[0m\n", \ 143 | __FILENAME__, \ 144 | __LINE__, \ 145 | ##__VA_ARGS__); \ 146 | exit(-1) 147 | 148 | #ifdef NDEBUG 149 | #define DEBUG(M, ...) 150 | #else 151 | #define DEBUG(M, ...) fprintf(stdout, "[DEBUG] " M "\n", ##__VA_ARGS__) 152 | #endif 153 | 154 | #define UNUSED(expr) \ 155 | do { \ 156 | (void) (expr); \ 157 | } while (0) 158 | 159 | #ifndef CHECK 160 | #define CHECK(A, M, ...) \ 161 | if (!(A)) { \ 162 | LOG_ERROR(M, ##__VA_ARGS__); \ 163 | goto error; \ 164 | } 165 | #endif 166 | 167 | /****************************************************************************** 168 | * DATA 169 | *****************************************************************************/ 170 | 171 | /** 172 | * Convert bytes to signed 8bit number 173 | */ 174 | int8_t int8(const uint8_t *data, const size_t offset); 175 | 176 | /** 177 | * Convert bytes to unsigned 8bit number 178 | */ 179 | uint8_t uint8(const uint8_t *data, const size_t offset); 180 | 181 | /** 182 | * Convert bytes to signed 16bit number 183 | */ 184 | int16_t int16(const uint8_t *data, const size_t offset); 185 | 186 | /** 187 | * Convert bytes to unsigned 16bit number 188 | */ 189 | uint16_t uint16(const uint8_t *data, const size_t offset); 190 | 191 | /** 192 | * Convert bytes to signed 32bit number 193 | */ 194 | int32_t int32(const uint8_t *data, const size_t offset); 195 | 196 | /** 197 | * Convert bytes to unsigned 32bit number 198 | */ 199 | uint32_t uint32(const uint8_t *data, const size_t offset); 200 | 201 | /** 202 | * Allocate memory for a C-style string 203 | */ 204 | char *malloc_string(const char *s); 205 | 206 | /** 207 | * Get number of rows in CSV file. 208 | * @returns Number of rows in CSV file else -1 for failure. 209 | */ 210 | int csv_rows(const char *fp); 211 | 212 | /** 213 | * Get number of cols in CSV file. 214 | * @returns Number of cols in CSV file else -1 for failure. 215 | */ 216 | int csv_cols(const char *fp); 217 | 218 | /** 219 | * Return csv fields as strings and number of fields in csv file. 220 | */ 221 | char **csv_fields(const char *fp, int *nb_fields); 222 | 223 | /** 224 | * Load data in csv file `fp`. Assumming the data are real_ts. Also returns 225 | * number of rows and cols in `nb_rows` and `nb_cols` respectively. 226 | */ 227 | real_t **csv_data(const char *fp, int *nb_rows, int *nb_cols); 228 | 229 | /** 230 | * Load integer arrays in csv file located at `csv_path`. The number of arrays 231 | * is returned in `nb_arrays`. 232 | */ 233 | int **load_iarrays(const char *csv_path, int *nb_arrays); 234 | 235 | /** 236 | * Load real_t arrays in csv file located at `csv_path`. The number of arrays 237 | * is returned in `nb_arrays`. 238 | */ 239 | real_t **load_darrays(const char *csv_path, int *nb_arrays); 240 | 241 | /** 242 | * Get number of rows in CSV file. 243 | * @returns Number of rows in CSV file else -1 for failure. 244 | */ 245 | int csv_rows(const std::string &file_path); 246 | 247 | /** 248 | * Get number of columns in CSV file. 249 | * @returns Number of columns in CSV file else -1 for failure. 250 | */ 251 | int csv_cols(const std::string &file_path); 252 | 253 | /** 254 | * Convert CSV file to matrix. 255 | * @returns 0 for success, -1 for failure 256 | */ 257 | int csv2mat(const std::string &file_path, const bool header, matx_t &data); 258 | 259 | /** 260 | * Convert matrix to csv file. 261 | * @returns 0 for success, -1 for failure 262 | */ 263 | int mat2csv(const std::string &file_path, const matx_t &data); 264 | 265 | /** 266 | * Convert vector to csv file. 267 | * @returns 0 for success, -1 for failure 268 | */ 269 | int vec2csv(const std::string &file_path, const std::deque &data); 270 | 271 | /** 272 | * Convert timestamps to csv file. 273 | * @returns 0 for success, -1 for failure 274 | */ 275 | int ts2csv(const std::string &file_path, const std::deque &data); 276 | 277 | /** 278 | * Print progress to screen 279 | */ 280 | void print_progress(const real_t percentage); 281 | 282 | /** 283 | * Check if vector `x` is all true. 284 | */ 285 | bool all_true(const std::vector x); 286 | 287 | int check_jacobian(const std::string &jac_name, 288 | const matx_t &fdiff, 289 | const matx_t &jac, 290 | const real_t threshold, 291 | const bool print = false); 292 | 293 | /****************************************************************************** 294 | * FILESYSTEM 295 | *****************************************************************************/ 296 | 297 | /** 298 | * Open file in `path` with `mode` and set `nb_rows`. 299 | * @returns File pointer on success, nullptr on failure. 300 | */ 301 | FILE *file_open(const std::string &path, 302 | const std::string &mode, 303 | int *nb_rows = nullptr); 304 | 305 | /** 306 | * Skip file line in file `fp`. 307 | */ 308 | void skip_line(FILE *fp); 309 | 310 | /** 311 | * Get number of rows in file. 312 | * @returns Number of rows in file else -1 for failure. 313 | */ 314 | int file_rows(const std::string &file_path); 315 | 316 | /** 317 | * Copy file from path `src` to path `dest. 318 | * 319 | * @returns 0 for success else -1 if `src` file could not be opend, or -2 if 320 | * `dest` file could not be opened. 321 | */ 322 | int file_copy(const std::string &src, const std::string &dest); 323 | 324 | /** 325 | * Return file extension in `path`. 326 | */ 327 | std::string parse_fext(const std::string &path); 328 | 329 | /** 330 | * Return basename 331 | */ 332 | std::string parse_fname(const std::string &path); 333 | 334 | /** 335 | * Check if file exists 336 | * 337 | * @param path Path to file 338 | * @returns true or false 339 | */ 340 | bool file_exists(const std::string &path); 341 | 342 | /** 343 | * Check if path exists 344 | * 345 | * @param path Path 346 | * @returns true or false 347 | */ 348 | bool dir_exists(const std::string &path); 349 | 350 | /** 351 | * Create directory 352 | * 353 | * @param path Path 354 | * @returns 0 for success, -1 for failure 355 | */ 356 | int dir_create(const std::string &path); 357 | 358 | /** 359 | * Return directory name 360 | * 361 | * @param path Path 362 | * @returns directory name 363 | */ 364 | std::string dir_name(const std::string &path); 365 | 366 | /** 367 | * Strips a target character from the start and end of a string 368 | * 369 | * @param s String to strip 370 | * @param target Target character to strip 371 | * @returns Stripped string 372 | */ 373 | std::string strip(const std::string &s, const std::string &target = " "); 374 | 375 | /** 376 | * Strips a target character from the end of a string 377 | * 378 | * @param s String to strip 379 | * @param target Target character to strip 380 | * @returns Stripped string 381 | */ 382 | std::string strip_end(const std::string &s, const std::string &target = " "); 383 | 384 | /** 385 | * Create directory 386 | * 387 | * @param path Path to directory 388 | * @returns 0 for success, -1 for failure 389 | */ 390 | int create_dir(const std::string &path); 391 | 392 | /** 393 | * Remove directory 394 | * 395 | * @param path Path to directory 396 | * @returns 0 for success, -1 for failure 397 | */ 398 | int remove_dir(const std::string &path); 399 | 400 | /** 401 | * Remove file extension 402 | * 403 | * @param path Path to directory 404 | * @returns File path without extension 405 | */ 406 | std::string remove_ext(const std::string &path); 407 | 408 | /** 409 | * List directory 410 | * 411 | * @param path Path to directory 412 | * @param results List of files and directories 413 | * @returns 0 for success, -1 for failure 414 | */ 415 | int list_dir(const std::string &path, std::vector &results); 416 | 417 | /** 418 | * Split path into a number of elements 419 | * 420 | * @param path Path 421 | * @returns List of path elements 422 | */ 423 | std::vector path_split(const std::string path); 424 | 425 | /** 426 | * Combine `path1` and `path2` 427 | * 428 | * @param path1 Path 1 429 | * @param path2 Path 22 430 | * @returns Combined path 431 | */ 432 | std::string paths_combine(const std::string path1, const std::string path2); 433 | 434 | /****************************************************************************** 435 | * ALGEBRA 436 | *****************************************************************************/ 437 | 438 | /** 439 | * Sign of number 440 | * 441 | * @param[in] x Number to check sign 442 | * @return 443 | * - 0: Number is zero 444 | * - 1: Positive number 445 | * - -1: Negative number 446 | */ 447 | int sign(const real_t x); 448 | 449 | /** 450 | * Floating point comparator 451 | * 452 | * @param[in] f1 First value 453 | * @param[in] f2 Second value 454 | * @return 455 | * - 0: if equal 456 | * - 1: if f1 > f2 457 | * - -1: if f1 < f2 458 | */ 459 | int fltcmp(const real_t f1, const real_t f2); 460 | 461 | /** 462 | * Calculate binomial coefficient 463 | * 464 | * @param[in] n 465 | * @param[in] k 466 | * @returns Binomial coefficient 467 | */ 468 | real_t binomial(const real_t n, const real_t k); 469 | 470 | /** 471 | * Return evenly spaced numbers over a specified interval. 472 | */ 473 | template 474 | std::vector linspace(const T start, const T end, const int num) { 475 | std::vector linspaced; 476 | 477 | if (num == 0) { 478 | return linspaced; 479 | } 480 | if (num == 1) { 481 | linspaced.push_back(start); 482 | return linspaced; 483 | } 484 | 485 | const real_t diff = static_cast(end - start); 486 | const real_t delta = diff / static_cast(num - 1); 487 | for (int i = 0; i < num - 1; ++i) { 488 | linspaced.push_back(start + delta * i); 489 | } 490 | linspaced.push_back(end); 491 | return linspaced; 492 | } 493 | 494 | /****************************************************************************** 495 | * LINEAR ALGEBRA 496 | *****************************************************************************/ 497 | 498 | /** 499 | * Print shape of a matrix 500 | * 501 | * @param[in] name Name of matrix 502 | * @param[in] A Matrix 503 | */ 504 | void print_shape(const std::string &name, const matx_t &A); 505 | 506 | /** 507 | * Print shape of a vector 508 | * 509 | * @param[in] name Name of vector 510 | * @param[in] v Vector 511 | */ 512 | void print_shape(const std::string &name, const vecx_t &v); 513 | 514 | /** 515 | * Print array 516 | * 517 | * @param[in] name Name of array 518 | * @param[in] array Target array 519 | * @param[in] size Size of target array 520 | */ 521 | void print_array(const std::string &name, 522 | const real_t *array, 523 | const size_t size); 524 | 525 | /** 526 | * Print vector `v` with a `name`. 527 | */ 528 | void print_vector(const std::string &name, const vecx_t &v); 529 | 530 | /** 531 | * Print matrix `m` with a `name`. 532 | */ 533 | void print_matrix(const std::string &name, const matx_t &m); 534 | 535 | /** 536 | * Print quaternion `q` with a `name`. 537 | */ 538 | void print_quaternion(const std::string &name, const quat_t &q); 539 | 540 | /** 541 | * Array to string 542 | * 543 | * @param[in] array Target array 544 | * @param[in] size Size of target array 545 | * @returns String of array 546 | */ 547 | std::string array2str(const real_t *array, const size_t size); 548 | 549 | /** 550 | * Convert real_t array to Eigen::Vector 551 | * 552 | * @param[in] x Input array 553 | * @param[in] size Size of input array 554 | * @param[out] y Output vector 555 | */ 556 | void array2vec(const real_t *x, const size_t size, vecx_t &y); 557 | 558 | /** 559 | * Vector to array 560 | * 561 | * @param[in] v Vector 562 | * @returns Array 563 | */ 564 | real_t *vec2array(const vecx_t &v); 565 | 566 | /** 567 | * Matrix to array 568 | * 569 | * @param[in] m Matrix 570 | * @returns Array 571 | */ 572 | real_t *mat2array(const matx_t &m); 573 | 574 | /** 575 | * Quaternion to array 576 | * 577 | * *VERY IMPORTANT*: The returned array is (x, y, z, w). 578 | * 579 | * @param[in] q Quaternion 580 | * @returns Array 581 | */ 582 | real_t *quat2array(const quat_t &q); 583 | 584 | /** 585 | * Vector to array 586 | * 587 | * @param[in] v Vector 588 | * @param[out] out Output array 589 | */ 590 | void vec2array(const vecx_t &v, real_t *out); 591 | 592 | /** 593 | * Matrix to array 594 | * 595 | * @param[in] m Matrix 596 | * @param[in] out Output array 597 | */ 598 | void mat2array(const matx_t &m, real_t *out); 599 | 600 | /** 601 | * Matrix to list of vectors 602 | * 603 | * @param[in] m Matrix 604 | * @param[in] row_wise Row wise 605 | * @returns Vectors 606 | */ 607 | std::vector mat2vec(const matx_t &m, const bool row_wise = true); 608 | 609 | /** 610 | * Matrix to list of vectors of size 3 611 | * 612 | * @param[in] m Matrix 613 | * @param[in] row_wise Row wise 614 | * @returns Vectors 615 | */ 616 | vec3s_t mat2vec3(const matx_t &m, const bool row_wise = true); 617 | 618 | /** 619 | * Matrix to list of vectors of size 3 620 | * 621 | * @param[in] m Matrix 622 | * @param[in] row_wise Row wise 623 | * @returns Vectors 624 | */ 625 | vec2s_t mat2vec2(const matx_t &m, const bool row_wise = true); 626 | 627 | /** 628 | * Vectors to matrix 629 | */ 630 | matx_t vecs2mat(const vec3s_t &vs); 631 | 632 | /** 633 | * Vector to string 634 | * 635 | * @param[in] v Vector 636 | * @param[in] brackets Brakcets around vector string 637 | * @returns Vector as a string 638 | */ 639 | std::string vec2str(const vecx_t &v, const bool brackets = true); 640 | 641 | /** 642 | * Array to string 643 | * 644 | * @param[in] arr Array 645 | * @param[in] len Length of array 646 | * @param[in] brackets Brakcets around vector string 647 | * @returns Array as a string 648 | */ 649 | std::string arr2str(const real_t *arr, const size_t len, bool brackets = true); 650 | 651 | /** 652 | * Matrix to string 653 | * 654 | * @param[in] m Matrix 655 | * @param[in] indent Indent string 656 | * @returns Array as a string 657 | */ 658 | std::string mat2str(const matx_t &m, const std::string &indent = " "); 659 | 660 | /** 661 | * Normalize vector x 662 | */ 663 | vec2_t normalize(const vec2_t &x); 664 | 665 | /** 666 | * Normalize vector `v`. 667 | */ 668 | vec3_t normalize(const vec3_t &v); 669 | 670 | /** 671 | * Condition number of `A`. 672 | */ 673 | real_t cond(const matx_t &A); 674 | 675 | /** 676 | * Zeros-matrix 677 | * 678 | * @param rows Number of rows 679 | * @param cols Number of cols 680 | * @returns Zeros matrix 681 | */ 682 | matx_t zeros(const int rows, const int cols); 683 | 684 | /** 685 | * Zeros square matrix 686 | * 687 | * @param size Square size of matrix 688 | * @returns Zeros matrix 689 | */ 690 | matx_t zeros(const int size); 691 | 692 | /** 693 | * Identity-matrix 694 | * 695 | * @param rows Number of rows 696 | * @param cols Number of cols 697 | * @returns Identity matrix 698 | */ 699 | matx_t I(const int rows, const int cols); 700 | 701 | /** 702 | * Identity square matrix 703 | * 704 | * @param size Square size of matrix 705 | * @returns Identity square matrix 706 | */ 707 | matx_t I(const int size); 708 | 709 | /** 710 | * Ones-matrix 711 | * 712 | * @param rows Number of rows 713 | * @param cols Number of cols 714 | * @returns Ones square matrix 715 | */ 716 | matx_t ones(const int rows, const int cols); 717 | 718 | /** 719 | * Ones square matrix 720 | * 721 | * @param size Square size of matrix 722 | * @returns Ones square matrix 723 | */ 724 | matx_t ones(const int size); 725 | 726 | /** 727 | * Horizontally stack matrices A and B 728 | * 729 | * @param A Matrix A 730 | * @param B Matrix B 731 | * @returns Stacked matrix 732 | */ 733 | matx_t hstack(const matx_t &A, const matx_t &B); 734 | 735 | /** 736 | * Vertically stack matrices A and B 737 | * 738 | * @param A Matrix A 739 | * @param B Matrix B 740 | * @returns Stacked matrix 741 | */ 742 | matx_t vstack(const matx_t &A, const matx_t &B); 743 | 744 | /** 745 | * Diagonally stack matrices A and B 746 | * 747 | * @param A Matrix A 748 | * @param B Matrix B 749 | * @returns Stacked matrix 750 | */ 751 | matx_t dstack(const matx_t &A, const matx_t &B); 752 | 753 | /** 754 | * Skew symmetric-matrix 755 | * 756 | * @param w Input vector 757 | * @returns Skew symmetric matrix 758 | */ 759 | mat3_t skew(const vec3_t &w); 760 | 761 | /** 762 | * Skew symmetric-matrix squared 763 | * 764 | * @param w Input vector 765 | * @returns Skew symmetric matrix squared 766 | */ 767 | mat3_t skewsq(const vec3_t &w); 768 | 769 | /** 770 | * Enforce Positive Semi-Definite 771 | * 772 | * @param A Input matrix 773 | * @returns Positive semi-definite matrix 774 | */ 775 | matx_t enforce_psd(const matx_t &A); 776 | 777 | /** 778 | * Null-space of A 779 | * 780 | * @param A Input matrix 781 | * @returns Null space of A 782 | */ 783 | matx_t nullspace(const matx_t &A); 784 | 785 | /** 786 | * Load std::vector of real_ts to an Eigen::Matrix 787 | * 788 | * @param[in] x Matrix values 789 | * @param[in] rows Number of matrix rows 790 | * @param[in] cols Number of matrix colums 791 | * @param[out] y Output matrix 792 | */ 793 | void load_matrix(const std::vector &x, 794 | const int rows, 795 | const int cols, 796 | matx_t &y); 797 | 798 | /** 799 | * Load an Eigen::Matrix into a std::vector of real_ts 800 | * 801 | * @param[in] A Matrix 802 | * @param[out] x Output vector of matrix values 803 | */ 804 | void load_matrix(const matx_t A, std::vector &x); 805 | 806 | /** 807 | * Perform Schur's Complement 808 | */ 809 | void schurs_complement(const matx_t &H, const vecx_t &b, 810 | const size_t m, const size_t r, 811 | matx_t &H_marg, vecx_t &b_marg, 812 | const bool precond=false, const bool debug=false); 813 | 814 | /****************************************************************************** 815 | * Geometry 816 | *****************************************************************************/ 817 | 818 | /** 819 | * Sinc function. 820 | */ 821 | real_t sinc(const real_t x); 822 | 823 | /** 824 | * Degrees to radians 825 | * 826 | * @param[in] d Degree to be converted 827 | * @return Degree in radians 828 | */ 829 | real_t deg2rad(const real_t d); 830 | 831 | /** 832 | * Degrees to radians 833 | * 834 | * @param[in] d Degree to be converted 835 | * @return Degree in radians 836 | */ 837 | vec3_t deg2rad(const vec3_t d); 838 | 839 | /** 840 | * Radians to degree 841 | * 842 | * @param[in] r Radian to be converted 843 | * @return Radian in degrees 844 | */ 845 | real_t rad2deg(const real_t r); 846 | 847 | /** 848 | * Radians to degree 849 | * 850 | * @param[in] r Radian to be converted 851 | * @return Radian in degrees 852 | */ 853 | vec3_t rad2deg(const vec3_t &r); 854 | 855 | /** 856 | * Wrap angle in degrees to 180 857 | * 858 | * @param[in] d Degrees 859 | * @return Angle wraped to 180 860 | */ 861 | real_t wrap180(const real_t d); 862 | 863 | /** 864 | * Wrap angle in degrees to 360 865 | * 866 | * @param[in] d Degrees 867 | * @return Angle wraped to 360 868 | */ 869 | real_t wrap360(const real_t d); 870 | 871 | /** 872 | * Wrap angle in radians to PI 873 | * 874 | * @param[in] r Radians 875 | * @return Angle wraped to PI 876 | */ 877 | real_t wrapPi(const real_t r); 878 | 879 | /** 880 | * Wrap angle in radians to 2 PI 881 | * 882 | * @param[in] r Radians 883 | * @return Angle wraped to 2 PI 884 | */ 885 | real_t wrap2Pi(const real_t r); 886 | 887 | /** 888 | * Create a circle point of radius `r` at angle `theta` radians. 889 | */ 890 | vec2_t circle(const real_t r, const real_t theta); 891 | 892 | /** 893 | * Create the sphere point with sphere radius `rho` at longitude `theta` 894 | * [radians] and latitude `phi` [radians]. 895 | */ 896 | vec3_t sphere(const real_t rho, const real_t theta, const real_t phi); 897 | 898 | /** 899 | * Create look at matrix. 900 | */ 901 | mat4_t lookat(const vec3_t &cam_pos, 902 | const vec3_t &target, 903 | const vec3_t &up_axis = vec3_t{0.0, -1.0, 0.0}); 904 | 905 | /** 906 | * Cross-Track error based on waypoint line between p1, p2, and robot position 907 | * 908 | * @param[in] p1 Waypoint 1 909 | * @param[in] p2 Waypoint 2 910 | * @param[in] pos Robot position 911 | * @return Cross track error 912 | */ 913 | real_t cross_track_error(const vec2_t &p1, const vec2_t &p2, const vec2_t &pos); 914 | 915 | /** 916 | * Check if point `pos` is left or right of line formed by `p1` and `p2` 917 | * 918 | * @param[in] p1 Waypoint 1 919 | * @param[in] p2 Waypoint 2 920 | * @param[in] pos Robot position 921 | * @returns 922 | * - 1: Point is left of waypoint line formed by `p1` and `p2` 923 | * - 2: Point is right of waypoint line formed by `p1` and `p2` 924 | * - 0: Point is colinear with waypoint line formed by `p1` and `p2` 925 | */ 926 | int point_left_right(const vec2_t &p1, const vec2_t &p2, const vec2_t &pos); 927 | 928 | /** 929 | * Calculate closest point given waypoint line between `p1`, `p2` and robot 930 | * position 931 | * 932 | * @param[in] p1 Waypoint 1 933 | * @param[in] p2 Waypoint 2 934 | * @param[in] p3 Robot position 935 | * @param[out] closest Closest point 936 | * @returns 937 | * Unit number denoting where the closest point is on waypoint line. For 938 | * example, a return value of 0.5 denotes the closest point is half-way 939 | * (50%) of the waypoint line, alternatively a negative number denotes the 940 | * closest point is behind the first waypoint. 941 | */ 942 | real_t closest_point(const vec2_t &p1, 943 | const vec2_t &p2, 944 | const vec2_t &p3, 945 | vec2_t &closest); 946 | 947 | #define EARTH_RADIUS_M 6378137.0 948 | 949 | /** 950 | * Calculate new latitude and logitude coordinates with an offset in North and 951 | * East direction. 952 | * 953 | * IMPORTANT NOTE: This function is only an approximation. As such do not rely 954 | * on this function for precise latitude, longitude offsets. 955 | * 956 | * @param lat_ref Latitude of origin (decimal format) 957 | * @param lon_ref Longitude of origin (decimal format) 958 | * @param offset_N Offset in North direction (meters) 959 | * @param offset_E Offset in East direction (meters) 960 | * @param lat_new New latitude (decimal format) 961 | * @param lon_new New longitude (decimal format) 962 | */ 963 | void latlon_offset(real_t lat_ref, 964 | real_t lon_ref, 965 | real_t offset_N, 966 | real_t offset_E, 967 | real_t *lat_new, 968 | real_t *lon_new); 969 | 970 | /** 971 | * Calculate difference in distance in North and East from two GPS coordinates 972 | * 973 | * IMPORTANT NOTE: This function is only an approximation. As such do not rely 974 | * on this function for precise latitude, longitude diffs. 975 | * 976 | * @param lat_ref Latitude of origin (decimal format) 977 | * @param lon_ref Longitude of origin (decimal format) 978 | * @param lat Latitude of point of interest (decimal format) 979 | * @param lon Longitude of point of interest (decimal format) 980 | * @param dist_N Distance of point of interest in North axis [m] 981 | * @param dist_E Distance of point of interest in East axis [m] 982 | */ 983 | void latlon_diff(real_t lat_ref, 984 | real_t lon_ref, 985 | real_t lat, 986 | real_t lon, 987 | real_t *dist_N, 988 | real_t *dist_E); 989 | 990 | /** 991 | * Calculate Euclidean distance between two GPS coordintes 992 | * 993 | * IMPORTANT NOTE: This function is only an approximation. As such do not rely 994 | * on this function for precise latitude, longitude distance. 995 | * 996 | * @param lat_ref Latitude of origin (decimal format) 997 | * @param lon_ref Longitude of origin (decimal format) 998 | * @param lat Latitude of point of interest (decimal format) 999 | * @param lon Longitude of point of interest (decimal format) 1000 | * 1001 | * @returns Euclidean distance between two GPS coordinates [m] 1002 | */ 1003 | real_t latlon_dist(real_t lat_ref, real_t lon_ref, real_t lat, real_t lon); 1004 | 1005 | /***************************************************************************** 1006 | * DIFFERENTIAL GEOMETRY 1007 | *****************************************************************************/ 1008 | 1009 | namespace lie { 1010 | 1011 | mat3_t Exp(const vec3_t &phi); 1012 | vec3_t Log(const mat3_t &C); 1013 | mat3_t Jr(const vec3_t &psi); 1014 | 1015 | } // namespace lie 1016 | 1017 | 1018 | /****************************************************************************** 1019 | * STATISTICS 1020 | *****************************************************************************/ 1021 | 1022 | /** 1023 | * Create random integer 1024 | * 1025 | * @param[in] ub Upper bound 1026 | * @param[in] lb Lower bound 1027 | * @return Random integer 1028 | */ 1029 | int randi(const int ub, const int lb); 1030 | 1031 | /** 1032 | * Create random real_t 1033 | * 1034 | * @param[in] ub Upper bound 1035 | * @param[in] lb Lower bound 1036 | * @return Random floating point 1037 | */ 1038 | real_t randf(const real_t ub, const real_t lb); 1039 | 1040 | /** 1041 | * Calculate median given an array of numbers 1042 | * 1043 | * @param[in] v Array of numbers 1044 | * @return Median of given array 1045 | */ 1046 | real_t median(const std::vector &v); 1047 | 1048 | /** 1049 | * Mean vector 1050 | * 1051 | * @param[in] x List of vectors 1052 | * @return Mean vector 1053 | */ 1054 | vec3_t mean(const vec3s_t &x); 1055 | 1056 | /** 1057 | * Shannon Entropy of a given covariance matrix `covar`. 1058 | */ 1059 | real_t shannon_entropy(const matx_t &covar); 1060 | 1061 | /** 1062 | * Multivariate normal. 1063 | */ 1064 | vec3_t mvn(std::default_random_engine &engine, 1065 | const vec3_t &mu = vec3_t{0.0, 0.0, 0.0}, 1066 | const vec3_t &stdev = vec3_t{1.0, 1.0, 1.0}); 1067 | 1068 | /** 1069 | * Gassian normal. 1070 | * http://c-faq.com/lib/gaussian.html 1071 | */ 1072 | real_t gauss_normal(); 1073 | 1074 | /***************************************************************************** 1075 | * TRANSFORM 1076 | *****************************************************************************/ 1077 | 1078 | /** 1079 | * Form a 4x4 homogeneous transformation matrix from a 1080 | * rotation matrix `C` and translation vector `r`. 1081 | */ 1082 | template 1083 | Eigen::Matrix tf(const Eigen::Matrix &C, 1084 | const Eigen::Matrix &r) { 1085 | Eigen::Matrix transform = Eigen::Matrix::Identity(); 1086 | transform.block(0, 0, 3, 3) = C; 1087 | transform.block(0, 3, 3, 1) = r; 1088 | return transform; 1089 | } 1090 | 1091 | /** 1092 | * Form a 4x4 homogeneous transformation matrix from a pointer to real_t array 1093 | * containing (quaternion + translation) 7 elements: (qw, qx, qy, qz, x, y, z) 1094 | */ 1095 | mat4_t tf(const vecx_t ¶ms); 1096 | 1097 | /** 1098 | * Form a 4x4 homogeneous transformation matrix from a 1099 | * rotation matrix `C` and translation vector `r`. 1100 | */ 1101 | mat4_t tf(const mat3_t &C, const vec3_t &r); 1102 | 1103 | /** 1104 | * Form a 4x4 homogeneous transformation matrix from a 1105 | * Hamiltonian quaternion `q` and translation vector `r`. 1106 | */ 1107 | mat4_t tf(const quat_t &q, const vec3_t &r); 1108 | 1109 | /** 1110 | * Extract rotation from transform 1111 | */ 1112 | inline mat3_t tf_rot(const mat4_t &tf) { return tf.block<3, 3>(0, 0); } 1113 | 1114 | /** 1115 | * Extract rotation and convert to quaternion from transform 1116 | */ 1117 | inline quat_t tf_quat(const mat4_t &tf) { return quat_t{tf.block<3, 3>(0, 0)}; } 1118 | 1119 | /** 1120 | * Extract translation from transform 1121 | */ 1122 | inline vec3_t tf_trans(const mat4_t &tf) { return tf.block<3, 1>(0, 3); } 1123 | 1124 | 1125 | /** 1126 | * Perturb the rotation element in the tranform `T` by `step_size` at index 1127 | * `i`. Where i = 0 for x-axis, i = 1 for y-axis, and i = 2 for z-axis. 1128 | */ 1129 | mat4_t tf_perturb_rot(const mat4_t &T, real_t step_size, const int i); 1130 | 1131 | /** 1132 | * Perturb the translation element in the tranform `T` by `step_size` at index 1133 | * `i`. Where i = 0 for x-axis, i = 1 for y-axis, and i = 2 for z-axis. 1134 | */ 1135 | mat4_t tf_perturb_trans(const mat4_t &T, real_t step_size, const int i); 1136 | 1137 | /** 1138 | * Transform point `p` with transform `T`. 1139 | */ 1140 | vec3_t tf_point(const mat4_t &T, const vec3_t &p); 1141 | 1142 | /** 1143 | * Rotation matrix around x-axis (counter-clockwise, right-handed). 1144 | * @returns Rotation matrix 1145 | */ 1146 | mat3_t rotx(const real_t theta); 1147 | 1148 | /** 1149 | * Rotation matrix around y-axis (counter-clockwise, right-handed). 1150 | * @returns Rotation matrix 1151 | */ 1152 | mat3_t roty(const real_t theta); 1153 | 1154 | /** 1155 | * Rotation matrix around z-axis (counter-clockwise, right-handed). 1156 | * @returns Rotation matrix 1157 | */ 1158 | mat3_t rotz(const real_t theta); 1159 | 1160 | /** 1161 | * Convert euler sequence 123 to rotation matrix R 1162 | * This function assumes we are performing a body fixed intrinsic rotation. 1163 | * 1164 | * Source: 1165 | * 1166 | * Kuipers, Jack B. Quaternions and Rotation Sequences: A Primer with 1167 | * Applications to Orbits, Aerospace, and Virtual Reality. Princeton, N.J: 1168 | * Princeton University Press, 1999. Print. 1169 | * 1170 | * Page 86. 1171 | * 1172 | * @returns Rotation matrix 1173 | */ 1174 | mat3_t euler123(const vec3_t &euler); 1175 | 1176 | /** 1177 | * Convert euler sequence 321 to rotation matrix R 1178 | * This function assumes we are performing a body fixed intrinsic rotation. 1179 | * 1180 | * Source: 1181 | * 1182 | * Kuipers, Jack B. Quaternions and Rotation Sequences: A Primer with 1183 | * Applications to Orbits, Aerospace, and Virtual Reality. Princeton, N.J: 1184 | * Princeton University Press, 1999. Print. 1185 | * 1186 | * Page 86. 1187 | * 1188 | * @returns Rotation matrix 1189 | */ 1190 | mat3_t euler321(const vec3_t &euler); 1191 | 1192 | /** 1193 | * Convert roll, pitch and yaw to quaternion. 1194 | */ 1195 | quat_t euler2quat(const vec3_t &euler); 1196 | 1197 | /** 1198 | * Convert rotation vectors to rotation matrix using measured acceleration 1199 | * `a_m` from an IMU and gravity vector `g`. 1200 | */ 1201 | mat3_t vecs2rot(const vec3_t &a_m, const vec3_t &g); 1202 | 1203 | /** 1204 | * Convert rotation vector `rvec` to rotation matrix. 1205 | */ 1206 | mat3_t rvec2rot(const vec3_t &rvec, const real_t eps = 1e-5); 1207 | 1208 | /** 1209 | * Convert quaternion to euler angles. 1210 | */ 1211 | vec3_t quat2euler(const quat_t &q); 1212 | 1213 | /** 1214 | * Convert quaternion to rotation matrix. 1215 | */ 1216 | mat3_t quat2rot(const quat_t &q); 1217 | 1218 | /** 1219 | * Convert small angle euler angle to quaternion. 1220 | */ 1221 | quat_t quat_delta(const vec3_t &dalpha); 1222 | 1223 | /** 1224 | * Return left quaternion product matrix. 1225 | */ 1226 | mat4_t quat_lmul(const quat_t &q); 1227 | 1228 | /** 1229 | * Return left quaternion product matrix (but only for x, y, z components). 1230 | */ 1231 | mat3_t quat_lmul_xyz(const quat_t &q); 1232 | 1233 | /** 1234 | * Return right quaternion product matrix. 1235 | */ 1236 | mat4_t quat_rmul(const quat_t &q); 1237 | 1238 | /** 1239 | * Return right quaternion product matrix (but only for x, y, z components). 1240 | */ 1241 | mat3_t quat_rmul_xyz(const quat_t &q); 1242 | 1243 | /** 1244 | * Return only the x, y, z, components of a quaternion matrix. 1245 | */ 1246 | mat3_t quat_mat_xyz(const mat4_t &Q); 1247 | 1248 | /** 1249 | * Initialize attitude using IMU gyroscope `w_m` and accelerometer `a_m` 1250 | * measurements. The calculated attitude outputted into to `C_WS`. Note: this 1251 | * function does not calculate initial yaw angle in the world frame. Only the 1252 | * roll, and pitch are inferred from IMU measurements. 1253 | */ 1254 | void imu_init_attitude(const vec3s_t w_m, 1255 | const vec3s_t a_m, 1256 | mat3_t &C_WS, 1257 | const size_t buffer_size = 50); 1258 | 1259 | /***************************************************************************** 1260 | * TIME 1261 | *****************************************************************************/ 1262 | 1263 | /** 1264 | * Print timestamp. 1265 | */ 1266 | void timestamp_print(const timestamp_t &ts, const std::string &prefix = ""); 1267 | 1268 | /** 1269 | * Convert ts to second. 1270 | */ 1271 | real_t ts2sec(const timestamp_t &ts); 1272 | 1273 | /** 1274 | * Convert nano-second to second. 1275 | */ 1276 | real_t ns2sec(const uint64_t ns); 1277 | 1278 | /** 1279 | * Start timer. 1280 | */ 1281 | struct timespec tic(); 1282 | 1283 | /** 1284 | * Stop timer and return number of seconds. 1285 | */ 1286 | float toc(struct timespec *tic); 1287 | 1288 | /** 1289 | * Stop timer and return miliseconds elasped. 1290 | */ 1291 | float mtoc(struct timespec *tic); 1292 | 1293 | /** 1294 | * Get time now in milliseconds since epoch 1295 | */ 1296 | real_t time_now(); 1297 | 1298 | /***************************************************************************** 1299 | * BA DATA 1300 | ****************************************************************************/ 1301 | 1302 | struct pose_t { 1303 | vec_t<7> param; 1304 | 1305 | pose_t(); 1306 | pose_t(const mat4_t &T); 1307 | 1308 | quat_t rot() const; 1309 | vec3_t trans() const; 1310 | mat4_t T() const; 1311 | 1312 | quat_t rot(); 1313 | vec3_t trans(); 1314 | mat4_t T(); 1315 | 1316 | void set_trans(const vec3_t &r); 1317 | void set_rot(const quat_t &q); 1318 | void set_rot(const mat3_t &C); 1319 | }; 1320 | typedef std::vector poses_t; 1321 | typedef std::vector keypoints_t; 1322 | 1323 | void pose_print(const std::string &prefix, const pose_t &pose); 1324 | 1325 | poses_t load_poses(const std::string &csv_path); 1326 | keypoints_t parse_keypoints_line(const char *line); 1327 | std::vector load_keypoints(const std::string &data_path); 1328 | void keypoints_print(const keypoints_t &keypoints); 1329 | poses_t load_poses(const std::string &csv_path); 1330 | keypoints_t parse_keypoints_line(const char *line); 1331 | std::vector load_keypoints(const std::string &data_path); 1332 | void keypoints_print(const keypoints_t &keypoints); 1333 | mat3_t load_camera(const std::string &data_path); 1334 | poses_t load_camera_poses(const std::string &data_path); 1335 | poses_t load_target_pose(const std::string &data_path); 1336 | real_t **load_points(const std::string &data_path, int *nb_points); 1337 | int **load_point_ids(const std::string &data_path, int *nb_points); 1338 | --------------------------------------------------------------------------------