예제 #1
0
void thread_grabbing(TThreadParam& p)
{
	try
	{
		CFileGZOutputStream f_out_rawlog;
		if (arg_out_rawlog.isSet())
		{
			if (!f_out_rawlog.open(arg_out_rawlog.getValue()))
				THROW_EXCEPTION_FMT(
					"Error creating output rawlog file: %s",
					arg_out_rawlog.getValue().c_str());
		}
		auto arch = mrpt::serialization::archiveFrom(f_out_rawlog);

		mrpt::hwdrivers::CVelodyneScanner velodyne;

		if (arg_verbose.isSet()) velodyne.enableVerbose(true);

		// Set params:
		velodyne.setModelName(mrpt::typemeta::TEnumType<
							  mrpt::hwdrivers::CVelodyneScanner::model_t>::
								  name2value(arg_model.getValue()));
		if (arg_ip_filter.isSet())
			velodyne.setDeviceIP(
				arg_ip_filter.getValue());  // Default: from any IP
		if (arg_in_pcap.isSet())
			velodyne.setPCAPInputFile(arg_in_pcap.getValue());
		if (arg_out_pcap.isSet())
			velodyne.setPCAPOutputFile(arg_out_pcap.getValue());

		// If you have a calibration file, better than default values:
		if (arg_calib_file.isSet())
		{
			mrpt::obs::VelodyneCalibration calib;
			if (!calib.loadFromXMLFile(arg_calib_file.getValue()))
				throw std::runtime_error(
					"Aborting: error loading calibration file.");
			velodyne.setCalibration(calib);
		}

		// Open:
		cout << "Calling CVelodyneScanner::initialize()...";
		velodyne.initialize();
		cout << "OK\n";

		cout << "Waiting for first data packets (Press CTRL+C to abort)...\n";

		CTicTac tictac;
		int nScans = 0;
		bool hard_error = false;

		while (!hard_error && !p.quit)
		{
			// Grab new observations from the camera:
			CObservationVelodyneScan::Ptr
				obs;  // (initially empty) Smart pointers to observations
			CObservationGPS::Ptr obs_gps;

			hard_error = !velodyne.getNextObservation(obs, obs_gps);

			// Save to log file:
			if (f_out_rawlog.fileOpenCorrectly())
			{
				if (obs) arch << *obs;
				if (obs_gps) arch << *obs_gps;
			}

			if (obs)
			{
				std::atomic_store(&p.new_obs, obs);
				nScans++;
			}
			if (obs_gps) std::atomic_store(&p.new_obs_gps, obs_gps);

			if (p.pushed_key != 0)
			{
				switch (p.pushed_key)
				{
					case 27:
						p.quit = true;
						break;
				}

				// Clear pushed key flag:
				p.pushed_key = 0;
			}

			if (nScans > 5)
			{
				p.Hz = nScans / tictac.Tac();
				nScans = 0;
				tictac.Tic();
			}
		}
	}
	catch (const std::exception& e)
	{
		cout << "Exception in Velodyne thread: " << mrpt::exception_to_str(e)
			 << endl;
		p.quit = true;
	}
}
예제 #2
0
// ------------------------------------------------------
//				VelodyneView main entry point
// ------------------------------------------------------
int VelodyneView(int argc, char** argv)
{
	// Parse arguments:
	if (!cmd.parse(argc, argv)) return 1;  // should exit.

	if (!arg_nologo.isSet())
	{
		printf(" velodyne-view - Part of the MRPT\n");
		printf(
			" MRPT C++ Library: %s - Sources timestamp: %s\n",
			mrpt::system::MRPT_getVersion().c_str(),
			mrpt::system::MRPT_getCompilationDate().c_str());
	}

	// Launch grabbing thread:
	// --------------------------------------------------------
	TThreadParam thrPar;
	std::thread thHandle(thread_grabbing, std::ref(thrPar));

	// Wait until data stream starts so we can say for sure the sensor has been
	// initialized OK:
	cout << "Waiting for sensor initialization...\n";
	do
	{
		CObservation::Ptr possiblyNewObs = std::atomic_load(&thrPar.new_obs);
		if (possiblyNewObs && possiblyNewObs->timestamp != INVALID_TIMESTAMP)
			break;
		else
			std::this_thread::sleep_for(10ms);
	} while (!thrPar.quit);

	// Check error condition:
	if (thrPar.quit) return 0;

	// Create window and prepare OpenGL object in the scene:
	// --------------------------------------------------------
	mrpt::gui::CDisplayWindow3D win3D("Velodyne 3D view", 800, 600);

	// Allow rendering large number of points without decimation:
	mrpt::global_settings::OCTREE_RENDER_MAX_DENSITY_POINTS_PER_SQPIXEL(1);
	mrpt::global_settings::OCTREE_RENDER_MAX_POINTS_PER_NODE(1e7);

	win3D.setCameraAzimuthDeg(140);
	win3D.setCameraElevationDeg(20);
	win3D.setCameraZoom(8.0);
	win3D.setFOV(90);
	win3D.setCameraPointingToPoint(0, 0, 0);
	mrpt::opengl::CPointCloudColoured::Ptr gl_points =
		mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
	gl_points->setPointSize(2.5);

	{
		mrpt::opengl::COpenGLScene::Ptr& scene = win3D.get3DSceneAndLock();

		// Create the Opengl object for the point cloud:
		scene->insert(gl_points);
		scene->insert(mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>());
		scene->insert(mrpt::opengl::stock_objects::CornerXYZ());

		win3D.unlockAccess3DScene();
		win3D.repaint();
	}

	CObservationVelodyneScan::Ptr last_obs;
	CObservationGPS::Ptr last_obs_gps;
	bool view_freeze = false;  // for pausing the view
	CObservationVelodyneScan::TGeneratePointCloudParameters pc_params;

	while (win3D.isOpen() && !thrPar.quit)
	{
		bool do_view_refresh = false;

		CObservationVelodyneScan::Ptr possiblyNewObs =
			std::atomic_load(&thrPar.new_obs);
		CObservationGPS::Ptr possiblyNewObsGps =
			std::atomic_load(&thrPar.new_obs_gps);

		if (possiblyNewObsGps &&
			possiblyNewObsGps->timestamp != INVALID_TIMESTAMP &&
			(!last_obs_gps ||
			 possiblyNewObsGps->timestamp != last_obs_gps->timestamp))
		{
			// It IS a new observation:
			last_obs_gps = std::atomic_load(&thrPar.new_obs_gps);

			std::string rmc_datum;
			if (last_obs_gps->has_RMC_datum)
			{
				rmc_datum = mrpt::format(
					"Lon=%.09f deg  Lat=%.09f deg  Valid?: '%c'\n",
					last_obs_gps->getMsgByClass<gnss::Message_NMEA_RMC>()
						.fields.longitude_degrees,
					last_obs_gps->getMsgByClass<gnss::Message_NMEA_RMC>()
						.fields.latitude_degrees,
					last_obs_gps->getMsgByClass<gnss::Message_NMEA_RMC>()
						.fields.validity_char);
			}
			else
				rmc_datum = "NO";

			win3D.get3DSceneAndLock();
			win3D.addTextMessage(
				5, 40,
				format(
					"POS. frame rx at %s, RMC=%s",
					mrpt::system::dateTimeLocalToString(last_obs_gps->timestamp)
						.c_str(),
					rmc_datum.c_str()),
				TColorf(1, 1, 1), "mono", 10.0, mrpt::opengl::NICE, 102);
			win3D.unlockAccess3DScene();
			do_view_refresh = true;
		}

		if (possiblyNewObs && possiblyNewObs->timestamp != INVALID_TIMESTAMP &&
			(!last_obs || possiblyNewObs->timestamp != last_obs->timestamp))
		{
			// It IS a new observation:
			last_obs = possiblyNewObs;

			if (!last_obs->scan_packets.empty())
			{
				win3D.get3DSceneAndLock();
				win3D.addTextMessage(
					5, 55,
					format(
						"LIDAR scan rx at %s with %u packets",
						mrpt::system::dateTimeLocalToString(last_obs->timestamp)
							.c_str(),
						static_cast<unsigned int>(
							last_obs->scan_packets.size())),
					TColorf(1, 1, 1), "mono", 10.0, mrpt::opengl::NICE, 103);
				win3D.unlockAccess3DScene();
				do_view_refresh = true;
			}

			// Update visualization ---------------------------------------

			// Show 3D points:
			if (!view_freeze)
			{
				last_obs->generatePointCloud(pc_params);

				CColouredPointsMap pntsMap;
				pntsMap.loadFromVelodyneScan(*last_obs);

				win3D.get3DSceneAndLock();
				gl_points->loadFromPointsMap(&pntsMap);
				win3D.unlockAccess3DScene();
			}

			// Estimated grabbing rate:
			win3D.get3DSceneAndLock();
			win3D.addTextMessage(
				-150, -20, format("%.02f Hz", thrPar.Hz), TColorf(1, 1, 1), 100,
				MRPT_GLUT_BITMAP_HELVETICA_18);
			win3D.unlockAccess3DScene();
			do_view_refresh = true;
		}  // end update visualization:

		// Force opengl repaint:
		if (do_view_refresh) win3D.repaint();

		// Process possible keyboard commands:
		// --------------------------------------
		if (win3D.keyHit() && thrPar.pushed_key == 0)
		{
			const int key = tolower(win3D.getPushedKey());

			switch (key)
			{
				// Some of the keys are processed in this thread:
				case 'o':
					win3D.setCameraZoom(win3D.getCameraZoom() * 1.2);
					win3D.repaint();
					break;
				case 'i':
					win3D.setCameraZoom(win3D.getCameraZoom() / 1.2);
					win3D.repaint();
					break;
				case ' ':
					view_freeze = !view_freeze;
					break;
				case '1':
					pc_params.dualKeepLast = !pc_params.dualKeepLast;
					break;
				case '2':
					pc_params.dualKeepStrongest = !pc_params.dualKeepStrongest;
					break;
				// ...and the rest in the sensor thread:
				default:
					thrPar.pushed_key = key;
					break;
			};
		}

		win3D.get3DSceneAndLock();
		win3D.addTextMessage(
			5, 10,
			"'o'/'i'-zoom out/in, mouse: orbit 3D, spacebar: freeze, ESC: quit",
			TColorf(1, 1, 1), "mono", 10.0, mrpt::opengl::NICE, 110);
		win3D.addTextMessage(
			5, 25,
			mrpt::format(
				"'1'/'2': Toggle view dual last (%s)/strongest(%s) returns.",
				pc_params.dualKeepLast ? "ON" : "OFF",
				pc_params.dualKeepStrongest ? "ON" : "OFF"),
			TColorf(1, 1, 1), "mono", 10.0, mrpt::opengl::NICE, 111);
		win3D.unlockAccess3DScene();

		std::this_thread::sleep_for(50ms);
	}

	cout << "Waiting for grabbing thread to exit...\n";
	thrPar.quit = true;
	thHandle.join();
	cout << "Bye!\n";
	return 0;
}
예제 #3
0
// ======================================================================
//     main() of graph-slam
// ======================================================================
int main(int argc, char **argv)
{
	vector<TCLAP::Arg*> arg_ops;  // to be destroyed on exit.
	int ret_val = 0;

	try
	{
		// --------------- List of possible operations ---------------
		map<string,TOperationFunctor>  ops_functors;

		arg_ops.push_back(new TCLAP::SwitchArg("","levmarq",
			"Op: Optimizes the graph with sparse Levenberg-Marquartd using global coordinates (via mrpt::graphslam::optimize_graph_spa_levmarq).\n"
			"   Can be used together with: --view, --output, --max-iters, --no-span, --initial-lambda"
			,cmd, false) );
		ops_functors["levmarq"] = &op_levmarq;

		arg_ops.push_back(new TCLAP::SwitchArg("","dijkstra",
			"Op: Executes CNetworkOfPoses::dijkstra_nodes_estimate() to estimate the global pose of nodes from a Dijkstra tree and the edge relative poses.\n"
			"   Can be used together with: --view, --output"
			,cmd, false) );
		ops_functors["dijkstra"] = &op_dijkstra;

		arg_ops.push_back(new TCLAP::SwitchArg("","info",
			"Op: Loads the graph and displays statistics and information on it.\n"
			,cmd, false) );
		ops_functors["info"] = &op_info;
		// --------------- End of list of possible operations --------

		// Parse arguments:
		if (!cmd.parse( argc, argv ))
			throw std::runtime_error(""); // should exit.

		// Exactly 1 or --2d & --3d must be specified:
		if ( (arg_2d.isSet() && arg_3d.isSet()) || (!arg_2d.isSet() && !arg_3d.isSet()) )
			throw std::runtime_error("Exactly one --2d or --3d must be used.");

		const bool is3d = arg_3d.isSet();

		string input_file  = arg_input_file.getValue();
		const bool verbose = !arg_quiet.getValue();

		// Check the selected operation:
		//  Only one of the ops should be selected:
		string selected_op;
		for (size_t i=0;i<arg_ops.size();i++)
			if (arg_ops[i]->isSet())
			{
				if (selected_op.empty())
				{
					selected_op = arg_ops[i]->getName();
				}
				else	throw std::runtime_error(
					"Exactly one operation must be indicated on command line.\n"
					"Use --help to see the list of possible operations.");
			}

		// The "--view" argument needs a bit special treatment:
		if (selected_op.empty())
		{
			if (!arg_view.isSet())
				throw std::runtime_error(
					"Don't know what to do: No operation was indicated.\n"
					"Use --help to see the list of possible operations.");
			else
			{
				VERBOSE_COUT << "Operation to perform: " "view" << endl;
				op_view(input_file,is3d,cmd,verbose);
			}
		}
		else
		{
			VERBOSE_COUT << "Operation to perform: " << selected_op << endl;

			// ------------------------------------
			//  EXECUTE THE REQUESTED OPERATION
			// ------------------------------------
			ASSERTMSG_(ops_functors.find(selected_op)!=ops_functors.end(), "Internal error: Unknown operation functor!")

			// Call the selected functor:
			ops_functors[selected_op](input_file,is3d,cmd,verbose);
		}

		// successful end of program.
		ret_val = 0;
	}
	catch(std::exception &e)
	{
		if (strlen(e.what())) std::cerr << e.what() << std::endl;
		ret_val = -1;
	}

	// Free mem:
	for (size_t i=0;i<arg_ops.size();i++)
		delete arg_ops[i];

	// end:
	return ret_val;
}
예제 #4
0
int main(int argc, char **argv)
{
	try
	{
		printf(" gps2rawlog - Part of the MRPT\n");
		printf(" MRPT C++ Library: %s - Sources timestamp: %s\n", MRPT_getVersion().c_str(), MRPT_getCompilationDate().c_str());

		// Parse arguments:
		if (!cmd.parse( argc, argv ))
			throw std::runtime_error(""); // should exit.

		const string input_gps_file  = arg_input_file.getValue();
		string output_rawlog_file = arg_output_file.getValue();
		if (output_rawlog_file.empty())
			output_rawlog_file = mrpt::system::fileNameChangeExtension(input_gps_file,"rawlog");

		ASSERT_FILE_EXISTS_(input_gps_file)

		// Open input rawlog:
		CFileGZInputStream  fil_input;
		cout << "Opening for reading: '" << input_gps_file << "'...\n";
		fil_input.open(input_gps_file);
		cout << "Open OK.\n";

		// Open output:
		if (mrpt::system::fileExists(output_rawlog_file) && !arg_overwrite.isSet())
		{
			cout << "Output file already exists: `"<<output_rawlog_file<<"`, aborting. Use `-w` flag to overwrite.\n";
			return 1;
		}

		CFileGZOutputStream fil_out;
		cout << "Opening for writing: '" << output_rawlog_file << "'...\n";
		if (!fil_out.open(output_rawlog_file))
			throw std::runtime_error("Error writing file!");

		// GPS object:
		CGPSInterface  gps_if;
		gps_if.bindStream(&fil_input);
		
		// ------------------------------------
		//  Parse:
		// ------------------------------------
		while ( !fil_input.checkEOF() )
		{
			gps_if.doProcess();

			CGenericSensor::TListObservations lst_obs;
			gps_if.getObservations(lst_obs);

			printf("%u bytes parsed, %u new observations identified...\n",(unsigned)fil_input.getPosition(),(unsigned)lst_obs.size());
			for (CGenericSensor::TListObservations::const_iterator it=lst_obs.begin();it!=lst_obs.end();++it) {
				fil_out << *it->second;
			}
		}

		// successful end of program.
		return 0;
	}
	catch(std::exception &e)
	{
		if (strlen(e.what())) std::cerr << e.what() << std::endl;
		return 1;
	}
} // end of main()
int main(const int argc, const char* argv[]) {
  std::cout << "DatCC v0.2 created by pastelmind\n" << std::endl;
  datcc::setCurrentProgramDir(argv[0]);

  try {
    const char exampleStr[] = "EXAMPLES:"
      "\nDatCC -d -u ."
      "\n\tDecompiles the default units.dat file."
      "\nDatCC -c -w \"C:\\My Mod\\my weapons.ini\""
      "\n\tCompiles \"C:\\My Mod\\my weapons.ini\" into \"C:\\My Mod\\my weapons.dat\""
      "\nDatCC -c -t \"C:\\test\\tech.ini\" -b C:\\test\\techdata.dat"
      "\n\tCompiles \"C:\\test\\tech.ini\" into \"C:\\test\\tech.dat\", using C:\\test\\techdata.dat as the base DAT file"
      "\nDatCC -r -f \"example mod-flingy.dat\" output.ini"
      "\n\tCompares \"example mod-flingy.dat\" with the default flingy.dat and save the differences to output.ini";
    TCLAP::CmdLine cmd(exampleStr, ' ', "0.1");

    TCLAP::SwitchArg isCompileModeArg  ("c", "compile",   "Compiles INI files to DAT files.");
    TCLAP::SwitchArg isDecompileModeArg("d", "decompile", "Decompiles DAT files to INI files.");
    TCLAP::SwitchArg isCompareModeArg  ("r", "compare",   "Compares the DAT file with the base DAT file and decompiles the differences to an INI file");

    std::vector<TCLAP::Arg*> modeSwitchArgs;
    modeSwitchArgs.push_back(&isCompileModeArg);
    modeSwitchArgs.push_back(&isDecompileModeArg);
    modeSwitchArgs.push_back(&isCompareModeArg);
    cmd.xorAdd(modeSwitchArgs);

    TCLAP::ValueArg<std::string> baseDatArg("b", "basedat",
      "Base DAT file to use when compiling/comparing. If omitted, the default DAT files are used.",
      false, ".", "base file");
    cmd.add(baseDatArg);

    TCLAP::UnlabeledValueArg<std::string> inputFileArg("input",
      "In compile mode, specify the INI file to compile. In decompile or compare mode, specify the DAT file to decompile or compare. Use . to decompile the default DAT files.",
      true, "", "input file");
    cmd.add(inputFileArg);

    TCLAP::UnlabeledValueArg<std::string> outputFileArg("output",
      "Specify the output DAT file (in compile mode) or INI file (in decompile/compare mode). If omitted, the output file is named after the input file.",
      false, "", "output file");
    cmd.add(outputFileArg);

    TCLAP::SwitchArg useUnitsDatArg   ("u", "units",    "Operate on units.dat");
    TCLAP::SwitchArg useWeaponsDatArg ("w", "weapons",  "Operate on weapons.dat");
    TCLAP::SwitchArg useFlingyDatArg  ("f", "flingy",   "Operate on flingy.dat");
    TCLAP::SwitchArg useSpritesDatArg ("s", "sprites",  "Operate on sprites.dat");
    TCLAP::SwitchArg useImagesDatArg  ("i", "images",   "Operate on images.dat");
    TCLAP::SwitchArg useUpgradesDatArg("g", "upgrades", "Operate on upgrades.dat");
    TCLAP::SwitchArg useTechdataDatArg("t", "techdata", "Operate on techdata.dat");
    TCLAP::SwitchArg useSfxdataDatArg ("x", "sfxdata",  "Operate on sfxdata.dat");
    //TCLAP::SwitchArg usePortdataDatArg("p", "portdata", "Operate on portdata.dat (NOT SUPPORTED YET!)");
    //TCLAP::SwitchArg useMapdataDatArg ("m", "mapdata",  "Operate on mapdata.dat (NOT SUPPORTED YET)");
    TCLAP::SwitchArg useOrdersDatArg  ("o", "orders",   "Operate on orders.dat");

    std::vector<TCLAP::Arg*> datSwitchArgs;
    datSwitchArgs.push_back(&useUnitsDatArg);
    datSwitchArgs.push_back(&useWeaponsDatArg);
    datSwitchArgs.push_back(&useFlingyDatArg);
    datSwitchArgs.push_back(&useSpritesDatArg);
    datSwitchArgs.push_back(&useImagesDatArg);
    datSwitchArgs.push_back(&useUpgradesDatArg);
    datSwitchArgs.push_back(&useTechdataDatArg);
    datSwitchArgs.push_back(&useSfxdataDatArg);
    //datSwitchArgs.push_back(&usePortdataDatArg);
    //datSwitchArgs.push_back(&useMapdataDatArg);
    datSwitchArgs.push_back(&useOrdersDatArg);
    cmd.xorAdd(datSwitchArgs);

    cmd.parse(argc, argv);

    //-------- Main program logic start --------//

    datcc::loadData();

    if (isCompileModeArg.isSet()) {
      //Compile mode
      if (useUnitsDatArg.isSet())
        datcc::compileUnits(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());

      else if (useWeaponsDatArg.isSet())
        datcc::compileWeapons(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
      
      else if (useFlingyDatArg.isSet())
        datcc::compileFlingy(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());

      else if (useSpritesDatArg.isSet())
        datcc::compileSprites(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());

      else if (useImagesDatArg.isSet())
        datcc::compileImages(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());

      else if (useUpgradesDatArg.isSet())
        datcc::compileUpgrades(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());

      else if (useTechdataDatArg.isSet())
        datcc::compileTechdata(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());

      else if (useSfxdataDatArg.isSet())
        datcc::compileSfxdata(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
      
      else if (useOrdersDatArg.isSet())
        datcc::compileOrders(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
      
      else
        throw TCLAP::ArgException("Unsupported DAT file format, please wait for new version.",
          "UnsupportedFormat", "Unsupported DAT format exception");
    }
    else if (isDecompileModeArg.isSet()) {
      if (baseDatArg.isSet())
        throw TCLAP::ArgException("Base DAT argument is unnecessary for decompile mode", "unused_basedat", "Unused base DAT argument");

      //Decompile mode
      if (useUnitsDatArg.isSet())
        datcc::decompileUnits(inputFileArg.getValue(), outputFileArg.getValue());
      
      else if (useWeaponsDatArg.isSet())
        datcc::decompileWeapons(inputFileArg.getValue(), outputFileArg.getValue());

      else if (useFlingyDatArg.isSet())
        datcc::decompileFlingy(inputFileArg.getValue(), outputFileArg.getValue());

      else if (useSpritesDatArg.isSet())
        datcc::decompileSprites(inputFileArg.getValue(), outputFileArg.getValue());

      else if (useImagesDatArg.isSet())
        datcc::decompileImages(inputFileArg.getValue(), outputFileArg.getValue());

      else if (useUpgradesDatArg.isSet())
        datcc::decompileUpgrades(inputFileArg.getValue(), outputFileArg.getValue());

      else if (useTechdataDatArg.isSet())
        datcc::decompileTechdata(inputFileArg.getValue(), outputFileArg.getValue());

      else if (useSfxdataDatArg.isSet())
        datcc::decompileSfxdata(inputFileArg.getValue(), outputFileArg.getValue());

      else if (useOrdersDatArg.isSet())
        datcc::decompileOrders(inputFileArg.getValue(), outputFileArg.getValue());

      else
        throw TCLAP::ArgException("Unsupported DAT file format, please wait for new version.",
          "UnsupportedFormat", "Unsupported DAT format exception");
    }
    else if (isCompareModeArg.isSet()) {
      //Compare mode
      if (useUnitsDatArg.isSet())
        datcc::compareUnits(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());

      else if (useWeaponsDatArg.isSet())
        datcc::compareWeapons(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
      
      else if (useFlingyDatArg.isSet())
        datcc::compareFlingy(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());

      else if (useSpritesDatArg.isSet())
        datcc::compareSprites(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());

      else if (useImagesDatArg.isSet())
        datcc::compareImages(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());

      else if (useUpgradesDatArg.isSet())
        datcc::compareUpgrades(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());

      else if (useTechdataDatArg.isSet())
        datcc::compareTechdata(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
      
      else if (useSfxdataDatArg.isSet())
        datcc::compareSfxdata(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());

      else if (useOrdersDatArg.isSet())
        datcc::compareOrders(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
      
      else
        throw TCLAP::ArgException("Unsupported DAT file format, please wait for new version.",
          "UnsupportedFormat", "Unsupported DAT format exception");
    }
    else { //Should never reach here
      throw TCLAP::ArgException("Cannot determine compile/decompile mode");
    }
  }
  catch (TCLAP::ArgException &e) {
    std::cerr << "Error: " << e.what() << std::endl;
  }

  return 0;
}
예제 #6
0
int dem_gmrf_main(int argc, char **argv)
{
	if (!cmd.parse( argc, argv )) // Parse arguments:
		return 1; // should exit.

	mrpt::utils::CTimeLogger timlog;

	printf(" dem-gmrf (C) University of Almeria\n");
	printf(" Powered by %s - BUILD DATE %s\n", MRPT_getVersion().c_str(), MRPT_getCompilationDate().c_str());
	printf("-------------------------------------------------------------------\n");

	const std::string sDataFile = arg_in_file.getValue();
	ASSERT_FILE_EXISTS_(sDataFile);
	const string sPrefix = arg_out_prefix.getValue();

	printf("\n[1] Loading `%s`...\n", sDataFile.c_str());
	timlog.enter("1.load_dataset");

	CMatrix raw_xyz;
	raw_xyz.loadFromTextFile(sDataFile.c_str());
	const size_t N = raw_xyz.rows(), nCols = raw_xyz.cols();
	printf("[1] Done. Points: %7u  Columns: %3u\n", (unsigned int)N, (unsigned int)nCols);

	timlog.leave("1.load_dataset");
	ASSERT_(nCols>=3);

	// File types: 
	// * 3 columns: x y z
	// * 4 columns: x y z stddev 
	// Z: 1e+38 error en raster (preguntar no data)
	const bool all_readings_same_stddev = nCols==3;

	// ---------------
	printf("\n[2] Determining bounding box...\n");
	timlog.enter("2.bbox");

	double minx = std::numeric_limits<double>::max();
	double miny = std::numeric_limits<double>::max();
	double minz = std::numeric_limits<double>::max();
	double maxx = -std::numeric_limits<double>::max();
	double maxy = -std::numeric_limits<double>::max();
	double maxz = -std::numeric_limits<double>::max();

	for (size_t i=0;i<N;i++)
	{
		const mrpt::math::TPoint3D pt( raw_xyz(i,0),raw_xyz(i,1),raw_xyz(i,2) );
		mrpt::utils::keep_max(maxx,pt.x); mrpt::utils::keep_min(minx,pt.x);
		mrpt::utils::keep_max(maxy,pt.y); mrpt::utils::keep_min(miny,pt.y);
		if (std::abs(pt.z)<1e6) {
			mrpt::utils::keep_max(maxz,pt.z); mrpt::utils::keep_min(minz,pt.z);
		}
	}

	const double BORDER = 10.0;
	minx-= BORDER; maxx += BORDER;
	miny-= BORDER; maxy += BORDER;
	minz-= BORDER; maxz += BORDER;

	timlog.leave("2.bbox");
	printf("[2] Bbox: x=%11.2f <-> %11.2f (D=%11.2f)\n", minx,maxx,maxx-minx);
	printf("[2] Bbox: y=%11.2f <-> %11.2f (D=%11.2f)\n", miny,maxy,maxy-miny);
	printf("[2] Bbox: z=%11.2f <-> %11.2f (D=%11.2f)\n", minz,maxz,maxz-minz);


	// ---------------
	printf("\n[3] Picking random checkpoints...\n");
	timlog.enter("3.select_chkpts");

	const double chkpts_ratio = arg_checkpoints_ratio.getValue();
	ASSERT_(chkpts_ratio>=0.0 && chkpts_ratio <=1.0);

	// Generate a list with all indices, then keep the first "N-Nchk" for insertion in the map, "Nchk" as checkpoints
	std::vector<size_t> pts_indices;
	mrpt::math::linspace((size_t)0,N-1,N, pts_indices);

	std::srand (unsigned(std::time(0)));
	std::random_shuffle(pts_indices.begin(), pts_indices.end());
	const size_t N_chk_pts    = mrpt::utils::round( chkpts_ratio * N );
	const size_t N_insert_pts = N - N_chk_pts;

	timlog.leave("3.select_chkpts");
	printf("[3] Checkpoints: %9u (%.02f%%)  Rest of points: %9u\n", (unsigned)N_chk_pts, 100.0*chkpts_ratio, (unsigned)N_insert_pts );
	
	// ---------------
	printf("\n[4] Initializing RMF DEM map estimator...\n");
	timlog.enter("4.dem_map_init");

	const double RESOLUTION = arg_dem_resolution.getValue();

	mrpt::maps::CHeightGridMap2D_MRF  dem_map( CRandomFieldGridMap2D::mrGMRF_SD /*map type*/, 0,1, 0,1, 0.5, false /* run_first_map_estimation_now */); // dummy initial size
	
	// Set map params:
	dem_map.insertionOptions.GMRF_lambdaPrior = 1.0/ mrpt::utils::square( arg_std_prior.getValue() );
	dem_map.insertionOptions.GMRF_lambdaObs   = 1.0/ mrpt::utils::square( arg_std_observations.getValue() );
	dem_map.insertionOptions.GMRF_skip_variance = arg_skip_variance.isSet();

	// Resize to actual map extension:
	{
		TRandomFieldCell def(0,0); // mean, std
		dem_map.setSize(minx,maxx,miny,maxy,RESOLUTION,&def);
	}

	timlog.leave("4.dem_map_init");
	printf("[4] Done.\n");

	dem_map.enableVerbose(true);
	dem_map.enableProfiler(true);

	// ---------------
	printf("\n[5] Inserting %u points in DEM map...\n",(unsigned)N_insert_pts);
	timlog.enter("5.dem_map_insert_points");

	for (size_t k=0;k<N_insert_pts;k++)
	{
		const size_t i=pts_indices[k];
		const mrpt::math::TPoint3D pt( raw_xyz(i,0),raw_xyz(i,1),raw_xyz(i,2) );
		
		double reading_stddev;
		if (all_readings_same_stddev) {
			reading_stddev = arg_std_observations.getValue();
		}
		else {
			reading_stddev = raw_xyz(i, 3);
		}

		dem_map.insertIndividualReading(
			pt.z, 
			TPoint2D(pt.x,pt.y), 
			false /* do not update map now */,
			true /*time invariant*/, 
			reading_stddev );
	}
	timlog.leave("5.dem_map_insert_points");
	printf("[5] Done.\n");

	// ---------------
	printf("\n[6] Running GMRF estimator (cell count=%e)...\n",(double)(dem_map.getSizeX()*dem_map.getSizeY()));
	timlog.enter("6.dem_map_update_gmrf");

	dem_map.updateMapEstimation();

	timlog.leave("6.dem_map_update_gmrf");
	printf("[6] Done.\n");

	// ---------------
	if (N_chk_pts)
	{
		printf("\n[7] Eval checkpoints...\n");
		timlog.enter("7.eval_chkpts");

		Eigen::VectorXd  residuals_NN(N_chk_pts), residuals_Bi(N_chk_pts);

		for (size_t k=0;k<N_chk_pts;k++)
		{
			const size_t i=pts_indices[k+N_insert_pts];

			// Neirest neighbor:
			double dem_z_NN, dem_std_NN;
			dem_map.predictMeasurement(raw_xyz(i,0),raw_xyz(i,1), dem_z_NN, dem_std_NN, false /* sensor normalization */, CRandomFieldGridMap2D::gimNearest);
			residuals_NN[k] = raw_xyz(i,2) - dem_z_NN;

			// Bilinear interp:
			double dem_z_Bi, dem_std_Bi;
			dem_map.predictMeasurement(raw_xyz(i,0),raw_xyz(i,1), dem_z_Bi, dem_std_Bi, false /* sensor normalization */, CRandomFieldGridMap2D::gimBilinear);
			residuals_Bi[k] = raw_xyz(i,2) - dem_z_Bi;
		}

		// Residuals:
		residuals_NN.saveToTextFile( sPrefix + string("_chkpt_residuals_NN.txt") );
		residuals_Bi.saveToTextFile( sPrefix + string("_chkpt_residuals_Bi.txt") );

		// Residuals stats:
		std::string stats_hdr;
		Eigen::VectorXd residuals_NN_stats,residuals_Bi_stats;
		do_residuals_stats(residuals_NN, residuals_NN_stats,stats_hdr);
		do_residuals_stats(residuals_Bi, residuals_Bi_stats,stats_hdr);

		residuals_NN_stats.saveToTextFile( sPrefix + string("_chkpt_residuals_NN_stats.txt"), MATRIX_FORMAT_ENG, false, stats_hdr );
		residuals_Bi_stats.saveToTextFile( sPrefix + string("_chkpt_residuals_Bi_stats.txt"), MATRIX_FORMAT_ENG, false, stats_hdr );

		timlog.leave("7.eval_chkpts");
		printf("[7] Done.\n");
	}
	// ---------------
	printf("\n[9] Generate TXT output files...\n");
	timlog.enter("9.save_points");
	{
		CFileOutputStream  fil_pts_map( sPrefix + string("_pts_map.txt") );
		for (size_t k=0;k<N_insert_pts;k++)
		{
			const size_t i=pts_indices[k];
			fil_pts_map.printf("%f, %f, %f\n",raw_xyz(i,0),raw_xyz(i,1),raw_xyz(i,2));
		}
	}
	{
		CFileOutputStream  fil_pts_chk( sPrefix + string("_pts_chk.txt") );
		for (size_t k=0;k<N_chk_pts;k++)
		{
			const size_t i=pts_indices[k+N_insert_pts];
			fil_pts_chk.printf("%f, %f, %f\n",raw_xyz(i,0),raw_xyz(i,1),raw_xyz(i,2));
		}
	}

	dem_map.saveMetricMapRepresentationToFile(sPrefix + string("_grmf") );
	dem_map.saveAsMatlab3DGraph(sPrefix + string("_grmf_draw.m") );

	timlog.leave("9.save_points");
	printf("[9] Done.\n");

#if MRPT_HAS_WXWIDGETS
	if (!arg_no_gui.isSet())
	{
		registerClass( CLASS_ID( CSetOfObjects ) );

		// 3D view:
		mrpt::opengl::CSetOfObjectsPtr glObj_mean = mrpt::opengl::CSetOfObjects::Create();
		mrpt::opengl::CSetOfObjectsPtr glObj_var  = mrpt::opengl::CSetOfObjects::Create();
		dem_map.getAs3DObject( glObj_mean, glObj_var );
		glObj_mean->setLocation( -0.5*(minx+maxx), -0.5*(miny+maxy), -0.5*(minz+maxz) );
		glObj_var->setLocation( -0.5*(minx+maxx) + 1.1*(maxx-minx), -0.5*(miny+maxy), -0.5*(minz+maxz) );

		mrpt::gui::CDisplayWindow3D win("Map",640,480);
		win.setCameraZoom( mrpt::utils::max3( maxz-minz, maxx-minx, maxy-miny) );
		win.setMinRange(0.1);
		win.setMaxRange(1e7);
		mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock();
		scene->insert( glObj_mean );
		//scene->insert( glObj_var );
		win.unlockAccess3DScene();
		win.repaint();

		win.waitForKey();
	}
#endif
	return 0;
}