Beispiel #1
0
void CMultiMetricMap_setListOfMaps(
	CMultiMetricMap& self, TSetOfMetricMapInitializers& initializers)
{
	self.setListOfMaps(initializers);
}
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
// ------------------------------------------------------
//						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;
	}
}