//
// 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;
}
