void RobotEnvironment_dtor (RobotEnvironment *r)
{
freeCompressedImage ((void*)&r->mapToSubmit);
freeCompressedImage ((void*)&r->mapFromBoard);
List_clear (&r->unreachableIkDests, 1);
List_clear (&r->unreachableIkTargets, 1);
Image_dtor (&r->navMap);
#if defined(ROBOT) && !defined(IS_GUMSTIX)
Image_dtor (&r->originalSim);
List_clear (&r->originalObstacleList, 1);
#endif
}
void test(){
/*
测试链表函数
测试结果:全部函数运行通过
*/
List A;
int a[]={1,2,3,4};
List B;
int b[]={5,6,7,8,9};
List_Init(&A);
List_Init(&B);
List_creat(&A,a,4);
List_creat(&B,b,5);
printf("\nA:");
List_printf(&A);
printf("\nB:");
List_printf(&B);
List_Insert(&A,1,2);
printf("\nA:");
List_printf(&A);
printf("\nA length:%d",List_getLength(&A));
List_clear(&B);
printf("\nB:");
List_printf(&B);
List_destroy(&B);
}
void List_done(void *list,
ListNodeFreeFunction listNodeFreeFunction,
void *listNodeFreeUserData
)
{
assert(list != NULL);
List_clear(list,listNodeFreeFunction,listNodeFreeUserData);
}
/*==============================================================================
* Name : void List_free(LINK_LIST* list)
* Abstr : Free memory of the link list
* Params : LINK_LIST* list : the link list need to free
* Return :
* Modify :
*=============================================================================*/
void List_free(LINK_LIST* list)
{
/* clear the list */
List_clear(list);
/* free handle */
safe_free((void*)list);
log_debug(log_cat, "list: free success");
}
void *test_list_clear()
{
int rc;
printf("test_list_clear()\n");
printf("-----------------\n");
rc = List_clear(list);
check(!rc, "List_clear return code = %d", rc);
printf("(done.)\n");
return NULL;
error:
return "List_clear failed";
}
void StringList_clear(StringList *stringList)
{
assert(stringList != NULL);
List_clear(stringList,(ListNodeFreeFunction)freeStringNode,NULL);
}
void RobotRecognitionTesting_testRobotRecognition (const int doRecognition)
{
FILE *imgFile;
FILE *outputFile;
int i, nTrainingImages;
Image *img;
Image *displayImg;
CvSize size = {176, 143};
IplImage *iplImage;
int windowSize[] = {100, 10, 450, 450};
char inputDir[256];
float *colourScoreGrid;
float *camScoreGrid;
TrainingData *data;
ImgFeatures imgFeatures;
ImgCellFeatures *cellFeatures;
OccupancyGrid *occupancyGrid;
int dummy;
RobotData robotData[3]; // In normal code, will be taken from groupData
int ownRobotIndex, visRobotIndex, remainingRobotIndex;
int isLocSetInImg;
int anyRobotCells;
CamVectors camVectors;
UncertaintyConstants uncertaintyConstants;
List robotEstimates;
RobotEstimate *robotEstimate;
ListNode *iter;
PointF focalPt, estLoc, error;
#if VERBOSE_BLOB_DETECTION
int nColours;
uchar colours[256];
#endif
Image_ctor (&img, 176, 143, 3);
Image_ctor (&displayImg, 176, 143, 3);
iplImage = cvCreateImage (size, 8, 3);
data = (TrainingData*)malloc (sizeof (TrainingData) * 500);
TrainingData_init (data, 500);
nTrainingImages = RobotRecognitionTraining_setupTrainingData (data);
cellFeatures = (ImgCellFeatures*)malloc (sizeof (ImgCellFeatures) * SIZE_COOP_LOC_GRID);
colourScoreGrid = (float*)malloc (sizeof (float) * N_ROBOT_COLOURS * SIZE_COOP_LOC_GRID);
camScoreGrid = (float*)malloc (sizeof (float) * SIZE_COOP_LOC_GRID);
occupancyGrid = OccupancyGrid_alloc();
// Setup cam params for whatever robot was defined as ROBOT_PLATFORM in
// RobotDefs.h for now, and then overwrite for the observer robot
// specified for each training image
setupCamParams (ROBOT_PLATFORM);
CamVectors_init (&camVectors);
UncertaintyConstants_init (&uncertaintyConstants);
// Setup basic pose params for all robots - used in blob detection
robotData[0].stdDev = 50.0f;
robotData[1].stdDev = 50.0f;
robotData[2].stdDev = 50.0f;
if (doRecognition)
{
outputFile = fopen ("__RobotRecognitionBlobDetectionTestOutput__.txt", "w");
}
for (i = 0; i < nTrainingImages; ++i)
// for (i = 7; i < 8; ++i)
// for (i = 73; i < 74; ++i)
// for (i = 213; i < 214; ++i)
{
sprintf (inputDir, "%s/calibration/images/%s", rootDirName, data[i].imgName);
imgFile = fopen (inputDir, "r");
if (!imgFile)
{
printf ("Error: inputDir %s\n", inputDir);
}
assert (imgFile);
fread (img->data, 1, img->width * img->height * 3, imgFile);
fclose (imgFile);
Image_rectify (img->data);
Image_flip(img->data, CAM_IMG_FLIP_V, CAM_IMG_FLIP_H);
Image_clone (img, displayImg);
Image_toHSV (img->data);
assert (data[i].obs != -1 && data[i].vis != -1);
robotEstimates = initList();
// Set index of observer and set camera params for that robot platform
ownRobotIndex = data[i].obs;
visRobotIndex = data[i].vis;
remainingRobotIndex = RobotRecognitionTesting_getRemainingRobotIndex (ownRobotIndex, visRobotIndex);
setupCamParams (robotPlatformIds[ownRobotIndex]);
CamVectors_init (&camVectors); // Setup cam vectors again once cam params are changed
isLocSetInImg = data[i].isLoc;
if (doRecognition && isLocSetInImg)
{
// Set pose of observer robot, with orient assumed to be 0
robotData[ownRobotIndex].pose.loc.x = data[i].loc.x;
robotData[ownRobotIndex].pose.loc.y = data[i].loc.y;
robotData[ownRobotIndex].pose.orient = 0.0f;
// Set pose of visible robot to origin, but with specified orient
robotData[visRobotIndex].pose.loc.x = 0.0f;
robotData[visRobotIndex].pose.loc.y = 0.0f;
robotData[visRobotIndex].pose.orient = data[i].orient;
// Set pose of remaining robot to something arbitrary
robotData[remainingRobotIndex].pose.loc.x = 1000.0f;
robotData[remainingRobotIndex].pose.loc.y = 1000.0f;
robotData[remainingRobotIndex].pose.orient = 0.0f;
#if VERBOSE_BLOB_DETECTION
fprintf (outputFile,
"<RobotTrainingLoc>robotIndex=%d robotOrient=%f ownLoc=%f,%f</RobotTrainingLoc>\n",
data[i].vis, data[i].orient,
data[i].loc.x, data[i].loc.y);
#endif
}
else
{
// Required for blob detection - even if the obs/vis loc isn't set, we
// still do blob detection. So set the visible robot to something reasonable.
robotData[ownRobotIndex].pose.loc.x = 0.0f;
robotData[ownRobotIndex].pose.loc.y = 0.0f;
robotData[ownRobotIndex].pose.orient = 0.0f;
robotData[visRobotIndex].pose.loc.x = 200.0f;
robotData[visRobotIndex].pose.loc.y = 0.0f;
robotData[visRobotIndex].pose.orient = 0.0f;
robotData[remainingRobotIndex].pose.loc.x = 1000.0f;
robotData[remainingRobotIndex].pose.loc.y = 1000.0f;
robotData[remainingRobotIndex].pose.orient = 0.0f;
}
printf ("\nIMAGE %d\n", i);
if (doRecognition)
{
// The functions called here should mirror CameraProcessing_processImageData
fprintf (outputFile, "<TestRobotRecognition>image=%d file=%s</TestRobotRecognition>\n", i, inputDir);
if (!RobotRecognitionTesting_anyRobotColoursInTrainingImage (
data[i].occupancyGrid))
{
printf ("No robot colours\n");
fprintf (outputFile, "<NoRobotColours />\n");
continue;
}
#if VERBOSE_BLOB_DETECTION
nColours = RobotRecognitionTesting_getColoursInTrainingImage (
colours,
data[i].occupancyGrid);
RobotRecognitionTesting_printColoursInTrainingImage (
outputFile,
nColours,
colours);
#endif
OccupancyGrid_init (occupancyGrid);
ObstacleRecognition_calcImageFeatures (
img,
cellFeatures,
&imgFeatures);
#if defined(DEBUGGING_ROBOT_REC)
#error "adsf"
assert (1);
#else
// DEBUGGING_ROBOT_REC should be defined in so params can be passed to functions
assert (0);
#endif
anyRobotCells = RobotRecognition_detectRobotsNEW (
outputFile,
#if VERBOSE_BLOB_DETECTION
displayImg,
iplImage,
windowSize,
colours,
#endif
#if defined(DEBUGGING_ROBOT_REC)
isLocSetInImg,
#endif
img,
&camVectors,
&uncertaintyConstants,
robotData,
ownRobotIndex,
&robotEstimates,
colourScoreGrid,
cellFeatures,
occupancyGrid);
if (robotEstimates.size)
{
Geometry_ptFromOrient (
robotData[ownRobotIndex].pose.loc.x,
robotData[ownRobotIndex].pose.loc.y,
&focalPt.x,
&focalPt.y,
CAM_P_OFFSET_WORLD,
robotData[ownRobotIndex].pose.orient);
printf ("estimates: %d ownLoc=(%f,%f) focal=(%f,%f)\n",
robotEstimates.size,
robotData[ownRobotIndex].pose.loc.x, robotData[ownRobotIndex].pose.loc.y,
focalPt.x, focalPt.y);
iter = robotEstimates.front;
while (iter)
{
robotEstimate = (RobotEstimate*)iter->value;
if (robotEstimate->estType < FACE_EST_EDGE)
{
iter = iter->next;
continue;
}
estLoc.x = focalPt.x + robotEstimate->estLoc.x;
estLoc.y = focalPt.y + robotEstimate->estLoc.y;
printf ("estimate: robotIndex=%d est=(%.2f,%.2f) cov=(%.2f,%.2f,%.2f,%.2f) estType=%d\n",
robotEstimate->robotIndex,
estLoc.x, estLoc.y,
robotEstimate->estCov.mat[0], robotEstimate->estCov.mat[1], robotEstimate->estCov.mat[2], robotEstimate->estCov.mat[3],
robotEstimate->estType);
if (isLocSetInImg)
{
error.x = estLoc.x - robotData[data[i].vis].pose.loc.x;
error.y = estLoc.y - robotData[data[i].vis].pose.loc.y;
printf ("error=(%.2f,%.2f)\n", error.x, error.y);
}
#if 1
fprintf (outputFile,
"<RobotTrainingEst>robotIndex=%d relEst=(%f,%f) focalPt=(%f,%f) finalEst=(%f,%f) cov=(%f,%f,%f,%f)",
robotEstimate->robotIndex,
robotEstimate->estLoc.x, robotEstimate->estLoc.y,
focalPt.x, focalPt.y,
estLoc.x, estLoc.y,
robotEstimate->estCov.mat[0], robotEstimate->estCov.mat[1], robotEstimate->estCov.mat[2], robotEstimate->estCov.mat[3]);
if (isLocSetInImg)
{
fprintf (outputFile, " error=(%f,%f,%f) actualLoc=(%f,%f)",
error.x, error.y, sqrt (error.x * error.x + error.y * error.y),
robotData[data[i].vis].pose.loc.x, robotData[data[i].vis].pose.loc.y);
}
fprintf (outputFile, "</RobotTrainingEst>\n");
#endif
iter = iter->next;
}
List_clear (&robotEstimates, 1);
}
ObstacleRecognition_calcOccupancy (
// outputFile,
img,
cellFeatures,
&imgFeatures,
occupancyGrid,
camScoreGrid,
anyRobotCells,
&dummy);
memcpy (iplImage->imageData, displayImg->data, iplImage->widthStep * iplImage->height);
Image_drawOccupancy3 (
iplImage->imageData,
occupancyGrid,
CAM_OCCUPANCY_GRID_ORIGIN_X,
CAM_OCCUPANCY_GRID_ORIGIN_Y,
CAM_OCCUPANCY_GRID_Y);
cvNamedWindow ("occupGrid", windowSize);
cvShowImage ("occupGrid", iplImage);
cvWaitKey (1);
fflush (outputFile);
}
else
{
RobotRecognitionTesting_displayTrainingImage (
iplImage,
windowSize,
displayImg,
data[i].occupancyGrid,
1);
}
if (doRecognition)
{
fprintf (outputFile, "\n\n\n");
fflush (outputFile);
}
// cvWaitKey (0);
cvWaitKey (1);
// break;
}
if (doRecognition)
{
fclose (outputFile);
}
free (occupancyGrid);
free (camScoreGrid);
free (colourScoreGrid);
free (data);
free (cellFeatures);
Image_dtor (&displayImg);
Image_dtor (&img);
cvReleaseImage (&iplImage);
cvCleanup();
}
int main(void) {
List_T L = NULL;
Bootstrap(); // Need to initialize library
printf("============> Start List Tests\n\n");
printf("=> Test0: create\n");
{
L = List_new();
assert(L);
List_free(&L);
}
printf("=> Test0: OK\n\n");
printf("=> Test1: List_push() & List_length()\n");
{
L = List_new();
List_push(L, "1");
List_push(L, "2");
List_push(L, "3");
List_push(L, "4");
List_push(L, "5");
List_push(L, "6");
List_push(L, "7");
List_push(L, "8");
List_push(L, "9");
List_push(L, "10");
List_push(L, "11");
List_push(L, "12");
List_push(L, "13");
List_push(L, "14");
List_push(L, "15");
assert(Str_isEqual(L->tail->e, "1"));
assert(Str_isEqual(L->head->e, "15"));
assert(List_length(L) == 15);
}
printf("=> Test1: OK\n\n");
printf("=> Test2: List_pop()\n");
{
int i= 0;
list_t p;
while (List_pop(L)) ;
assert(List_length(L) == 0);
// Ensure that nodes are retained in the freelist
for (p= L->freelist; p; p= p->next) i++;
assert(i == 15);
List_free(&L);
}
printf("=> Test2: OK\n\n");
printf("=> Test3: List_append()\n");
{
L = List_new();
List_append(L, "1");
List_append(L, "2");
List_append(L, "3");
List_append(L, "4");
List_append(L, "5");
List_append(L, "6");
List_append(L, "7");
List_append(L, "8");
List_append(L, "9");
List_append(L, "10");
List_append(L, "11");
List_append(L, "12");
List_append(L, "13");
List_append(L, "14");
List_append(L, "15");
assert(Str_isEqual(L->tail->e, "15"));
assert(Str_isEqual(L->head->e, "1"));
assert(List_length(L) == 15);
}
printf("=> Test3: OK\n\n");
printf("=> Test4: List_cat()\n");
{
List_T t= List_new();
List_append(t, "a");
List_append(t, "b");
List_append(t, "c");
List_append(t, "d");
List_cat(L, t);
assert(Str_isEqual(L->tail->e, "d"));
assert(Str_isEqual(L->head->e, "1"));
assert(List_length(L) == 19);
}
printf("=> Test4: OK\n\n");
printf("=> Test5: List_reverse()\n");
{
list_t p;
List_T l= List_new();
List_append(l, "a");
List_append(l, "b");
List_append(l, "c");
List_append(l, "d");
printf("\tList before reverse: ");
for (p= l->head; p; p= p->next)
printf("%s%s", (char*)p->e, p->next?"->":"\n");
assert(Str_isEqual(l->head->e, "a"));
assert(Str_isEqual(l->tail->e, "d"));
List_reverse(l);
printf("\tList after reverse: ");
for (p= l->head; p; p= p->next)
printf("%s%s", (char*)p->e, p->next?"->":"\n");
assert(Str_isEqual(l->head->e, "d"));
assert(Str_isEqual(l->tail->e, "a"));
List_free(&l);
}
printf("=> Test5: OK\n\n");
printf("=> Test6: List_map()\n");
{
int i = 0;
List_map(L, apply, &i);
assert(i == 19);
}
printf("=> Test6: OK\n\n");
printf("=> Test7: List_clear()\n");
{
List_clear(L);
assert(List_length(L) == 0);
assert(L->freelist);
}
printf("=> Test7: OK\n\n");
List_free(&L);
printf("=> Test8: List malloc\n");
{
L = List_new();
List_push(L, "1");
List_push(L, "2");
List_push(L, "3");
List_push(L, "4");
List_push(L, "5");
List_push(L, "6");
List_push(L, "7");
List_push(L, "8");
List_push(L, "9");
List_push(L, "10");
List_push(L, "11");
List_push(L, "12");
List_push(L, "13");
List_push(L, "14");
List_push(L, "15");
assert(Str_isEqual(L->tail->e, "1"));
assert(Str_isEqual(L->head->e, "15"));
assert(List_length(L) == 15);
List_clear(L);
List_append(L, "1");
List_append(L, "2");
List_append(L, "3");
List_append(L, "4");
List_append(L, "5");
List_append(L, "6");
List_append(L, "7");
List_append(L, "8");
List_append(L, "9");
List_append(L, "10");
List_append(L, "11");
List_append(L, "12");
List_append(L, "13");
List_append(L, "14");
List_append(L, "15");
assert(Str_isEqual(L->tail->e, "15"));
assert(Str_isEqual(L->head->e, "1"));
assert(List_length(L) == 15);
List_free(&L);
}
printf("=> Test8: OK\n\n");
printf("=> Test9: List remove\n");
{
char *one = "1";
char *two = "2";
L = List_new();
printf("\tRemove from empty list.. ");
assert(List_remove(L, "1") == NULL);
printf("OK\n");
List_push(L, one);
printf("\tRemove from 1 element list.. ");
assert(List_remove(L, one) == one);
assert(List_length(L) == 0);
printf("OK\n");
List_push(L, one);
List_push(L, two);
printf("\tRemove last from list.. ");
assert(List_remove(L, two) == two);
assert(List_length(L) == 1);
printf("OK\n");
List_append(L, two);
List_append(L, two);
List_append(L, two);
List_append(L, "5");
printf("\tRemove first occurrence.. ");
assert(List_remove(L, two) == two);
assert(List_length(L) == 4);
printf("OK\n");
List_free(&L);
}
printf("=> Test9: OK\n\n");
printf("=> Test10: check pointers\n");
{
L = List_new();
printf("\tCheck pop.. ");
List_push(L, "1");
List_push(L, "2");
List_pop(L);
List_pop(L);
List_push(L, "1");
assert(L->head == L->tail);
List_pop(L);
List_append(L, "1");
assert(L->head == L->tail);
printf("OK\n");
printf("\tCheck remove.. ");
List_push(L, "1");
List_append(L, "2");
List_remove(L, "2");
List_remove(L, "1");
assert(L->head == L->tail);
printf("OK\n");
List_free(&L);
}
printf("=> Test10: OK\n\n");
printf("=> Test11: List_toArray()\n");
{
List_T l = List_new();
List_append(l, "a");
List_append(l, "b");
List_append(l, "c");
List_append(l, "d");
char **array = (char**)List_toArray(l);
assert(Str_isEqual(array[0], "a"));
assert(Str_isEqual(array[1], "b"));
assert(Str_isEqual(array[2], "c"));
assert(Str_isEqual(array[3], "d"));
assert(array[4] == NULL);
FREE(array);
List_free(&l);
}
printf("=> Test11: OK\n\n");
printf("============> List Tests: OK\n\n");
return 0;
}
PatternList *PatternList_clear(PatternList *patternList)
{
assert(patternList != NULL);
return (PatternList*)List_clear(patternList,(ListNodeFreeFunction)freePatternNode,NULL);
}
int RobotRecognition_detectRobotsNEW (
FILE *outputFile,
#if VERBOSE_BLOB_DETECTION
Image *displayImg,
IplImage *iplImage,
const int *windowSize,
const uchar *colourIdsThisTrainingImg,
#endif
#if defined(DEBUGGING_ROBOT_REC)
const int isLocSet,
#endif
Image *originalImg,
CamVectors *camVectors,
UncertaintyConstants *uncertaintyConstants,
const RobotData *robotDataArray,
const int ownIndex,
List *robotEstimates,
float *colourScoreGrid,
ImgCellFeatures *cellFeatures,
OccupancyGrid *camOccupancyGrid)
{
float strongestScore;
List colourBlobs;
int nValidBlobs;
int anyRobotCells = 0;
#if defined(SIMULATION)
#if defined(DEBUGGING_ROBOT_REC)
#else
assert (robotEstimates);
assert (robotDataArray);
#endif
#endif
colourBlobs = initList();
strongestScore = RobotRecognitionModel_calcColourScores (
cellFeatures,
colourScoreGrid);
if (strongestScore < COLOUR_CONFIDENCE_THRESHOLD)
{
anyRobotCells = RobotRecognitionCore_markRobotOccupiedCells (
outputFile,
colourScoreGrid,
camOccupancyGrid,
COLOUR_CONFIDENCE_THRESHOLD);
#if VERBOSE_BLOB_DETECTION == 2
RobotRecognitionBlobDetection_displayRobotCells (
iplImage,
windowSize,
displayImg,
camOccupancyGrid);
#endif
RobotRecognitionBlobDetection_createBlobs (
outputFile,
#if VERBOSE_BLOB_DETECTION
iplImage,
windowSize,
originalImg,
colourIdsThisTrainingImg,
#endif
ownIndex,
colourScoreGrid,
cellFeatures,
camOccupancyGrid,
&colourBlobs);
nValidBlobs = RobotRecognitionBlobDetection_analyseBlobs (
#if VERBOSE_BLOB_DETECTION
colourIdsThisTrainingImg,
#endif
robotDataArray,
ownIndex,
camVectors,
outputFile,
&colourBlobs);
#if VERBOSE_BLOB_DETECTION
RobotRecognitionBlobDetection_displayBlobs (
outputFile,
iplImage,
windowSize,
displayImg,
&colourBlobs,
nValidBlobs);
#endif
#if defined(DEBUGGING_ROBOT_REC)
if (isLocSet)
#endif
{
// Calc blob edge cells, make rough estimate, calc exact blob edges, make accurate estimate
RobotRecognition_identifyRobotFaces (
outputFile,
camVectors,
robotDataArray,
ownIndex,
&colourBlobs,
robotEstimates);
RobotRecognitionRoughEstimation_estimateRobotLocations (
outputFile,
camVectors,
robotDataArray,
uncertaintyConstants,
ownIndex,
&colourBlobs,
robotEstimates);
#if READY_FOR_ACCURATE_REC
RobotRecognitionAccurateEstimation_estimateRobotLocationsNEW (
outputFile,
originalImg,
camVectors,
robotDataArray,
uncertaintyConstants,
ownIndex,
robotEstimates);
#endif // READY_FOR_ACCURATE_REC
}
}
#if VERBOSE_BLOB_DETECTION
else
{
printf ("No colour responses in this image\n");
memcpy (iplImage->imageData, originalImg->data, originalImg->height * originalImg->wStep);
cvNamedWindow ("emptyImg", windowSize);
cvShowImage ("emptyImg", iplImage);
cvWaitKey (1);
}
#endif
List_clear (&colourBlobs, 1);
return anyRobotCells;
}
void LongNum_clear (LongNum **num)
{
List_clear(&(*num)->digits);
(*num)->sign = 0;
return;
}
EntryList *EntryList_clear(EntryList *entryList)
{
assert(entryList != NULL);
return (EntryList*)List_clear(entryList,(ListNodeFreeFunction)freeEntryNode,NULL);
}
void PatternList_clear(PatternList *patternList)
{
assert(patternList != NULL);
List_clear(patternList,(ListNodeFreeFunction)freePatternNode,NULL);
}
