// // This calls the procedures in the other files which do all the real work. // If you wanted to not use hierarchical SLAM, you could remove all references here to High*, and make // certain to set LOW_DURATION in low.h to some incredibly high number. // void *Slam(void *a) { TPath *path, *trashPath; TSenseLog *obs, *trashObs; InitHighSlam(); InitLowSlam(); while (continueSlam) { LowSlam(continueSlam, &path, &obs); HighSlam(path, obs); // Get rid of the path and log of observations while (path != NULL) { trashPath = path; path = path->next; free(trashPath); } while (obs != NULL) { trashObs = obs; obs = obs->next; free(trashObs); } } CloseLowSlam(); return NULL; }
// // This calls the procedures in the other files which do all the real work. // If you wanted to not use hierarchical SLAM, you could remove all references here to High*, and make // certain to set LOW_DURATION in low.h to some incredibly high number. // void *Slam(void * /*arg unused */) { TPath *path, *trashPath; TSenseLog *obs, *trashObs; double oldmC_D = 0; double oldmC_T = 0; double oldvC_D = 0; double oldvC_T = 0; double oldmD_D = 0; double oldmD_T = 0; double oldvD_D = 0; double oldvD_T = 0; double oldmT_D = 0; double oldmT_T = 0; double oldvT_D = 0; double oldvT_T = 0; outFile = fopen("motionModel.txt", "w"); fprintf(outFile, "This file lists the motion model parameters in the following order : \n"); fprintf(outFile, " - mean value from observed translation\n"); fprintf(outFile, " - mean value from observed rotation\n"); fprintf(outFile, " - variance contributed from observed translation\n"); fprintf(outFile, " - variance contributed from observed rotation\n"); fprintf(outFile, "\n"); fprintf(outFile, "------------Intermediate Models------------------\n"); fprintf(outFile, " m_dist m_turn v_dist v_turn\n"); fprintf(outFile, "C: %.4f %.4f %.4f %.4f\n", meanC_D, meanC_T, varC_D, varC_T); fprintf(outFile, "D: %.4f %.4f %.4f %.4f\n", meanD_D, meanD_T, varD_D, varD_T); fprintf(outFile, "T: %.4f %.4f %.4f %.4f\n", meanT_D, meanT_T, varT_D, varT_T); fprintf(outFile, "\n"); fclose(outFile); while ((fabs(oldmC_D-meanC_D) > 0.01)||(fabs(oldmC_T-meanC_T) > 0.03)||(fabs(oldvC_D-varC_D) > 0.01)||(fabs(oldvC_T-varC_T) > 0.1) || (fabs(oldmD_D-meanD_D) > 0.01)||(fabs(oldmD_T-meanD_T) > 0.03)||(fabs(oldvD_D-varD_D) > 0.01)||(fabs(oldvD_T-varD_T) > 0.1) || (fabs(oldmT_D-meanT_D) > 0.03)||(fabs(oldmT_T-meanT_T) > 0.01)||(fabs(oldvT_D-varT_D) > 0.03)||(fabs(oldvT_T-varT_T) > 0.01)) { readFile = carmen_fopen(PLAYBACK, "r"); if(readFile == NULL) carmen_die("Error: could not open file %s for reading.\n", PLAYBACK); logfile_index = carmen_logfile_index_messages(readFile); InitLowSlam(); LowSlam(&path, &obs); carmen_fclose(readFile); oldmC_D = meanC_D; oldmC_T = meanC_T; oldvC_D = varC_D; oldvC_T = varC_T; oldmD_D = meanD_D; oldmD_T = meanD_T; oldvD_D = varD_D; oldvD_T = varD_T; oldmT_D = meanT_D; oldmT_T = meanT_T; oldvT_D = varT_D; oldvT_T = varT_T; outFile = fopen("motionModel.txt", "a"); Learn(path); fclose(outFile); // Get rid of the path and log of observations while (path != NULL) { trashPath = path; path = path->next; free(trashPath); } while (obs != NULL) { trashObs = obs; obs = obs->next; free(trashObs); } } outFile = fopen("motionModel.txt", "a"); fprintf(outFile, "\n"); fprintf(outFile, "------------Final Model------------------\n"); fprintf(outFile, " m_dist m_turn v_dist v_turn\n"); fprintf(outFile, "C: %.4f %.4f %.4f %.4f\n", meanC_D, meanC_T, varC_D, varC_T); fprintf(outFile, "D: %.4f %.4f %.4f %.4f\n", meanD_D, meanD_T, varD_D, varD_T); fprintf(outFile, "T: %.4f %.4f %.4f %.4f\n", meanT_D, meanT_T, varT_D, varT_T); fprintf(outFile, "\n"); fprintf(outFile, "#define meanC_D %.4f\n#define meanC_T %.4f\n#define varC_D %.4f\n#define varC_T %.4f\n\n", meanC_D, meanC_T, varC_D, varC_T); fprintf(outFile, "#define meanD_D %.4f\n#define meanD_T %.4f\n#define varD_D %.4f\n#define varD_T %.4f\n\n", meanD_D, meanD_T, varD_D, varD_T); fprintf(outFile, "#define meanT_D %.4f\n#define meanT_T %.4f\n#define varT_D %.4f\n#define varT_T %.4f\n\n", meanT_D, meanT_T, varT_D, varT_T); fclose(outFile); CloseLowSlam(); return NULL; }