/*---------------------------------------------------------------
addMapFrame
---------------------------------------------------------------*/
unsigned int CIncrementalMapPartitioner::addMapFrame(
const CSensoryFramePtr &frame,
const CPose3DPDFPtr &robotPose )
{
size_t i=0,j=0,n=0;
CPose3D pose_i, pose_j, relPose;
CPose3DPDFPtr posePDF_i, posePDF_j;
CSensoryFramePtr sf_i, sf_j;
CMultiMetricMap *map_i=NULL,*map_j=NULL;
int debug_CheckPoint = 0;
mrpt::utils::TMatchingPairList corrs;
static CPose3D nullPose(0,0,0);
// Set options in graph partition:
CGraphPartitioner::DEBUG_SAVE_EIGENVECTOR_FILES = options.debugSaveAllEigenvectors;
// Create the maps:
TSetOfMetricMapInitializers mapInitializer;
TMetricMapInitializer mapElement;
mapElement.metricMapClassType = CLASS_ID( CSimplePointsMap );
mapInitializer.push_back( mapElement );
// mapElement.metricMapClassType = CLASS_ID( COccupancyGridMap2D );
// mapElement.occupancyGridMap2D_options.resolution = options.gridResolution;
// mapInitializer.push_back( mapElement );
mapElement.metricMapClassType = CLASS_ID( CLandmarksMap );
// mapElement.landmarksMap_options.insertionOpts.
mapInitializer.push_back( mapElement );
// Add new metric map to "m_individualMaps"
// --------------------------------------------
//CMultiMetricMap *newMetricMap = new CMultiMetricMap( &mapInitializer );
m_individualMaps.push_back( CMultiMetricMap() );
CMultiMetricMap &newMetricMap = m_individualMaps.back();
newMetricMap.setListOfMaps( &mapInitializer );
MRPT_START
// Create the metric map:
// -----------------------------------------------------------------
ASSERT_(newMetricMap.m_pointsMaps.size()>0);
newMetricMap.m_pointsMaps[0]->insertionOptions.isPlanarMap = false; // true
newMetricMap.m_pointsMaps[0]->insertionOptions.minDistBetweenLaserPoints = 0.20f;
options.minDistForCorrespondence = max(options.minDistForCorrespondence,1.3f*newMetricMap.m_pointsMaps[0]->insertionOptions.minDistBetweenLaserPoints);
// JLBC,17/AGO/2006: "m_individualMaps" were created from the robot pose, but it is
// more convenient now to save them as the robot being at (0,0,0).
//frame->insertObservationsInto( newMetricMap.m_pointsMaps[0] );
newMetricMap.m_pointsMaps[0]->copyFrom( * frame->buildAuxPointsMap<CPointsMap>(&newMetricMap.m_pointsMaps[0]->insertionOptions)); // Faster :-)
// Insert just the VisualLandmarkObservations:
newMetricMap.m_landmarksMap->insertionOptions.insert_SIFTs_from_monocular_images = false;
newMetricMap.m_landmarksMap->insertionOptions.insert_SIFTs_from_stereo_images = false;
newMetricMap.m_landmarksMap->insertionOptions.insert_Landmarks_from_range_scans = false;
frame->insertObservationsInto( newMetricMap.m_landmarksMap );
debug_CheckPoint=1;
// Add to corresponding vectors:
m_individualFrames.insert(robotPose, frame);
// Already added to "m_individualMaps" above
debug_CheckPoint=2;
// Ampliar la matriz de adyacencias:
// -----------------------------------------------------------------
n = m_A.getColCount();
n++;
m_A.setSize(n,n);
debug_CheckPoint=3;
ASSERT_(m_individualMaps.size() == n);
ASSERT_(m_individualFrames.size() == n);
// Ajustar tamaño del vector de nodos modificados:
// ---------------------------------------------------
// El nuevo evidentemente debe ser tenido en cuenta:
m_modified_nodes.push_back(true);
// Methods to compute adjacency matrix:
// true: matching between maps
// false: matching between observations through "CObservation::likelihoodWith"
// ------------------------------------------------------------------------------
bool useMapOrSF = options.useMapMatching;
debug_CheckPoint=4;
// Calcular los nuevos matchings y meterlos en la matriz:
// ----------------------------------------------------------------
//for (i=n-1;i<n;i++)
i=n-1; // Solo ejecutar para "i=n-1" la ultima fila/columna que esta vacia
{
// Get node "i":
m_individualFrames.get(i, posePDF_i, sf_i);
posePDF_i->getMean(pose_i);
// And its points map:
map_i = &m_individualMaps[i];
debug_CheckPoint=5;
// for (j=0;j<n;j++)
for (j=0;j<n-1;j++)
{
// Get node "j":
m_individualFrames.get(j, posePDF_j, sf_j);
posePDF_j->getMean( pose_j );
debug_CheckPoint=6;
relPose = pose_j - pose_i;
// And its points map:
map_j = &m_individualMaps[j];
debug_CheckPoint=66;
// Compute matching ratio:
if (useMapOrSF)
{
m_A(i,j) = map_i->compute3DMatchingRatio(
map_j,
relPose,
options.minDistForCorrespondence,
options.minMahaDistForCorrespondence );
}
else
{
//m_A(i,j) = sf_i->likelihoodWith(sf_j.pointer());
m_A(i,j) = observationsOverlap(sf_i, sf_j, &relPose );
}
} // for j
} // for i
for (i=0;i<n-1;i++) // Solo ejecutar para "i=n-1" la ultima fila/columna que esta vacia
{
debug_CheckPoint=8;
// Get node "i":
m_individualFrames.get(i, posePDF_i, sf_i);
posePDF_i->getMean(pose_i);
// And its points map:
map_i = &m_individualMaps[i];
debug_CheckPoint=9;
j=n-1; //for (j=n-1;j<n;j++)
{
// Get node "j":
m_individualFrames.get(j, posePDF_j, sf_j);
posePDF_j->getMean(pose_j);
debug_CheckPoint=10;
relPose = pose_j - pose_i;
// And its points map:
map_j = &m_individualMaps[j];
// Compute matching ratio:
if (useMapOrSF)
{
m_A(i,j) = map_i->compute3DMatchingRatio(
map_j,
CPose3D(relPose),
options.minDistForCorrespondence,
options.minMahaDistForCorrespondence );
}
else
{
//m_A(i,j) = sf_i->likelihoodWith(sf_j.pointer());
m_A(i,j) = observationsOverlap(sf_i, sf_j, &relPose );
}
debug_CheckPoint=12;
} // for j
} // for i
debug_CheckPoint=13;
// Self-similatity: Not used
m_A(n-1,n-1) = 0;
// Hacer que la matriz sea simetrica:
// -----------------------------------------------------------------
for (i=0;i<n;i++)
for (j=i+1;j<n;j++)
m_A(i,j) = m_A(j,i) = 0.5f * (m_A(i,j) + m_A(j,i) );
debug_CheckPoint=14;
/* DEBUG: Guardar la matriz: * /
A.saveToTextFile("debug_matriz.txt",1);
/ **/
// Añadir a la lista de nodos modificados los nodos afectados:
// -----------------------------------------------------------------
for (i=0;i<n;i++)
m_modified_nodes[i] = m_A(i,n-1) > 0;
debug_CheckPoint=15;
if (m_last_last_partition_are_new_ones)
{
// Insert into the "new_ones" partition:
m_last_partition[m_last_partition.size()-1].push_back( n-1 );
}
else
{
// Add a new partition:
vector_uint dummyPart;
dummyPart.push_back(n-1);
m_last_partition.push_back( dummyPart );
// The last one is the new_ones partition:
m_last_last_partition_are_new_ones = true;
}
return n-1; // Index of the new node
MRPT_END_WITH_CLEAN_UP( \
cout << "Unexpected runtime error at checkPoint="<< debug_CheckPoint << "\n"; \
cout << "\tn=" << n << "\n"; \
cout << "\ti=" << i << "\n"; \
cout << "\tj=" << j << "\n"; \
cout << "\tmap_i=" << map_i << "\n"; \
cout << "\tmap_j=" << map_j << "\n"; \
cout << "relPose: "<< relPose << endl; \
cout << "map_i.size()=" << map_i->m_pointsMaps[0]->size() << "\n"; \
cout << "map_j.size()=" << map_j->m_pointsMaps[0]->size() << "\n"; \
map_i->m_pointsMaps[0]->save2D_to_text_file(string("debug_DUMP_map_i.txt")); \
map_j->m_pointsMaps[0]->save2D_to_text_file(string("debug_DUMP_map_j.txt")); \
m_A.saveToTextFile("debug_DUMP_exception_A.txt"); \
);
// ------------------------------------------------------
// 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;
}
}
// TSetOfMetricMapInitializers
void TSetOfMetricMapInitializers_push_back(
TSetOfMetricMapInitializers& self, TMetricMapInitializer::Ptr& o)
{
self.push_back(o);
}
// ------------------------------------------------------
// MAIN
// ------------------------------------------------------
//int main(int argc, char **argv)
int icp_Main()
{
try
{
/*
bool showHelp = argc>1 && !os::_strcmp(argv[1],"--help");
bool showVersion = argc>1 && !os::_strcmp(argv[1],"--version");
printf(" icp-slam version 0.1 - Part of the MRPT\n");
printf(" MRPT C++ Library: %s - BUILD DATE %s\n", MRPT_getVersion().c_str(), MRPT_getCompilationDate().c_str());
if (showVersion)
return 0; // Program end
printf("-------------------------------------------------------------------\n");
// Process arguments:
if (argc<2 || showHelp )
{
printf("Usage: %s <config_file.ini>\n\n",argv[0]);
if (!showHelp)
{
mrpt::system::pause();
return -1;
}
else return 0;
}
*/
// INI_FILENAME = std::string( "config.ini" );
INI_FILENAME = "./cplus/slam/config.ini";
ASSERT_(fileExists(INI_FILENAME));
CConfigFile iniFile( INI_FILENAME );
// ------------------------------------------
// Load config from file:
// ------------------------------------------
RAWLOG_FILE = iniFile.read_string("MappingApplication","rawlog_file","", /*Force existence:*/ true);
rawlog_offset = iniFile.read_int("MappingApplication","rawlog_offset",0, /*Force existence:*/ true);
OUT_DIR_STD = iniFile.read_string("MappingApplication","logOutput_dir","log_out", /*Force existence:*/ true);
LOG_FREQUENCY = iniFile.read_int("MappingApplication","LOG_FREQUENCY",5, /*Force existence:*/ true);
SAVE_POSE_LOG = iniFile.read_bool("MappingApplication","SAVE_POSE_LOG", false, /*Force existence:*/ true);
SAVE_3D_SCENE = iniFile.read_bool("MappingApplication","SAVE_3D_SCENE", false, /*Force existence:*/ true);
CAMERA_3DSCENE_FOLLOWS_ROBOT = iniFile.read_bool("MappingApplication","CAMERA_3DSCENE_FOLLOWS_ROBOT", true, /*Force existence:*/ true);
MRPT_LOAD_CONFIG_VAR( SHOW_PROGRESS_3D_REAL_TIME, bool, iniFile, "MappingApplication");
MRPT_LOAD_CONFIG_VAR( SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS, int, iniFile, "MappingApplication");
OUT_DIR = OUT_DIR_STD.c_str();
MRPT_LOAD_CONFIG_VAR( insertionLinDistance, float, iniFile, "MappingApplication");
MRPT_LOAD_CONFIG_VAR_DEGREES( insertionAngDistance, iniFile, "MappingApplication");
metricMapsOpts.loadFromConfigFile(iniFile, "MappingApplication");
icpOptions.loadFromConfigFile(iniFile, "ICP");
matchAgainstTheGrid = iniFile.read_bool("MappingApplication","matchAgainstTheGrid", true );
// Print params:
printf("Running with the following parameters:\n");
printf(" RAWLOG file:'%s'\n", RAWLOG_FILE.c_str());
printf(" Output directory:\t\t\t'%s'\n",OUT_DIR);
printf(" matchAgainstTheGrid:\t\t\t%c\n", matchAgainstTheGrid ? 'Y':'N');
printf(" Log record freq:\t\t\t%u\n",LOG_FREQUENCY);
printf(" SAVE_3D_SCENE:\t\t\t%c\n", SAVE_3D_SCENE ? 'Y':'N');
printf(" SAVE_POSE_LOG:\t\t\t%c\n", SAVE_POSE_LOG ? 'Y':'N');
printf(" CAMERA_3DSCENE_FOLLOWS_ROBOT:\t%c\n",CAMERA_3DSCENE_FOLLOWS_ROBOT ? 'Y':'N');
printf("\n");
metricMapsOpts.dumpToConsole();
icpOptions.dumpToConsole();
// Checks:
ASSERT_(RAWLOG_FILE.size()>0);
ASSERT_(fileExists(RAWLOG_FILE));
// Run:
if(LASER_TEST==0)
MapBuilding_ICP();
else if(LASER_TEST==1)
laserTest();
//pause();
return 0;
} catch (std::exception &e)
{
setConsoleColor(CONCOL_RED,true);
std::cerr << "Program finished for an exception!!" << std::endl;
setConsoleColor(CONCOL_NORMAL,true);
std::cerr << e.what() << std::endl;
mrpt::system::pause();
return -1;
}
catch (...)
{
setConsoleColor(CONCOL_RED,true);
std::cerr << "Program finished for an untyped exception!!" << std::endl;
setConsoleColor(CONCOL_NORMAL,true);
mrpt::system::pause();
return -1;
}
}
// ------------------------------------------------------
// 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;
}
}
// ------------------------------------------------------
// Benchmark: A whole ICP-SLAM run
// ------------------------------------------------------
double icp_test_1(int a1, int a2)
{
#ifdef MRPT_DATASET_DIR
const string rawlog_file = MRPT_DATASET_DIR "/2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog";
if (!mrpt::system::fileExists(rawlog_file))
return 1;
CTicTac tictac;
int step = 0;
size_t rawlogEntry = 0;
CFileGZInputStream rawlogFile( rawlog_file );
TSetOfMetricMapInitializers metricMapsOpts;
const bool use_grid = a1!=0;
if (use_grid)
{
COccupancyGridMap2D::TMapDefinition def;
def.resolution = 0.05;
metricMapsOpts.push_back( def );
}
else
{
CSimplePointsMap::TMapDefinition def;
def.insertionOpts.minDistBetweenLaserPoints = 0.03;
metricMapsOpts.push_back( def );
}
double insertionLinDistance = 0.75;
double insertionAngDistance = DEG2RAD(30);
CICP::TConfigParams icpOptions;
icpOptions.maxIterations = 40;
// ---------------------------------
// Constructor
// ---------------------------------
CMetricMapBuilderICP mapBuilder;
mapBuilder.ICP_options.mapInitializers = metricMapsOpts;
mapBuilder.ICP_options.insertionLinDistance = insertionLinDistance;
mapBuilder.ICP_options.insertionAngDistance = insertionAngDistance;
mapBuilder.ICP_options.matchAgainstTheGrid = use_grid ;
mapBuilder.ICP_params = icpOptions;
// Start with an empty map:
mapBuilder.initialize( CSimpleMap() );
// ---------------------------------
// CMetricMapBuilder::TOptions
// ---------------------------------
mapBuilder.options.verbose = false;
mapBuilder.options.enableMapUpdating = true;
// ----------------------------------------------------------
// Map Building
// ----------------------------------------------------------
CActionCollectionPtr action;
CSensoryFramePtr observations;
for (;;)
{
// Load action/observation pair from the rawlog:
// --------------------------------------------------
if (! CRawlog::readActionObservationPair( rawlogFile, action, observations, rawlogEntry) )
break; // file EOF
// Execute:
mapBuilder.processActionObservation( *action, *observations );
step++;
// printf("\n---------------- STEP %u | RAWLOG ENTRY %u ----------------\n",step, (unsigned)rawlogEntry);
// Free memory:
action.clear_unique();
observations.clear_unique();
}
#if 0
mapBuilder.getCurrentlyBuiltMetricMap()->saveMetricMapRepresentationToFile( format("icp_%i_result",a1) );
#endif
if (!step) step++;
return tictac.Tac()/step;
#else
return 1;
#endif
}
/*---------------------------------------------------------------
process
---------------------------------------------------------------*/
void CDetectorDoorCrossing::process(
CActionRobotMovement2D &in_poseChange,
CSensoryFrame &in_sf,
TDoorCrossingOutParams &out_estimation
)
{
// Variables for generic use:
size_t i;
out_estimation.cumulativeTurning = 0;
MRPT_START
// 1) Add new pair to the list:
// -----------------------------------------
lastObs.addAction( in_poseChange );
lastObs.addObservations( in_sf );
// 2) Remove oldest pair:
// -----------------------------------------
ASSERT_( options.windowSize > 1 );
ASSERT_( (lastObs.size() % 2) == 0 ); // Assure even size
while (lastObs.size()>options.windowSize*2)
{
lastObs.remove(0);
lastObs.remove(0);
}
if ( lastObs.size() < options.windowSize * 2 )
{
// Not enought old data yet:
out_estimation.enoughtInformation = false;
return;
}
// 3) Build an occupancy grid map with observations
// -------------------------------------------------
CPose2D p, pos;
TSetOfMetricMapInitializers mapInitializer;
{
CSimplePointsMap::TMapDefinition def;
mapInitializer.push_back( def );
}
{
COccupancyGridMap2D::TMapDefinition def;
def.resolution = options.gridResolution;
mapInitializer.push_back( def );
}
CMultiMetricMap auxMap( &mapInitializer );
for (i=0;i<options.windowSize;i++)
{
CActionCollectionPtr acts = lastObs.getAsAction( i*2+0 );
CActionPtr act = acts->get(0);
ASSERT_( act->GetRuntimeClass()->derivedFrom( CLASS_ID( CActionRobotMovement2D ) ) )
CActionRobotMovement2DPtr action = CActionRobotMovement2DPtr( act );
action->poseChange->getMean(pos);
out_estimation.cumulativeTurning+= fabs(pos.phi());
// Get the cumulative pose for the current observation:
p = p + pos;
// Add SF to the grid map:
CSensoryFramePtr sf = lastObs.getAsObservations( i*2+1 );
CPose3D pose3D(p);
sf->insertObservationsInto( &auxMap, &pose3D );
}
// 4) Compute the information differece between this
// "map patch" and the previous one:
// -------------------------------------------------------
auxMap.m_gridMaps[0]->computeEntropy( entropy );
if (!lastEntropyValid)
{
out_estimation.enoughtInformation = false;
}
else
{
// 5) Fill output data
// ---------------------------------
out_estimation.enoughtInformation = true;
out_estimation.informationGain = entropy.I - lastEntropy.I;
out_estimation.pointsMap.copyFrom( *auxMap.m_pointsMaps[0] );
}
// For next iterations:
lastEntropy = entropy;
lastEntropyValid = true;
MRPT_END
}