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 }