Skip to content

xipengwang/AprilSAM

Folders and files

NameName
Last commit message
Last commit date

Latest commit

b7a3a51 · Jul 17, 2021

History

7 Commits
Feb 12, 2019
Jul 19, 2018
Feb 12, 2019
Jul 19, 2018
Jun 29, 2018
Oct 18, 2018
Jun 29, 2018
Jul 17, 2021
Jun 29, 2018
Jun 29, 2018

Repository files navigation

AprilSAM: Real-time Smoothing and Mapping

Paper: AprilSAM: Real-time Smoothing and Mapping

Video

Tutorial NoteBook

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

Copyright APRIL Lab.

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

About

SLAM optimization algorithm

Topics

Resources

License

Stars

Watchers

Forks

Releases

No releases published

Packages

No packages published

Languages