Functions
icp.h File Reference
#include <gpc/gpc.h>
#include "../csm_all.h"
Include dependency graph for icp.h:
This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Functions

void compute_covariance_exact (LDP laser_ref, LDP laser_sens, const gsl_vector *x, val *cov0_x, val *dx_dy1, val *dx_dy2)
 
int compute_next_estimate (struct sm_params *params, const double x_old[3], double x_new[3])
 
void debug_correspondences (struct sm_params *params)
 
void find_correspondences (struct sm_params *params)
 
void find_correspondences_tricks (struct sm_params *params)
 
int icp_loop (struct sm_params *params, const double *q0, double *x_new, double *total_error, int *nvalid, int *iterations)
 
void kill_outliers_double (struct sm_params *params)
 
void kill_outliers_trim (struct sm_params *params, double *total_error)
 
void ld_invalid_if_outside (LDP ld, double min_reading, double max_reading)
 
void sm_icp (struct sm_params *params, struct sm_result *res)
 
void sm_icp_xy (struct sm_params *params, struct sm_result *res)
 
void swap_double (double *a, double *b)
 
int termination_criterion (struct sm_params *params, const double *delta)
 
void visibilityTest (LDP ld, const gsl_vector *x_old)
 

Function Documentation

void compute_covariance_exact ( LDP  laser_ref,
LDP  laser_sens,
const gsl_vector *  x,
val cov0_x,
val dx_dy1,
val dx_dy2 
)

Definition at line 13 of file icp_covariance.c.

int compute_next_estimate ( struct sm_params params,
const double  x_old[3],
double  x_new[3] 
)

This is the beef: computing in closed form the next estimate given the correspondences.

TODO: here we could use the estimated alpha

Incidence of the ray Note that alpha is relative to the first scan (not the world) and that pose_theta is the angle of the second scan with respect to the first, hence it's ok.

Definition at line 171 of file icp_loop.c.

void debug_correspondences ( struct sm_params params)

Checks that find_correspondences_tricks and find_correspondences behave the same. Exit(-1) on error.

Do the test

Definition at line 4 of file icp_debug.c.

void find_correspondences ( struct sm_params params)

Naif algorithm

Definition at line 30 of file icp_corr_dumb.c.

void find_correspondences_tricks ( struct sm_params params)

Smart algorithm

Search domain for j1

Current best match

and his distance

If last match was succesful, then start at that index + 1

Definition at line 56 of file icp_corr_tricks.c.

int icp_loop ( struct sm_params params,
const double *  q0,
double *  x_new,
double *  total_error,
int *  nvalid,
int *  iterations 
)

This is the meat

Compute laser_sens's points in laser_ref's coordinates by roto-translating by x_old

Find correspondences (the naif or smart way)

If debug_verify_tricks, make sure that find_correspondences_tricks() and find_correspondences() return the same answer

Checks for oscillations

PLICP terminates in a finite number of steps!

Definition at line 13 of file icp_loop.c.

void kill_outliers_double ( struct sm_params params)

If multiple points in laser_sens match to the same point in laser_ref, only the nearest one wins.

Uses: laser_sens->corr, laser_sens->p Modifies: laser_sens->corr

Definition at line 42 of file icp_outliers.c.

void kill_outliers_trim ( struct sm_params params,
double *  total_error 
)

Trims the corrispondences using an adaptive algorithm

Assumes cartesian coordinates computed. (points and points_w)

So, to disable this: outliers_maxPerc = 1 outliers_adaptive_order = 1

Definition at line 83 of file icp_outliers.c.

void ld_invalid_if_outside ( LDP  ld,
double  min_reading,
double  max_reading 
)

Marks a ray invalid if reading is outside range [min_reading, max_reading].

Definition at line 19 of file icp.c.

void sm_icp ( struct sm_params params,
struct sm_result res 
)

This sets the stage.

Mark as invalid the rays outside of (min_reading, max_reading]

Definition at line 29 of file icp.c.

void sm_icp_xy ( struct sm_params params,
struct sm_result res 
)

Mark as invalid the rays outside of (min_reading, max_reading]

Definition at line 198 of file icp.c.

void swap_double ( double *  a,
double *  b 
)

Definition at line 181 of file icp_outliers.c.

int termination_criterion ( struct sm_params params,
const double *  delta 
)

This termination criterium use epsilon_xy and epsilon_th. It is useless when using the point-to-line-distance; however, we put it here because one can choose to use the point-to-point distance.

Definition at line 165 of file icp_loop.c.

void visibilityTest ( LDP  laser_ref,
const gsl_vector *  u 
)

expects cartesian valid

Definition at line 9 of file icp_outliers.c.



csm
Author(s): Andrea Censi
autogenerated on Tue May 11 2021 02:18:23