/*---------------------------------------------------------------
						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"); \
		);
Beispiel #2
0
// ------------------------------------------------------
//				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;
	}

}
Beispiel #3
0
// TSetOfMetricMapInitializers
void TSetOfMetricMapInitializers_push_back(
	TSetOfMetricMapInitializers& self, TMetricMapInitializer::Ptr& o)
{
	self.push_back(o);
}
Beispiel #4
0
// ------------------------------------------------------
//						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;
	}
}
Beispiel #5
0
// ------------------------------------------------------
//						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;
	}
}
Beispiel #6
0
// ------------------------------------------------------
//				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

}