void CMultiMetricMap_setListOfMaps(
CMultiMetricMap& self, TSetOfMetricMapInitializers& initializers)
{
self.setListOfMaps(initializers);
}
// ------------------------------------------------------
// 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;
}
}
// ------------------------------------------------------
// MAIN
// ------------------------------------------------------
int main(int argc, char **argv)
{
try
{
printf(" observations2map - Part of the MRPT\n");
printf(" MRPT C++ Library: %s - BUILD DATE %s\n", MRPT_getVersion().c_str(), MRPT_getCompilationDate().c_str());
printf("-------------------------------------------------------------------\n");
// Process arguments:
if ((argc!=4 && argc!=6) || (argc==6 && 0!=mrpt::system::os::_strcmp(argv[4],"-s") ) )
{
cout << "Use: observations2map <config_file.ini> <observations.simplemap> <outputmap_prefix> [-s INI_FILE_SECTION_NAME] " << endl;
cout << " Default: INI_FILE_SECTION_NAME = MappingApplication" << endl;
cout << "Push any key to exit..." << endl;
os::getch();
return -1;
}
string configFile = std::string( argv[1] );
string inputFile = std::string( argv[2] );
string outprefix = std::string( argv[3] );
if (argc>4)
{
METRIC_MAP_CONFIG_SECTION = string(argv[5]);
}
// Load simplemap:
cout << "Loading simplemap...";
mrpt::slam::CSimpleMap simplemap;
CFileGZInputStream f( inputFile.c_str() );
f >> simplemap;
cout <<"done: " << simplemap.size() << " observations." << endl;
// Create metric maps:
TSetOfMetricMapInitializers mapCfg;
mapCfg.loadFromConfigFile( CConfigFile( configFile ), METRIC_MAP_CONFIG_SECTION );
CMultiMetricMap metricMap;
metricMap.setListOfMaps( &mapCfg );
// Build metric maps:
cout << "Building metric maps...";
metricMap.loadFromProbabilisticPosesAndObservations( simplemap );
cout << "done." << endl;
// Save metric maps:
// ---------------------------
metricMap.saveMetricMapRepresentationToFile( outprefix );
// grid maps:
size_t i;
for (i=0;i<metricMap.m_gridMaps.size();i++)
{
string str = format( "%s_gridmap_no%02u.gridmap", outprefix.c_str(), (unsigned)i );
cout << "Saving gridmap #" << i << " to " << str << endl;
CFileGZOutputStream f(str);
f << *metricMap.m_gridMaps[i];
cout << "done." << endl;
}
return 0;
}
catch (std::exception &e)
{
std::cerr << e.what() << std::endl << "Program finished for an exception!!" << std::endl;
mrpt::system::pause();
return -1;
}
catch (...)
{
std::cerr << "Untyped exception!!" << std::endl;
mrpt::system::pause();
return -1;
}
}
