Functions
icp_loop.c File Reference
#include <math.h>
#include <string.h>
#include <gsl/gsl_matrix.h>
#include <gpc/gpc.h>
#include <egsl/egsl_macros.h>
#include "../csm_all.h"
#include "icp.h"
Include dependency graph for icp_loop.c:

Go to the source code of this file.

Functions

int compute_next_estimate (struct sm_params *params, const double x_old[3], double x_new[3])
 
int icp_loop (struct sm_params *params, const double *q0, double *x_new, double *total_error, int *valid, int *iterations)
 
int termination_criterion (struct sm_params *params, const double *delta)
 

Function Documentation

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.

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.

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.



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