12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Reverted testOccupancyGrid.cpp back so that it does not break make check.

release/4.3a0
bpeasle 2012-05-23 15:57:12 +00:00
parent 69e8923690
commit 3264bd5a39
1 changed files with 20 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
tests/testOccupancyGrid.cpp
View File

@ -286,84 +286,38 @@ TEST_UNSAFE( OccupancyGrid, Test1) {
//Build a small grid and test optimization //Build a small grid and test optimization
//Build small grid //Build small grid
double width = 20; //meters double width = 3; //meters
double height = 20; //meters double height = 2; //meters
double resolution = 0.2; //meters double resolution = 0.5; //meters
OccupancyGrid occupancyGrid(width, height, resolution); //default center to middle OccupancyGrid occupancyGrid(width, height, resolution); //default center to middle
//Add measurements //Add measurements
// Pose2 pose(0,0,0); Pose2 pose(0,0,0);
// double range = 4.499765; double range = 1;
//
// occupancyGrid.addPrior(0, 0.7);
// EXPECT_LONGS_EQUAL(1, occupancyGrid.size());
//
// occupancyGrid.addLaser(pose, range);
// EXPECT_LONGS_EQUAL(2, occupancyGrid.size());
//add lasers occupancyGrid.addPrior(0, 0.7);
int n_frames = 1; EXPECT_LONGS_EQUAL(1, occupancyGrid.size());
int n_lasers_per_frame = 640;
char laser_list_file[1000]; occupancyGrid.addLaser(pose, range);
EXPECT_LONGS_EQUAL(2, occupancyGrid.size());
OccupancyGrid::Occupancy occupancy = occupancyGrid.emptyOccupancy();
EXPECT_LONGS_EQUAL(900, occupancyGrid.laserFactorValue(0,occupancy));
for(int i = 0; i < n_frames; i++){ occupancy[16] = 1;
sprintf(laser_list_file, "/home/brian/Desktop/research/user/bpeasle/code/KinectInterface/Data/ScanLinesAsLasers/KinectRecording9/laser_list%.4d", i); EXPECT_LONGS_EQUAL(1, occupancyGrid.laserFactorValue(0,occupancy));
FILE *fptr = fopen(laser_list_file,"r");
double x,y, theta;
double range, angle;
fscanf(fptr, "%lf %lf %lf", &x, &y, &theta);
for(int j = 0; j < n_lasers_per_frame; j++){ occupancy[15] = 1;
fscanf(fptr, "%lf %lf", &range, &angle); EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
//if(j == 159){
Pose2 pose(x,y, theta+angle);
occupancyGrid.addLaser(pose, range); occupancy[16] = 0;
//} EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
}
fclose(fptr);
}
// OccupancyGrid::Occupancy occupancy = occupancyGrid.emptyOccupancy();
// EXPECT_LONGS_EQUAL(900, occupancyGrid.laserFactorValue(0,occupancy));
//
//
// occupancy[16] = 1;
// EXPECT_LONGS_EQUAL(1, occupancyGrid.laserFactorValue(0,occupancy));
//
// occupancy[15] = 1;
// EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
//
// occupancy[16] = 0;
// EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
//run MCMC //run MCMC
OccupancyGrid::Marginals occupancyMarginals = occupancyGrid.runMetropolis(50000); OccupancyGrid::Marginals occupancyMarginals = occupancyGrid.runMetropolis(50000);
//EXPECT_LONGS_EQUAL( (width*height)/pow(resolution,2), occupancyMarginals.size()); EXPECT_LONGS_EQUAL( (width*height)/pow(resolution,2), occupancyMarginals.size());
//select a cell at a random to flip
printf("\n");
for(size_t i = 0, it = 0; i < occupancyGrid.height(); i++){
for(size_t j = 0; j < occupancyGrid.width(); j++, it++){
printf("%.2lf ", occupancyMarginals[it]);
}
printf("\n");
}
char marginalsOutput[1000];
sprintf(marginalsOutput, "/home/brian/Desktop/research/user/bpeasle/code/KinectInterface/marginals.txt");
FILE *fptr = fopen(marginalsOutput, "w");
fprintf(fptr, "%d %d\n", occupancyGrid.width(), occupancyGrid.height());
for(int i = 0; i < occupancyMarginals.size(); i++){
fprintf(fptr, "%lf ", occupancyMarginals[i]);
}
fclose(fptr);