void BatchMode()
{
vector<double> frame_rates;
for (size_t i = 0; i < rawlogs.size(); i++)
{
CRawlog rawlog;
rawlog.loadFromRawLogFile(string(rawlogsDir + rawlogs[i] + ".rawlog"));
cout << "Processing Rawlog [" << i + 1 << "/" << rawlogs.size()
<< "]: " << rawlogs[i] << endl;
const unsigned int size = rawlog.size();
mrpt::utils::CTicTac tictac;
size_t counter = 0;
// PROCESS RAWLOG
for (unsigned int j = 1; j < size; j++)
{
if (!counter) tictac.Tic();
CObservation3DRangeScan::Ptr o =
std::dynamic_pointer_cast<CObservation3DRangeScan>(
rawlog.getAsObservation(j));
ASSERT_(o);
vector_detectable_object detected;
faceDetector.detectObjects(o, detected);
if (++counter == 10)
{
double t = tictac.Tac();
frame_rates.push_back(counter / t);
counter = 0;
}
std::this_thread::sleep_for(2ms);
}
unsigned int falsePositivesDeleted, realFacesDeleted;
faceDetector.debug_returnResults(
falsePositives[i], ignored[i], falsePositivesDeleted,
realFacesDeleted);
cout << "False positives deleted: " << falsePositivesDeleted << endl;
cout << "Real faces delted: " << realFacesDeleted << endl << endl;
}
cout << "Mean frame rate: " << sum(frame_rates) / frame_rates.size();
// faceDetector.experimental_showMeasurements();
system::pause();
}
// ------------------------------------------------------
// BenchmarkGridmaps
// ------------------------------------------------------
void BenchmarkGridmaps()
{
randomGenerator.randomize(333);
CMultiMetricMap metricMap;
TSetOfMetricMapInitializers mapInit;
// Create gridmap:
mapInit.loadFromConfigFile( CConfigFile(iniFile), "METRIC_MAPS" );
metricMap.setListOfMaps(&mapInit);
// prepare the laser scan:
CObservation2DRangeScan scan1;
scan1.aperture = M_PIf;
scan1.rightToLeft = true;
scan1.validRange.resize( SCANS_SIZE );
scan1.scan.resize(SCANS_SIZE);
ASSERT_( sizeof(SCAN_RANGES_1) == sizeof(float)*SCANS_SIZE );
memcpy( &scan1.scan[0], SCAN_RANGES_1, sizeof(SCAN_RANGES_1) );
memcpy( &scan1.validRange[0], SCAN_VALID_1, sizeof(SCAN_VALID_1) );
#if 1
CRawlog rawlog;
rawlog.loadFromRawLogFile("/Trabajo/Code/MRPT/share/mrpt/datasets/2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog");
scan1 = *rawlog.getAsObservations(400)->getObservationByClass<CObservation2DRangeScan>();
#endif
ASSERT_( metricMap.m_gridMaps.size() );
COccupancyGridMap2DPtr gridMap = metricMap.m_gridMaps[0];
COccupancyGridMap2D gridMapCopy( *gridMap );
int i, N;
CTicTac tictac;
// test 1: getcell
// ----------------------------------------
if (1)
{
N = 10000000;
cout << "Running test #1: getCell... "; cout.flush();
//COccupancyGridMap2D::cellType cell;
float p=0;
tictac.Tic();
for (i=0;i<N;i++)
{
p += gridMap->getCell( 0, 0 );
}
double T = tictac.Tac();
cout << "-> " << 1e9*T/N << " ns/iter. p=" << p << endl; // the "p" is to avoid optimizing out the entire loop!
}
// test 2: setcell
// ----------------------------------------
if (1)
{
N = 10000000;
cout << "Running test #2: setCell... "; cout.flush();
float p=0.8f;
tictac.Tic();
for (i=0;i<N;i++)
{
gridMap->setCell( 0, 0, p );
}
double T = tictac.Tac();
cout << "-> " << 1e9*T/N << " ns/iter." << endl; // the "p" is to avoid optimizing out the entire loop!
}
// test 3: updateCell
// ----------------------------------------
if (1)
{
N = 1000000;
cout << "Running test #3: updateCell... "; cout.flush();
float p=0.57f;
tictac.Tic();
for (i=0;i<N;i++)
{
gridMap->updateCell( 0, 0, p );
}
double T = tictac.Tac();
cout << "-> " << 1e9*T/N << " ns/iter." << endl; // the "p" is to avoid optimizing out the entire loop!
}
// test 4: updateCell_fast
// ----------------------------------------
if (1)
{
N = 10000000;
cout << "Running test #4: updateCell_fast... "; cout.flush();
float p=0.57f;
COccupancyGridMap2D::cellType logodd_obs = COccupancyGridMap2D::p2l( p );
//float p_1 = 1-p;
COccupancyGridMap2D::cellType *theMapArray = gridMap->getRow(0);
unsigned theMapSize_x = gridMap->getSizeX();
COccupancyGridMap2D::cellType logodd_thres_occupied = COccupancyGridMap2D::OCCGRID_CELLTYPE_MIN+logodd_obs;
tictac.Tic();
for (i=0;i<N;i++)
{
COccupancyGridMap2D::updateCell_fast_occupied( 2, 2, logodd_obs,logodd_thres_occupied, theMapArray, theMapSize_x);
}
double T = tictac.Tac();
cout << "-> " << 1e9*T/N << " ns/iter." << endl; // the "p" is to avoid optimizing out the entire loop!
}
#if 0
for (i=50;i<51;i++)
{
CPose3D pose3D(0.21,0.34,0,-2);
//scan1.validRange.assign(scan1.validRange.size(), false);
//scan1.validRange[i]=true;
gridMap->clear();
gridMap->resizeGrid(-5,20,-15,15);
gridMap->insertObservation( &scan1, &pose3D );
gridMap->saveAsBitmapFile(format("./gridmap_with_widening_%04i.png",i));
}
#endif
// test 5: Laser insertion
// ----------------------------------------
if (1)
{
gridMap->insertionOptions.wideningBeamsWithDistance = false;
N = 3000;
cout << "Running test #5: Laser insert. w/o widen... "; cout.flush();
tictac.Tic();
for (i=0;i<N;i++)
{
#if 1
CPose2D pose(
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-M_PI,M_PI) );
CPose3D pose3D(pose);
#else
CPose3D pose3D;
#endif
gridMap->insertObservation( &scan1, &pose3D );
}
double T = tictac.Tac();
cout << "-> " << 1000*T/N << " ms/iter, scans/sec:" << N/T << endl;
CPose3D pose3D;
gridMap->clear();
gridMap->insertObservation( &scan1, &pose3D );
gridMap->saveAsBitmapFile("./gridmap_without_widening.png");
}
// test 6: Laser insertion without widening
// --------------------------------------------------
if (1)
{
gridMap->insertionOptions.wideningBeamsWithDistance = true;
N = 3000;
cout << "Running test #6: Laser insert. widen... "; cout.flush();
tictac.Tic();
for (i=0;i<N;i++)
{
#if 1
CPose2D pose(
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-M_PI,M_PI) );
CPose3D pose3D(pose);
#else
CPose3D pose3D;
#endif
gridMap->insertObservation( &scan1, &pose3D );
}
double T = tictac.Tac();
cout << "-> " << 1000*T/N << " ms/iter, scans/sec:" << N/T << endl;
CPose3D pose3D;
gridMap->clear();
gridMap->insertObservation( &scan1, &pose3D );
gridMap->saveAsBitmapFile("./gridmap_with_widening.png");
}
// test 7: Grid resize
// ----------------------------------------
if (1)
{
N = 400;
cout << "Running test #7: Grid resize... "; cout.flush();
tictac.Tic();
for (i=0;i<N;i++)
{
*gridMap = gridMapCopy;
gridMap->resizeGrid(-30,30,-40,40);
}
double T = tictac.Tac();
cout << "-> " << 1000*T/N << " ms/iter" << endl;
}
// test 8: Likelihood computation
// ----------------------------------------
if (1)
{
N = 5000;
*gridMap = gridMapCopy;
CPose3D pose3D(0,0,0);
gridMap->insertObservation( &scan1, &pose3D );
cout << "Running test #8: Likelihood... "; cout.flush();
double R = 0;
tictac.Tic();
for (i=0;i<N;i++)
{
CPose2D pose(
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-M_PI,M_PI) );
R+=gridMap->computeObservationLikelihood(&scan1,pose);
}
double T = tictac.Tac();
cout << "-> " << 1000*T/N << " ms/iter" << endl;
}
}
void getImages( CRawlog &i_rawlog, vector<CImage> &v_images,
size_t imgWidth, size_t width, size_t height, size_t N_channels,
string textToAdd, size_t N_sensors )
{
vector<CObservation3DRangeScanPtr> v_obs(4);
vector<bool> v_imagesLoaded(4,false);
// Insert a few frames to show the
CImage preImage(width,height,N_channels);
preImage.textOut(10,20,textToAdd,TColor(255,0,255));
for ( size_t i=0; i < 10; i++ )
v_images.push_back(preImage);
for ( size_t obsIndex = 0; obsIndex < i_rawlog.size(); obsIndex++ )
{
CObservationPtr obs = i_rawlog.getAsObservation(obsIndex);
if ( IS_CLASS(obs, CObservation3DRangeScan) )
{
CObservation3DRangeScanPtr obs3D = CObservation3DRangeScanPtr(obs);
obs3D->load();
size_t inserted;
if ( obs3D->sensorLabel == "RGBD_1" )
{
v_obs[0] = obs3D;
v_imagesLoaded[0] = true;
inserted = 0;
}
else if ( obs3D->sensorLabel == "RGBD_2" )
{
v_obs[1] = obs3D;
v_imagesLoaded[1] = true;
inserted = 1;
}
else if ( obs3D->sensorLabel == "RGBD_3" )
{
v_obs[2] = obs3D;
v_imagesLoaded[2] = true;
inserted = 2;
}
else if ( obs3D->sensorLabel == "RGBD_4" )
{
v_obs[3] = obs3D;
v_imagesLoaded[3] = true;
inserted = 3;
}
double sum = v_imagesLoaded[0] + v_imagesLoaded[1]
+ v_imagesLoaded[2] + v_imagesLoaded[3];
for ( size_t i = 0; i < 4; i++ )
{
if (( i == inserted ) || (v_obs[i].null()))
continue;
if ( timeDifference( v_obs[i]->timestamp, v_obs[inserted]->timestamp )
> 0.5 )
v_imagesLoaded[i] = 0;
}
if ( sum == 4 )
{
CImage img(height,width,N_channels);
size_t imagesDrawn = 0;
vector<size_t> v_imgsOrder(4);
v_imgsOrder[0] = 2;
v_imgsOrder[1] = 0;
v_imgsOrder[2] = 1;
v_imgsOrder[3] = 3;
for ( size_t i = 0; i < 4; i++ )
{
if ( v_sensorsToUse[i] )
{
if ( useDepthImg )
{
CImage imgDepth;
imgDepth.setFromMatrix(v_obs[v_imgsOrder[i]]->rangeImage);
img.drawImage(0,imgWidth*imagesDrawn,imgDepth);
}
else
img.drawImage(0,imgWidth*imagesDrawn,v_obs[v_imgsOrder[i]]->intensityImage);
imagesDrawn++;
}
}
CImage rotatedImg(width,height,N_channels);
for ( size_t row = 0; row < 320; row++ )
for ( size_t col = 0; col < imgWidth*N_sensors; col++ )
{
u_int8_t r, g,b;
if ( useDepthImg )
{
r = g = b = *(img.get_unsafe(row,col,0));
}
else
{
r = *(img.get_unsafe(row,col,2));
g = *(img.get_unsafe(row,col,1));
b = *(img.get_unsafe(row,col,0));
}
TColor color(r,g,b);
rotatedImg.setPixel(col,320-row,color);
}
rotatedImg.textOut(10,20,textToAdd,TColor(255,0,255));
v_images.push_back( rotatedImg );
v_imagesLoaded.clear();
v_imagesLoaded.resize(4,0);
}
}
}
}
