icp_debug.c
Go to the documentation of this file.
00001 #include <string.h>
00002 #include "icp.h"
00003 
00004 void debug_correspondences(struct sm_params * params) {
00005         LDP laser_sens = params->laser_sens;
00007         find_correspondences_tricks(params);
00008         struct correspondence c1[laser_sens->nrays];
00009         struct correspondence * c2 = laser_sens->corr;
00010         memcpy(c1, c2, sizeof(struct correspondence) * laser_sens->nrays);
00011         long hash1 = ld_corr_hash(laser_sens);
00012         find_correspondences(params);
00013         long hash2 = ld_corr_hash(laser_sens);
00014         if(hash1 != hash2) {
00015                 sm_error("find_correspondences_tricks might be buggy\n");
00016                 int i = 0; for(i=0;i<laser_sens->nrays;i++) {
00017                         if( (c1[i].valid != c2[i].valid) ||
00018                                 (c1[i].j1 != c2[i].j1) || (c1[i].j2 != c2[i].j2) ) {
00019                                         sm_error("\t   tricks: c1[%d].valid = %d j1 = %d  j2 = %d  dist2_j1 = %f\n",
00020                                                 i, c1[i].valid, c1[i].j1, c1[i].j2, c1[i].dist2_j1);
00021                                         sm_error("\tno tricks: c2[%d].valid = %d j1 = %d  j2 = %d  dist2_j1 = %f\n",
00022                                                 i, c2[i].valid, c2[i].j1, c2[i].j2, c2[i].dist2_j1);
00023                                 }
00024                 }
00025                 if(1) exit(-1);
00026         }
00027 }


csm
Author(s): Andrea Censi
autogenerated on Mon Jan 16 2017 03:48:29