/*--------------------------------------------------------------- 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 }