by xipengwang

xipengwang / AprilSAM

SLAM optimization algorithm

148 Stars 44 Forks Last release: Not found GNU Lesser General Public License v2.1 6 Commits 0 Releases

Available items

No Items, yet!

The developer of this repository has not created any items for sale yet. Need a bug fixed? Help with integration? A different license? Create a request here:

AprilSAM: Real-time Smoothing and Mapping

Copyright APRIL Lab.


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


A basic AprilSAM application can be seen in

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));
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);

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);

An example of saving and loading graph file can be seen in

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)

An example of saving and retrieving graph/node/factor attributes can be seen in

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)

An example of evaluting AprilSAM on Manhattan3500 dataset can be seen in

Test aprilsam optimization (Load an AprilSAM graph file):


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

We use cookies. If you continue to browse the site, you agree to the use of cookies. For more information on our use of cookies please see our Privacy Policy.