SLAM optimization algorithm
Copyright APRIL Lab.
https://april.eecs.umich.edu/
AprilSAM: Real-time Smoothing and Mapping
The default installation will place headers in /usr/local/include and shared library in /usr/local/lib. It also installs a pkg-config script into /usr/local/lib/pkgconfig.
$ make $ sudo make install
example/aprilsam_tutorial.c.
Create a graph:
april_graph_t *graph = april_graph_create();
Initialize optimize parameters:
struct april_graph_cholesky_param *chol_param = calloc(1,sizeof(april_graph_cholesky_param_t)); april_graph_cholesky_param_init(chol_param); chol_param->show_timing = 0; // chol_param->delta_xy = 0.1; chol_param->delta_theta = 0.1; chol_param->nthreshold = 100;
Add xyt node:
double ninit[3] = { 0,0,0 }; double nstate[3] = { 0,0,0 }; double ntruth[3] = { 0,0,0 }; april_graph_node_t *node = april_graph_node_xyt_create(nstate, ninit, ntruth); node->UID = zarray_size(graph->nodes); zarray_add(graph->nodes, &node);
Add xyt factor:
april_graph_node_t *na, *nb*; zarray_get(graph->nodes, aidx, &na); zarray_get(graph->nodes, bidx, &na); matd_t *W = matd_create(3,3); MATD_EL(W, 0, 0) = 1.0 / pow(0.1, 2); MATD_EL(W, 1, 1) = 1.0 / pow(0.1, 2); MATD_EL(W, 2, 2) = 1.0 / pow(to_radians(1), 2); april_graph_factor_t *factor = april_graph_factor_xyt_create(na->UID, nb->UID, z, NULL, W); zarray_add(graph->factors, &factor); matd_destroy(W);
Cholesky optimization:
void april_graph_cholesky(graph, chol_param);
AprilSAM increnmetal optimization:
void april_graph_cholesky_inc(graph, chol_param);
Graph Chi^2 error: (f(x)-z)^T \Sigma^-1 (f(x)-z)
double april_graph_chi2(graph);
example/aprilsam_graph_save_simple.c
Save a graph:
void april_graph_save(april_graph_t *graph, const char *path)
Load a graph:
april_graph_t* april_graph_create_from_file(const char *path)
example/aprilsam_graph_save_with_attributes.c
Add attributes:
void april_graph_attr_put(april_graph_t *graph, const stype_t *type, const char *key, void *data) void april_graph_node_attr_put(april_graph_node_t *node, const stype_t *type, const char *key, void *data) void april_graph_factor_attr_put(april_graph_factor_t *factor, const stype_t *type, const char *key, void *data)
Get attributes:
void* april_graph_attr_get(april_graph_t * graph, const char * key) void* april_graph_node_attr_get(april_graph_t * graph, const char * key) void* april_graph_factor_attr_get(april_graph_t * graph, const char * key)
example/aprilsam_demo.c
Test aprilsam optimization (Load an AprilSAM graph file):
./aprilsam_demo
Test aprilsam optimization (Load a text file):
./aprilsam_demo --datapath ../data/M3500.txt
Test batch update only step by step optimization:
./aprilsam_demo --datapath ../data/M3500.txt --batch_update_only