icp.h

Go to the documentation of this file.

#ifndef _H_ICP_
#define _H_ICP_

#include <gpc/gpc.h>
#include "../csm_all.h"


void sm_icp(struct sm_params*params, struct sm_result*res);

void sm_icp_xy(struct sm_params*params, struct sm_result*res);

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

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

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

void find_correspondences(struct sm_params*params);
void find_correspondences_tricks(struct sm_params*params);
        void debug_correspondences(struct sm_params * params);

void kill_outliers_trim(struct sm_params*params, double*total_error);
void kill_outliers_double(struct sm_params*params);
        
void compute_covariance_exact(
        LDP laser_ref, LDP laser_sens, const gsl_vector*x,
                val *cov0_x, val *dx_dy1, val *dx_dy2);

void visibilityTest(LDP ld, const gsl_vector*x_old);

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

void swap_double(double*a,double*b);

#endif