Robotics C
Need help with AprilSAM?
Click the “chat” button below for chat support from the developer who created it, or find similar developers for support.
xipengwang

Description

SLAM optimization algorithm

155 Stars 45 Forks GNU Lesser General Public License v2.1 6 Commits 2 Opened issues

Services available

Need anything else?

AprilSAM: Real-time Smoothing and Mapping

Copyright APRIL Lab.

https://april.eecs.umich.edu/

AprilSAM: Real-time Smoothing and Mapping

Video

INSTALL

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

USAGE

A basic AprilSAM application can be seen in
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);

An example of saving and loading graph file can be seen in
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)

An example of saving and retrieving graph/node/factor attributes can be seen in
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)

An example of evaluting AprilSAM on Manhattan3500 dataset can be seen in
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

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.