Beispiel #1
0
// Main
// ////////////////////////////////////////////////////////////
int main(int argc, char** argv)
{
	// initializign the logger instance
	COutputLogger logger("graphslam-engine_app");
	logger.logging_enable_keep_record = true;

	try
	{
		bool showHelp = argc > 1 && !os::_strcmp(argv[1], "--help");
		bool showVersion = argc > 1 && !os::_strcmp(argv[1], "--version");

		// Input Validation
		cmd_line.xorAdd(dim_2d, dim_3d);
		if (!cmd_line.parse(argc, argv) || showVersion || showHelp)
		{
			return 0;
		}

		// CGraphSlamEngine initialization
		if (dim_2d.getValue())
		{
			execGraphSlamEngine<CNetworkOfPoses2DInf>(&logger);
		}
		else
		{
			execGraphSlamEngine<CNetworkOfPoses3DInf>(&logger);
		}

		return 0;
	}
	catch (exception& e)
	{
		logger.logFmt(
			LVL_ERROR, "Program finished due to an exception!!\n%s\n",
			e.what());
		printf("%s", e.what());

		mrpt::system::pause();
		return -1;
	}
	catch (...)
	{
		logger.logFmt(
			LVL_ERROR, "Program finished due to an untyped exception!!");
		mrpt::system::pause();
		return -1;
	}

	return 0;
}
Beispiel #2
0
            gnuplot_prog ( TCLAP::CmdLine& cmd, shared_ptr<KeyGenerators::base> _keygen ) : base ( cmd, _keygen ),
                dataNameArg ( "o", "output", "Gnuplot data (all keys with correlation coefficient) file name", true, "", "path" ),
                scriptNameArg ( "s", "script-output", "Gnuplot script output (all keys with correlation coefficient) file name", true, "", "path" ),
                confidenceDataNameArg ( "r", "confidence-output", "Gnuplot data (best and second best keys with confidence interval) file name", true, "", "path" ),
                confidenceScriptNameArg ( "g", "confidence-script-output", "Gnuplot script output (best and second best keys with confidence interval) file name", true, "", "path" ),
                alphaArg ( "a", "alpha", "The alpha to compute the (1 - alpha) confidence interval", true, 0.05, "0-1" ),
                bestPearson() {
                bestPearson = Trace::Zero(KEYNUM);
				cmd.add ( dataNameArg );
				cmd.add ( scriptNameArg );
                cmd.add ( confidenceDataNameArg );
                cmd.add ( confidenceScriptNameArg );
                cmd.add ( alphaArg );
                currentTraces = 0;
			};
Beispiel #3
0
int main(int argc, char* argv[]) { //{{{
// 	Random semilla_uran;
// 	itpp::RNG_reset(semilla_uran.strong());
//   	cout << PurityRMT::QubitEnvironmentHamiltonian(3,0.) <<endl;
// 	cout << RMT::FlatSpectrumGUE(5,.1) <<endl;
//
  cmd.parse( argc, argv );
  cout.precision(16);

  string option=optionArg.getValue();
  if (option=="test_kick_single_spin"){ // {{{// {{{

    int dim=pow_2(qubits.getValue());
    cvec state(dim);
    double x,y;
    ifstream myReadFile;
    myReadFile.open("/tmp/estado.dat");
    for (int i=0; i<dim; i++){
      myReadFile >> x >> y ;
      state(i) = complex<double>(x,y) ;
    }
    myReadFile.close();
    
    vec b(3); b(0)=bx.getValue(); b(1)=by.getValue(); b(2)=bz.getValue();
    apply_magnetic_kick(state,b,position.getValue());
    for (int i=0; i<dim; i++){
      cout << real(state(i)) << " " << real(-Im*state(i)) << endl;
    }//}}}
  } else if(option=="test_kick") { // {{{
Beispiel #4
0
bool isFlagSet(TCLAP::CmdLine& cmdline, const std::string& arg_name)
{
	using namespace TCLAP;

	std::list<Arg*>& args = cmdline.getArgList();
	for (auto& arg : args)
		if (arg->getName() == arg_name) return arg->isSet();
	return false;
}
Beispiel #5
0
bool isFlagSet(TCLAP::CmdLine &cmdline, const std::string &arg_name)
{
	using namespace TCLAP;

	std::list<Arg*>& args = cmdline.getArgList();
	for (std::list<Arg*>::iterator it=args.begin();it!=args.end();++it)
		if ( (*it)->getName() == arg_name)
			return (*it)->isSet();
	return false;
}
Beispiel #6
0
// }}}
int main(int argc, char* argv[]) { // {{{
  // {{{ initial definitions
  cmd.parse( argc, argv );
  int error=0;
  string option=optionArg.getValue();
  cout.precision(17); cerr.precision(17);
  // }}}
  // {{{ Set seed for random
  unsigned int semilla=seed.getValue();
  if (semilla == 0){
    Random semilla_uran; semilla=semilla_uran.strong();
  } 
  RNG_reset(semilla);
  // }}}
  // {{{ Report on the screen
  if(!no_general_report.getValue()){
    cout << "#linea de comando: "; 
    for(int i=0;i<argc;i++){ 
      cout <<argv[i]<<" " ;
    } cout << endl ;
    cout << "#semilla = " << semilla << endl; 
    error += system("echo \\#hostname: $(hostname)");
    error += system("echo \\#comenzando en: $(date)");
    error += system("echo \\#uname -a: $(uname -a)");
    error += system("echo \\#working dir: $(pwd)");
  }
  // }}}
  if //{{{ option == loquesea
    (option == "get_gse_spectrum"){ // {{{
      vec eig=eig_sym(RandomGSEDeltaOne(dimension.getValue()));
      for (int i=0; i<eig.size(); i++){  cout << eig(i) << endl;}
//       cout << eig << endl;
      return 0;
    } // }}}
  else if (option == "get_gse_flat_spectrum"){ // {{{
      vec eig=FlatSpectrumGSE(dimension.getValue());
      for (int i=0; i<eig.size(); i++){  cout << eig(i) << endl;}
//     cout << eig <<endl;
    return 0;
  } // }}}
  else if (option == "get_gse_flat_spectrum"){ // {{{
      vec eig=FlatSpectrumGSE(dimension.getValue());
      for (int i=0; i<eig.size(); i++){  cout << eig(i) << endl;}
//     cout << eig <<endl;
    return 0;
  } // }}}
  //}}}
} // }}}
Beispiel #7
0
bool getArgValue(TCLAP::CmdLine &cmdline, const std::string &arg_name, T &out_val)
{
	using namespace TCLAP;

	std::list<Arg*>& args = cmdline.getArgList();
	for (std::list<Arg*>::iterator it=args.begin();it!=args.end();++it)
	{
		if ( (*it)->getName() == arg_name)
		{
			// Is it set? Return the default value anyway:
			TCLAP::ValueArg<T> *arg = static_cast<TCLAP::ValueArg<T> *>(*it);
			out_val = arg->getValue();
			return (*it)->isSet();
		}
	}
	return false;
}
Beispiel #8
0
bool getArgValue(
	TCLAP::CmdLine& cmdline, const std::string& arg_name, T& out_val)
{
	using namespace TCLAP;

	std::list<Arg*>& args = cmdline.getArgList();
	for (auto& it : args)
	{
		if (it->getName() == arg_name)
		{
			// Is it set? Return the default value anyway:
			auto* arg = static_cast<TCLAP::ValueArg<T>*>(it);
			out_val = arg->getValue();
			return it->isSet();
		}
	}
	return false;
}
// }}}
int main(int argc, char* argv[]){ // {{{
  // {{{ initial definitions
  cmd.parse( argc, argv );
  int error=0;
  string option=optionArg.getValue();
  cout.precision(17); cerr.precision(17);
  // }}}
  // {{{ Set seed for random
  unsigned int semilla=seed.getValue();
  if (semilla == 0){
    Random semilla_uran; semilla=semilla_uran.strong();
  } 
  RNG_reset(semilla);
  // }}}
  // {{{ Report on the screen
  if(!no_general_report.getValue()){
    cout << "#linea de comando: "; 
    for(int i=0;i<argc;i++){ 
      cout <<argv[i]<<" " ;
    } cout << endl ;
    cout << "#semilla = " << semilla << endl; 
    error += system("echo \\#hostname: $(hostname)");
    error += system("echo \\#comenzando en: $(date)");
    error += system("echo \\#uname -a: $(uname -a)");
    error += system("echo \\#working dir: $(pwd)");
  }
  // }}}
  if //{{{ option == loquesea
    (option == "loquesea"){ // {{{
    // }}}
  } else if (option == "nichts") { // {{{
  // }}}
  } else { // {{{
    cout << "Error en la opcion. Mira, esto es lo que paso: "
      << optionArg.getValue() << endl;
  } // }}}
// }}}
  // {{{ Final report
  if(!no_general_report.getValue()){
    error += system("echo \\#terminando:    $(date)");
  }
  // }}}
  return 0;
} // }}}
Beispiel #10
0
// ======================================================================
//     main() of rawlog-edit
// ======================================================================
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("","externalize",
			"Op: convert to external storage.\n"
			"Requires: -o (or --output)\n"
			"Optional: --image-format, --txt-externals",cmd, false) );
		ops_functors["externalize"] = &op_externalize;

		arg_ops.push_back(new TCLAP::SwitchArg("","info",
			"Op: parse input file and dump information and statistics.",cmd, false) );
		ops_functors["info"] = &op_info;

		arg_ops.push_back(new TCLAP::SwitchArg("","list-images",
			"Op: dump a list of all external image files in the dataset.\n"
			"Optionally the output text file can be changed with --text-file-output."
			,cmd, false) );
		ops_functors["list-images"] = &op_list_images;

		arg_ops.push_back(new TCLAP::SwitchArg("","list-poses",
			"Op: dump a list of all the poses of the observations in the dataset.\n"
			"Optionally the output text file can be changed with --text-file-output."
			,cmd, false) );
		ops_functors["list-poses"] = &op_list_poses;

		arg_ops.push_back(new TCLAP::SwitchArg("","list-timestamps",
			"Op: generates a list with all the observations' timestamp, sensor label and C++ class name.\n"
			"Optionally the output text file can be changed with --text-file-output."
			,cmd, false) );
		ops_functors["list-timestamps"] = &op_list_timestamps;

		arg_ops.push_back(new TCLAP::ValueArg<std::string>("","remap-timestamps",
			"Op: Change all timestamps t replacing it with the linear map 'a*t+b'."
			"The parameters 'a' and 'b' must be given separated with a semicolon.\n"
			"Requires: -o (or --output)",false,"","a;b",cmd) );
		ops_functors["remap-timestamps"] = &op_remap_timestamps;

		arg_ops.push_back(new TCLAP::SwitchArg("","list-range-bearing",
			"Op: dump a list of all landmark observations of type range-bearing.\n"
			"Optionally the output text file can be changed with --text-file-output."
			,cmd, false) );
		ops_functors["list-range-bearing"] = &op_list_rangebearing;

		arg_ops.push_back(new TCLAP::ValueArg<std::string>("","remove-label",
			"Op: Remove all observation matching the given sensor label(s)."
			"Several labels can be provided separated by commas.\n"
			"Requires: -o (or --output)",false,"","label[,label...]",cmd) );
		ops_functors["remove-label"] = &op_remove_label;

		arg_ops.push_back(new TCLAP::ValueArg<std::string>("","keep-label",
			"Op: Remove all observations not matching the given sensor label(s)."
			"Several labels can be provided separated by commas.\n"
			"Requires: -o (or --output)",false,"","label[,label...]",cmd) );
		ops_functors["keep-label"] = &op_keep_label;

		arg_ops.push_back(new TCLAP::SwitchArg("","export-gps-kml",
			"Op: Export GPS paths to Google Earth KML files.\n"
			"Generates one .kml file with different sections for each different sensor label of GPS observations in the dataset. "
			"The generated .kml files will be saved in the same path than the input rawlog, with the same "
			"filename + each sensorLabel."
			,cmd,false) );
		ops_functors["export-gps-kml"] = &op_export_gps_kml;

		arg_ops.push_back(new TCLAP::SwitchArg("","export-gps-gas-kml",
			"Op: Export GPS paths to Google Earth KML files coloured by the gas concentration.\n"
			"Generates one .kml file with different sections for each different sensor label of GPS observations in the dataset. "
			"The generated .kml files will be saved in the same path than the input rawlog, with the same "
			"filename + each sensorLabel."
			,cmd,false) );
		ops_functors["export-gps-gas-kml"] = &op_export_gps_gas_kml;

		arg_ops.push_back(new TCLAP::SwitchArg("","export-gps-txt",
			"Op: Export GPS GPGGA messages to TXT files.\n"
			"Generates one .txt file for each different sensor label of GPS observations in the dataset. "
			"The generated .txt files will be saved in the same path than the input rawlog, with the same "
			"filename + each sensorLabel."
			,cmd,false) );
		ops_functors["export-gps-txt"] = &op_export_gps_txt;

		arg_ops.push_back(new TCLAP::SwitchArg("","export-gps-all",
			"Op: Generic export all kinds of GPS/GNSS messages to separate TXT files.\n"
			"Generates one .txt file for each different sensor label and for each "
			"message type in the dataset, with a first header line describing each field."
			,cmd,false) );
		ops_functors["export-gps-all"] = &op_export_gps_all;

		arg_ops.push_back(new TCLAP::SwitchArg("","export-imu-txt",
			"Op: Export IMU readings to TXT files.\n"
			"Generates one .txt file for each different sensor label of an IMU observation in the dataset. "
			"The generated .txt files will be saved in the same path than the input rawlog, with the same "
			"filename + each sensorLabel."
			,cmd,false) );
		ops_functors["export-imu-txt"] = &op_export_imu_txt;

		arg_ops.push_back(new TCLAP::SwitchArg("","export-odometry-txt",
			"Op: Export absolute odometry readings to TXT files.\n"
			"Generates one .txt file for each different sensor label of an odometry observation in the dataset. "
			"The generated .txt files will be saved in the same path than the input rawlog, with the same "
			"filename + each sensorLabel."
			,cmd,false) );
		ops_functors["export-odometry-txt"] = &op_export_odometry_txt;

		arg_ops.push_back(new TCLAP::SwitchArg("", "export-enose-txt",
			"Op: Export e-nose readigns to TXT files.\n"
			"Generates one .txt file for each different sensor label of an e-nose observation in the dataset. "
			"The generated .txt files will be saved in the same path than the input rawlog, with the same "
			"filename + each sensorLabel."
			, cmd, false));
		ops_functors["export-enose-txt"] = &op_export_enose_txt;

		arg_ops.push_back(new TCLAP::SwitchArg("", "export-anemometer-txt",
			"Op: Export anemometer readigns to TXT files.\n"
			"Generates one .txt file for each different sensor label of an anemometer observation in the dataset. "
			"The generated .txt files will be saved in the same path than the input rawlog, with the same "
			"filename + each sensorLabel."
			, cmd, false));
		ops_functors["export-anemometer-txt"] = &op_export_anemometer_txt;

		arg_ops.push_back(new TCLAP::SwitchArg("","recalc-odometry",
			"Op: Recomputes odometry increments from new encoder-to-odometry constants.\n"
			"Requires: -o (or --output)\n"
			"Requires: --odo-KL, --odo-KR and --odo-D.\n"
			,cmd,false) );
		ops_functors["recalc-odometry"] = &op_recalc_odometry;


		arg_ops.push_back(new TCLAP::SwitchArg("","export-rawdaq-txt",
			"Op: Export raw DAQ readings to TXT files.\n"
			"Generates one .txt file for each different sensor label + DAQ task. "
			"The generated .txt files will be saved in the same path than the input rawlog."
			,cmd,false) );
		ops_functors["export-rawdaq-txt"] = &op_export_rawdaq_txt;
	
		arg_ops.push_back(new TCLAP::SwitchArg("","export-2d-scans-txt",
			"Op: Export 2D scans to TXT files.\n"
			"Generates two .txt files for each different sensor label of 2D scan observations, one with "
			"the timestamps and the other with range data.\n"
			"The generated .txt files will be saved in the same path than the input rawlog, with the same "
			"filename + each sensorLabel."
			,cmd,false) );
		ops_functors["export-2d-scans-txt"] = &op_export_2d_scans_txt;

		arg_ops.push_back(new TCLAP::SwitchArg("","cut",
			"Op: Cut a part of the input rawlog.\n"
			"Requires: -o (or --output)\n"
			"Requires: At least one of --from-index, --from-time, --to-index, --to-time. Use only one of the --from-* and --to-* at once.\n"
			"If only a --from-* is given, the rawlog will be saved up to its end. If only a --to-* is given, the rawlog will be saved from its beginning.\n"
			,cmd,false) );
		ops_functors["cut"] = &op_cut;

		arg_ops.push_back(new TCLAP::SwitchArg("","generate-3d-pointclouds",
			"Op: (re)generate the 3D pointclouds within CObservation3DRangeScan objects that have range data.\n"
			"Requires: -o (or --output)\n"
			,cmd,false));
		ops_functors["generate-3d-pointclouds"] = &op_generate_3d_pointclouds;

		arg_ops.push_back(new TCLAP::SwitchArg("","generate-pcd",
			"Op: Generate a PointCloud Library (PCL) PCD file with the point cloud for each sensor observation that can be converted into"
			" this representation: laser scans, 3D camera images, etc.\n"
			"Optional: --out-dir to change the output directory (default: \"./\")\n"
			,cmd,false));
		ops_functors["generate-pcd"] = &op_generate_pcd;

		arg_ops.push_back(new TCLAP::ValueArg<std::string>("","sensors-pose",
			"Op: batch change the poses of sensors from a rawlog-grabber-like configuration file that specifies the pose of sensors by their sensorLabel names.\n"
			"Requires: -o (or --output)\n",
			false,"","file.ini",cmd) );
		ops_functors["sensors-pose"] = &op_sensors_pose;

		arg_ops.push_back(new TCLAP::ValueArg<std::string>("","camera-params",
			"Op: change the camera parameters of all CObservationImage's with the given SENSOR_LABEL, with new params loaded from the given file, section '[CAMERA_PARAMS]' "
			"for monocular cameras, or '[CAMERA_PARAMS_LEFT]' and '[CAMERA_PARAMS_RIGHT]' for stereo.\n"
			"Requires: -o (or --output)\n"
			,false,"","SENSOR_LABEL,file.ini",cmd) );
		ops_functors["camera-params"] = &op_camera_params;

		arg_ops.push_back(new TCLAP::ValueArg<std::string>("","stereo-rectify",
			"Op: creates a new set of external images for all CObservationStereoImages with the given SENSOR_LABEL, using the camera parameters stored in the "
			"observations (which must be a valid calibration) and with the given alpha value. Alpha can be -1 for auto, or otherwise be in the range [0,1] (see OpenCV's docs for cvStereoRectify).\n"
			"Requires: -o (or --output)\n"
			"Optional: --image-format to set image format (default=jpg), \n"
			"          --image-size to resize output images (example: --image-size 640x480) \n"
			,false,"","SENSOR_LABEL,0.5",cmd) );
		ops_functors["stereo-rectify"] = &op_stereo_rectify;

		arg_ops.push_back(new TCLAP::SwitchArg("","rename-externals",
			"Op: Renames all the external storage file names within the rawlog (it doesn't change the external files, which may even not exist).\n"
			,cmd,false));
		ops_functors["rename-externals"] = &op_rename_externals;



		// --------------- End of list of possible operations --------

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

		string input_rawlog  = 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.");
			}

		if (selected_op.empty())
		{
			throw std::runtime_error(
				"Don't know what to do: No operation was indicated.\n"
				"Use --help to see the list of possible operations.");
		}

		VERBOSE_COUT << "Operation to perform: " << selected_op << endl;

		// This will be done for any operation: open the input rawlog
		// ------------------------------------------------------------
		if (!mrpt::system::fileExists(input_rawlog))
			throw runtime_error(format("Input file doesn't exist: '%s'",input_rawlog.c_str()));

		// Open input rawlog:
		CFileGZInputStream  fil_input;
		VERBOSE_COUT << "Opening '" << input_rawlog << "'...\n";
		fil_input.open(input_rawlog);
		VERBOSE_COUT << "Open OK.\n";

		// External storage directory?
		CImage::IMAGES_PATH_BASE = CRawlog::detectImagesDirectory(input_rawlog);
		if (mrpt::system::directoryExists(CImage::IMAGES_PATH_BASE)) {
			VERBOSE_COUT << "Found external storage directory: " << CImage::IMAGES_PATH_BASE << "\n";
		}
		else {
			VERBOSE_COUT << "Warning: No external storage directory was found (not an issue if the rawlog does not contain delayed-load images).\n";
		}


		// ------------------------------------
		//  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](fil_input,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;
} // end of main()
Beispiel #11
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;
}
Beispiel #12
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()
Beispiel #13
0
// Main
// ////////////////////////////////////////////////////////////
int main(int argc, char **argv)
{
	// initializign the logger instance
	COutputLogger logger("graphslam-engine_app");
	logger.logging_enable_keep_record = true;

	try {

		bool showHelp		 = argc>1 && !os::_strcmp(argv[1],"--help");
		bool showVersion = argc>1 && !os::_strcmp(argv[1],"--version");

		// Instance for managing the available graphslam deciders optimizers
		TUserOptionsChecker graphslam_opts;

		// Input Validation
		if (!cmd_line.parse( argc, argv ) ||  showVersion || showHelp) {
			return 0;
		}
		// fetch the command line graphslam_opts
		// ////////////////////////////////////////////////////////////

		// decide whether to display the help messages for the deciders/optimizers
		{
			bool list_registrars = false;

			if (list_all_registrars.getValue()) {
				graphslam_opts.dumpRegistrarsToConsole("all");
				list_registrars = true;
			}
			if (list_node_registrars.getValue()) {
				graphslam_opts.dumpRegistrarsToConsole("node");
				list_registrars = true;
			}
			if (list_edge_registrars.getValue()) {
				graphslam_opts.dumpRegistrarsToConsole("edge");
				list_registrars = true;
			}

			if (list_optimizers.getValue()) {
				graphslam_opts.dumpOptimizersToConsole();
			}

			if (list_registrars || list_optimizers.getValue()) {
				logger.logFmt(LVL_INFO, "Exiting.. ");
				return 0;
			}
		}

		// fetch which registration deciders / optimizer to use
		string node_reg = arg_node_reg.getValue();
		string edge_reg = arg_edge_reg.getValue();
		string optimizer = arg_optimizer.getValue();
		ASSERTMSG_(graphslam_opts.checkRegistrationDeciderExists(node_reg, "node"),
				format("\nNode Registration Decider %s is not available.\n",
					node_reg.c_str()) );
		ASSERTMSG_(graphslam_opts.checkRegistrationDeciderExists(edge_reg, "edge"),
				format("\nEdge Registration Decider %s is not available.\n",
					edge_reg.c_str()) );
		ASSERTMSG_(graphslam_opts.checkOptimizerExists(optimizer),
				format("\nOptimizer %s is not available\n",
					optimizer.c_str()) );

		// fetch the filenames
		// ini file
		string ini_fname = arg_ini_file.getValue();
		// rawlog file
		string rawlog_fname = arg_rawlog_file.getValue();

		// ground-truth file
		string ground_truth_fname;
		if ( arg_ground_truth_file.isSet() ) {
			ground_truth_fname = arg_ground_truth_file.getValue();
		}

		if (disable_visuals.getValue()) { // enabling Visualization objects
			logger.logFmt(LVL_WARN, "Running on headless mode - Visuals disabled");
		}

		logger.logFmt(LVL_INFO, "Node registration decider: %s", node_reg.c_str());
		logger.logFmt(LVL_INFO, "Edge registration decider: %s", edge_reg.c_str());
		logger.logFmt(LVL_INFO, "graphSLAM Optimizer: %s", optimizer.c_str());

		// CGraphSlamHandler initialization
		CGraphSlamHandler graphslam_handler;
		graphslam_handler.setOutputLoggerPtr(&logger);
		graphslam_handler.readConfigFname(ini_fname);
		graphslam_handler.setRawlogFname(rawlog_fname);

		// Visuals initialization
		if (!disable_visuals.getValue()) {
			graphslam_handler.initVisualization();
		}

		// CGraphSlamEngine initialization
		CGraphSlamEngine<CNetworkOfPoses2DInf> graphslam_engine(
				ini_fname,
				rawlog_fname,
				ground_truth_fname,
				graphslam_handler.win_manager,
				graphslam_opts.node_regs_map[node_reg](),
				graphslam_opts.edge_regs_map[edge_reg](),
				graphslam_opts.optimizers_map[optimizer]());

		// print the problem parameters
		graphslam_handler.printParams();
		graphslam_engine.printParams();

		// Variables initialization
		CFileGZInputStream rawlog_stream(rawlog_fname);
		CActionCollectionPtr action;
		CSensoryFramePtr observations;
		CObservationPtr observation;
		size_t curr_rawlog_entry;

		// Read the dataset and pass the measurements to CGraphSlamEngine
		bool cont_exec = true;
		while (CRawlog::getActionObservationPairOrObservation(
				rawlog_stream,
				action,
				observations,
				observation,
				curr_rawlog_entry) && cont_exec) {

			// actual call to the graphSLAM execution method
			// Exit if user pressed C-c
			cont_exec = graphslam_engine.execGraphSlamStep(
					action,
					observations,
					observation,
					curr_rawlog_entry);

		}
		logger.logFmt(LVL_WARN, "Finished graphslam execution.");

		//
		// Postprocessing
		//

		logger.logFmt(LVL_INFO, "Generating overall report...");
		graphslam_engine.generateReportFiles(graphslam_handler.output_dir_fname);
		// save the graph and the 3DScene 
		if (graphslam_handler.save_graph) {
			std::string save_graph_fname = 
				graphslam_handler.output_dir_fname +
				"/" +
				graphslam_handler.save_graph_fname;
			graphslam_engine.saveGraph(&save_graph_fname);
		}
		if (!disable_visuals.getValue() && graphslam_handler.save_3DScene) {
			std::string save_3DScene_fname = 
				graphslam_handler.output_dir_fname +
				"/" +
				graphslam_handler.save_3DScene_fname;

			graphslam_engine.save3DScene(&save_3DScene_fname);
		}

		// get the occupancy gridmap that was built
		if (graphslam_handler.save_gridmap) {
			COccupancyGridMap2D gridmap;
			graphslam_engine.getOccupancyGridMap2D(&gridmap);
			gridmap.saveMetricMapRepresentationToFile(
					graphslam_handler.output_dir_fname +
					"/" +
					graphslam_handler.save_gridmap_fname);
		}


		////////////////////////////////////////////////////////////////////////

	}
	catch (exception& e) {
		logger.logFmt(LVL_ERROR, "Program finished for an exception!!\n%s\n",
				e.what());

		mrpt::system::pause();
		return -1;
	}
	catch (...) {
		logger.logFmt(LVL_ERROR, "Program finished for an untyped exception!!");
		mrpt::system::pause();
		return -1;
	}

	return 0;
}
Beispiel #14
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;
}
Beispiel #15
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;
}
Beispiel #16
0
// -----------------------------------------------
//				MAIN
// -----------------------------------------------
int main(int argc, char** argv)
{
	try
	{
		// Parse arguments:
		if (!cmd.parse(argc, argv))
			throw std::runtime_error("");  // should exit.

		const string input_log = arg_input_file.getValue();
		const string output_file = arg_output_file.getValue();
		const bool verbose = !arg_quiet.getValue();
		const bool overwrite = arg_overwrite.getValue();
		const int compress_level = arg_gz_level.getValue();

		// Check files:
		if (!mrpt::system::fileExists(input_log))
			throw runtime_error(
				format("Input file doesn't exist: '%s'", input_log.c_str()));

		if (mrpt::system::fileExists(output_file) && !overwrite)
			throw runtime_error(format(
				"Output file already exist: '%s' (Use --overwrite to "
				"override)",
				output_file.c_str()));

		VERBOSE_COUT << "Input log        : " << input_log << endl;
		VERBOSE_COUT << "Output map file  : " << output_file
					 << " (Compression level: " << compress_level << ")\n";

		// Open I/O streams:
		std::ifstream input_stream(input_log.c_str());
		if (!input_stream.is_open())
			throw runtime_error(
				format("Error opening for read: '%s'", input_log.c_str()));

		// --------------------------------
		// The main loop
		// --------------------------------
		vector<CObservation::Ptr> importedObservations;
		mrpt::maps::CSimpleMap theSimpleMap;
		const mrpt::system::TTimeStamp base_timestamp = mrpt::system::now();

		const uint64_t totalInFileSize = mrpt::system::getFileSize(input_log);
		int decimateUpdateConsole = 0;

		while (carmen_log_parse_line(
			input_stream, importedObservations, base_timestamp))
		{
			CPose2D gt_pose;
			bool has_gt_pose = false;

			for (size_t i = 0; i < importedObservations.size(); i++)
			{
				// If we have an "odometry" observation but it's not alone, it's
				// probably
				//  a "corrected" odometry from some SLAM program, so save it as
				//  ground truth:
				if (importedObservations.size() > 1 &&
					IS_CLASS(importedObservations[i], CObservationOdometry))
				{
					CObservationOdometry::Ptr odo =
						std::dynamic_pointer_cast<CObservationOdometry>(
							importedObservations[i]);
					gt_pose = odo->odometry;
					has_gt_pose = true;
					break;
				}
			}

			// Only if we have a valid pose, save it to the simple map:
			if (has_gt_pose)
			{
				CSensoryFrame::Ptr SF =
					mrpt::make_aligned_shared<CSensoryFrame>();

				for (const auto& importedObservation : importedObservations)
				{
					if (!IS_CLASS(
							importedObservation,
							CObservationOdometry))  // Odometry was already used
					// as positioning...
					{
						SF->insert(importedObservation);
					}
				}

				// Insert (observations, pose) pair:
				CPosePDFGaussian::Ptr pos =
					mrpt::make_aligned_shared<CPosePDFGaussian>();
				pos->mean = gt_pose;
				theSimpleMap.insert(pos, SF);
			}

			// Update progress in the console:
			// ----------------------------------
			if (verbose && ++decimateUpdateConsole > 10)
			{
				decimateUpdateConsole = 0;

				const std::streampos curPos = input_stream.tellg();
				const double progress_ratio =
					double(curPos) / double(totalInFileSize);
				static const int nBlocksTotal = 50;
				const int nBlocks = progress_ratio * nBlocksTotal;
				cout << "\rProgress: [" << string(nBlocks, '#')
					 << string(nBlocksTotal - nBlocks, ' ')
					 << format(
							"] %6.02f%% (%u frames)", progress_ratio * 100,
							static_cast<unsigned int>(theSimpleMap.size()));
				cout.flush();
			}
		};
		cout << "\n";

		// Save final map object:
		{
			mrpt::io::CFileGZOutputStream out_map;
			if (!out_map.open(output_file, compress_level))
				throw runtime_error(format(
					"Error opening for write: '%s'", output_file.c_str()));

			cout << "Dumping simplemap object to file...";
			cout.flush();
			mrpt::serialization::archiveFrom(out_map) << theSimpleMap;
			cout << "Done\n";
			cout.flush();
		}

		// successful end of program.
		return 0;
	}
	catch (const std::exception& e)
	{
		std::cerr << mrpt::exception_to_str(e) << std::endl;
		return -1;
	}
}
 void add(TCLAP::CmdLine &cmd) {
     cmd.add(outputArg);
     cmd.add(threadsArg);
     cmd.add(heightArg);
     cmd.add(widthArg);
 }
Beispiel #18
0
// ======================================================================
//     main() of rawlog-edit
// ======================================================================
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("","externalize",
			"Op: convert to external storage.\n"
			"Requires: -o (or --output)\n"
			"Optional: --image-format",cmd, false) );
		ops_functors["externalize"] = &op_externalize;

		arg_ops.push_back(new TCLAP::SwitchArg("","info",
			"Op: parse input file and dump information and statistics.",cmd, false) );
		ops_functors["info"] = &op_info;

		arg_ops.push_back(new TCLAP::SwitchArg("","list-images",
			"Op: dump a list of all external image files in the dataset.\n"
			"Optionally the output text file can be changed with --text-file-output."
			,cmd, false) );
		ops_functors["list-images"] = &op_list_images;

		arg_ops.push_back(new TCLAP::SwitchArg("","list-range-bearing",
			"Op: dump a list of all landmark observations of type range-bearing.\n"
			"Optionally the output text file can be changed with --text-file-output."
			,cmd, false) );
		ops_functors["list-range-bearing"] = &op_list_rangebearing;

		arg_ops.push_back(new TCLAP::ValueArg<std::string>("","remove-label",
			"Op: Remove all observation matching the given sensor label(s)."
			"Several labels can be provided separated by commas.\n"
			"Requires: -o (or --output)",false,"","label[,label...]",cmd) );
		ops_functors["remove-label"] = &op_remove_label;

		arg_ops.push_back(new TCLAP::ValueArg<std::string>("","keep-label",
			"Op: Remove all observations not matching the given sensor label(s)."
			"Several labels can be provided separated by commas.\n"
			"Requires: -o (or --output)",false,"","label[,label...]",cmd) );
		ops_functors["keep-label"] = &op_keep_label;

		arg_ops.push_back(new TCLAP::SwitchArg("","export-gps-kml",
			"Op: Export GPS paths to Google Earth KML files.\n"
			"Generates one .kml file with different sections for each different sensor label of GPS observations in the dataset. "
			"The generated .kml files will be saved in the same path than the input rawlog, with the same "
			"filename + each sensorLabel."
			,cmd,false) );
		ops_functors["export-gps-kml"] = &op_export_gps_kml;

		arg_ops.push_back(new TCLAP::SwitchArg("","export-gps-txt",
			"Op: Export GPS readings to TXT files.\n"
			"Generates one .txt file for each different sensor label of GPS observations in the dataset. "
			"The generated .txt files will be saved in the same path than the input rawlog, with the same "
			"filename + each sensorLabel."
			,cmd,false) );
		ops_functors["export-gps-txt"] = &op_export_gps_txt;

		arg_ops.push_back(new TCLAP::SwitchArg("","cut",
			"Op: Cut a part of the input rawlog.\n"
			"Requires: -o (or --output)\n"
			"Requires: At least one of --from-index, --from-time, --to-index, --to-time. Use only one of the --from-* and --to-* at once.\n"
			"If only a --from-* is given, the rawlog will be saved up to its end. If only a --to-* is given, the rawlog will be saved from its beginning.\n"
			,cmd,false) );
		ops_functors["cut"] = &op_cut;

		arg_ops.push_back(new TCLAP::SwitchArg("","generate-3d-pointclouds",
			"Op: (re)generate the 3D pointclouds within CObservation3DRangeScan objects that have range data.\n"
			"Requires: -o (or --output)\n"
			,cmd,false));
		ops_functors["generate-3d-pointclouds"] = &op_generate_3d_pointclouds;

		arg_ops.push_back(new TCLAP::SwitchArg("","generate-pcd",
			"Op: Generate a PointCloud Library (PCL) PCD file with the point cloud for each sensor observation that can be converted into"
			" this representation: laser scans, 3D camera images, etc.\n"
			"May use: --out-dir to change the output directory (default: \"./\")\n"
			,cmd,false));
		ops_functors["generate-pcd"] = &op_generate_pcd;

		arg_ops.push_back(new TCLAP::ValueArg<std::string>("","sensors-pose",
			"Op: batch change the poses of sensors from a rawlog-grabber-like configuration file that specifies the pose of sensors by their sensorLabel names.\n"
			"Requires: -o (or --output)\n",
			false,"","file.ini",cmd) );
		ops_functors["sensors-pose"] = &op_sensors_pose;

		arg_ops.push_back(new TCLAP::ValueArg<std::string>("","camera-params",
			"Op: change the camera parameters of all CObservationImage's with the given SENSOR_LABEL, with new params loaded from the given file, section '[CAMERA_PARAMS]'.\n"
			"Requires: -o (or --output)\n"
			,false,"","SENSOR_LABEL,file.ini",cmd) );
		ops_functors["camera-params"] = &op_camera_params;

		// --------------- End of list of possible operations --------

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

		string input_rawlog  = 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.");
			}

		if (selected_op.empty())
		{
			throw std::runtime_error(
				"Don't know what to do: No operation was indicated.\n"
				"Use --help to see the list of possible operations.");
		}

		VERBOSE_COUT << "Operation to perform: " << selected_op << endl;

		// This will be done for any operation: open the input rawlog
		// ------------------------------------------------------------
		if (!mrpt::system::fileExists(input_rawlog))
			throw runtime_error(format("Input file doesn't exist: '%s'",input_rawlog.c_str()));

		// Open input rawlog:
		CFileGZInputStream  fil_input;
		VERBOSE_COUT << "Opening '" << input_rawlog << "'...\n";
		fil_input.open(input_rawlog);
		VERBOSE_COUT << "Open OK.\n";

		// External storage directory?
		CImage::IMAGES_PATH_BASE = CRawlog::detectImagesDirectory(input_rawlog);
		if (mrpt::system::directoryExists(CImage::IMAGES_PATH_BASE)) {
			VERBOSE_COUT << "Found external storage directory: " << CImage::IMAGES_PATH_BASE << "\n";
		}
		else {
			VERBOSE_COUT << "Warning: No external storage directory was found (not an issue if the rawlog does not contain delayed-load images).\n";
		}


		// ------------------------------------
		//  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](fil_input,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;
} // end of main()
int main(int argc, char* argv[])
{

cmd.parse( argc, argv );
cout.precision(12);

vec b(3), bpert(3), binhom(3); 
b(0)=bx.getValue(); 
b(1)=by.getValue();
b(2)=bz.getValue();
bpert=b;
binhom=(bepsilon.getValue(),0,0);
bpert(0)=b(0)+deltabx.getValue();
//binhom(0)=b(0)+deltabx.getValue()+bepsilon.getValue();

//Parametros de Ising

//Construccion de estado coherentre

cvec state, staterev, qustate;

qustate=BlochToQubit(theta.getValue(),phi.getValue());

state=TensorPow(qustate,qubits.getValue());

staterev=state;

//Lista de la fidelidad

vec list(steps.getValue());

for(int i=0;i<steps.getValue();i++){

list(i)=pow( abs( dot( conj(staterev),state)),2);

//cout<< pow( abs( dot( conj(staterev),state)),2) <<endl;

cout << list(i) <<endl;
// cout<< i<< " " << list(i) <<endl;

list(i)=sqrt(list(i));

apply_ising_allvsall(state, J.getValue());
//Symmetry Breaking
apply_ising_z(state, Jepsilon.getValue(),0,1);

apply_magnetic_inhom(state, b, b+binhom);

apply_ising_allvsall(staterev, J.getValue()+Jpert.getValue());
//Symmetry breaking for H_1
apply_ising_z(staterev, Jepsilon.getValue(),0,1);

apply_magnetic_inhom(staterev, bpert, bpert+binhom);

}
 
//fidelity.close();

//cout << staterev;

cout<< sum_positive_derivatives(list)<< endl;


}
int main (int argc, char *argv[]) {

  /** Set up the command line arguments */
  TCLAP::CmdLine command ("Parition a graph based on sparsification",
                          ' ');
  TCLAP::SwitchArg randomSwitch ("r",
                                 "random",
                                 "Use a random graph",
                                 command,
                                 false);
  TCLAP::SwitchArg debugSwitch ("d",
                                "debug",
                                "Print debug messages",
                                 command,
                                 false);
  TCLAP::ValueArg<double> tRatio ("t",
                                  "t-ratio",
                                  "Tau ratio; how big can Tau be w.r.t N",
                                  true,
                                  2.0,
                                  "double");
  command.add (tRatio);
  TCLAP::ValueArg<double> sRatio ("s",
                                  "s-ratio",
                                  "Ratio for num samples to take w.r.t Tau",
                                  true,
                                  1.0,
                                  "double");
  command.add (sRatio);
  TCLAP::ValueArg<int> kSteps ("k",
                               "k-steps",
                               "The number of B.F.S steps for local graph",
                               true,
                               2,
                               "int");
  command.add (kSteps);
  TCLAP::ValueArg<int> nParts ("p",
                               "n-parts",
                               "The number of partitions to create",
                               true,
                               2,
                               "int");
  command.add (nParts);
  TCLAP::ValueArg<int> rSeed ("x",
                              "r-seed",
                              "The seed for the PRNG",
                               false,
                               0,
                               "int");
  command.add (rSeed);
  TCLAP::ValueArg<bool> isErdos ("g",
                                 "r-type",
                                 "Is this a Erdos-Renyi graph",
                                  false,
                                  false,
                                  "bool");
  command.add (isErdos);
  TCLAP::ValueArg<int> nVerts ("m",
                               "n-verts",
                               "The number of vertices to generate",
                               false,
                               10,
                               "int");
  command.add (nVerts);
  TCLAP::ValueArg<int> nEdges ("n",
                               "n-edges",
                               "The number of edges to generate",
                               false,
                               10*9,
                               "int");
  command.add (nEdges);
  TCLAP::ValueArg<double> minWeight ("l",
                                     "min-weight",
                                     "Minimum edge weight",
                                     false,
                                     1.0,
                                     "double");
  command.add (minWeight);
  TCLAP::ValueArg<double> maxWeight ("u",
                                     "max-weight",
                                     "Maximum edge weight",
                                     false,
                                     1.0,
                                     "double");
  command.add (maxWeight);
  TCLAP::ValueArg<std::string> fileType ("i",
                                         "input-file",
                                         "Type of the file to read",
                                         false,
                                         "MATRIX_MARKET",
                                         "string");
  command.add (fileType);
  TCLAP::ValueArg<std::string> fileName ("f",
                                         "file-name",
                                         "Name of the file to read",
                                         false,
                                         "",
                                         "string");
  command.add (fileName);

  /* Parse the command line arguments */
  command.parse (argc, argv);

  /* Read in the command line arguments into variables */
  bool random_graph = randomSwitch.getValue ();
  bool debug = debugSwitch.getValue ();
  int k_steps = kSteps.getValue ();
  double t_ratio = tRatio.getValue ();
  double s_ratio = sRatio.getValue ();
  int n_parts = nParts.getValue ();

  char* file_type;
  char* file_name;
  int rand_seed;
  int num_vertices;
  int num_edges;
  double min_weight;
  double max_weight;

  FILE* input_fp; 
  FILE* output_fp;
  AdjacencyListType adjacency_list;

  if (random_graph) {
    std::cout << "Running on a random graph" << std::endl;

    rand_seed = rSeed.getValue ();
    num_vertices = nVerts.getValue ();
    num_edges = nEdges.getValue ();
    min_weight = minWeight.getValue ();
    max_weight = maxWeight.getValue ();

    /* Check the number of edges input */
    if (num_edges > (num_vertices*(num_vertices-1))) {
      fprintf (stderr, "A graph with %d vertices cannot have %d edges\n",
                                 num_vertices, num_edges);
      exit (1);
    } else if (n_parts > (s_ratio*num_vertices*log(num_vertices))) {
      fprintf (stderr, "Asking for too many partitions\n");
      exit (1);
    } else if (num_edges&0x1) {
      fprintf (stderr, "Currently, we only deal with even number of edges.\
 In other words, we only compute on symmetric graphs without self-loops.\n");
      exit (1);
    }

    /* Read in the graph */

    const bool is_erdos = isErdos.getValue ();

    if (is_erdos) 
      gen_erdos_renyi (num_vertices,
                       num_edges,
                       rand_seed,
                       false, /* no self-loop */
                       true, /* symmetric */
                       min_weight,
                       max_weight,
                       adjacency_list);
    else
      gen_rmat (num_vertices,
                num_edges,
                rand_seed,
                false, /* no self-loop */
                true, /* symmetric */
                min_weight,
                max_weight,
                adjacency_list);
  } else {
Beispiel #21
0
int main(int argc, char* argv[])
{

cmd.parse( argc, argv );
cout.precision(12);

// {{{ Set seed for random
unsigned int semilla=seed.getValue();
if (semilla == 0){
  Random semilla_uran; semilla=semilla_uran.strong();
} 
RNG_reset(semilla);
// }}}


// state selection
cvec init=to_cvec(BellState(belltheta.getValue()));


cmat H;
cvec list [realizations.getValue()];
int totrace=(int) 3*envdim.getValue();

// Total Hamiltonian
if(model.getValue()=="total"){
cmat U;
int i;
cvec state;
cvec initextend=TensorProduct(init,RandomState(envdim.getValue()));
cmat partialstate;
cvec eigenval;
double pur;
int j;
for(int num=0;num<realizations.getValue();num++){
j=0;
i=1;
H=RandomGUE(4*envdim.getValue());
while(i==1){
U=exponentiate_nonsym(-complex <double>(0,1)*delta.getValue()*j*H);
j++;
state=U*initextend;
partialstate=partial_trace_qubits(state,totrace);
pur=(double)Purity(partialstate);
if(pur<=purity.getValue()+epsilon.getValue() and pur>=purity.getValue()-epsilon.getValue()){
	i=0;
}
}
eigenval=eig(partialstate);
cout<<Chop(real(eigenval(0)))<<' '<<Chop(real(eigenval(1)))<<' '<<Chop(real(eigenval(2)))<<' '<<Chop(real(eigenval(3)))<<endl;
}
}
// Spectator Hamiltonian

if(model.getValue()=="spectator"){
cmat U;
int i;
cvec state;
cvec initextend=TensorProduct(init,RandomState(envdim.getValue()));
cmat partialstate;
cvec eigenval;
double pur;
int j;
for(int num=0;num<realizations.getValue();num++){
j=0;
i=1;
H=TensorProduct(eye_c(4),RandomGUE(envdim.getValue()))+coupling.getValue()*TensorProduct(eye_c(2),RandomGUE(2*envdim.getValue()));
while(i==1){
U=exponentiate_nonsym(-complex <double>(0,1)*delta.getValue()*j*H);
j++;
state=U*initextend;
partialstate=partial_trace_qubits(state,totrace);
pur=(double)Purity(partialstate);
if(pur<=purity.getValue()+epsilon.getValue() and pur>=purity.getValue()-epsilon.getValue()){
	i=0;
}
}
//list[num]=eig(partialstate);
eigenval=eig(partialstate);
cout<<Chop(real(eigenval(0)))<<' '<<Chop(real(eigenval(1)))<<' '<<Chop(real(eigenval(2)))<<' '<<Chop(real(eigenval(3)))<<endl;
}
//}
//cvec eigenval;
//for(int num=0;num<realizations.getValue();num++){
//eigenval=list[num];
//cout<<Chop(real(eigenval(0)))<<' '<<Chop(real(eigenval(1)))<<' '<<Chop(real(eigenval(2)))<<' '<<Chop(real(eigenval(3)))<<endl;
//}
}

//Spectator 2 : Generalization to 3 qubits. Use GHZ and W to see what happens.

//Tuneable Coupling

if(model.getValue()=="tuneable"){
cmat U;
int i;
cvec state;
cvec initextend=TensorProduct(init,RandomState(envdim.getValue()));
cmat partialstate;
cvec eigenval;
double pur;
int j;
for(int num=0;num<realizations.getValue();num++){
j=0;
i=1;
H=TensorProduct(eye_c(4),RandomGUE(envdim.getValue()))+coupling.getValue()*RandomGUE(4*envdim.getValue());
while(i==1){
U=exponentiate_nonsym(-complex <double>(0,1)*delta.getValue()*j*H);
j++;
state=U*initextend;
partialstate=partial_trace_qubits(state,totrace);
pur=(double)Purity(partialstate);
if(pur<=purity.getValue()+epsilon.getValue() and pur>=purity.getValue()-epsilon.getValue()){
	i=0;
}
}
eigenval=eig(partialstate);
cout<<Chop(real(eigenval(0)))<<' '<<Chop(real(eigenval(1)))<<' '<<Chop(real(eigenval(2)))<<' '<<Chop(real(eigenval(3)))<<endl;
}
}


}
// -----------------------------------------------
//				MAIN
// -----------------------------------------------
int main(int argc, char** argv)
{
	try
	{
		// Parse arguments:
		if (!cmd.parse(argc, argv))
			throw std::runtime_error("");  // should exit.

		const string input_log = arg_input_file.getValue();
		const string output_rawlog = arg_output_file.getValue();
		const bool verbose = !arg_quiet.getValue();
		const bool overwrite = arg_overwrite.getValue();
		const int compress_level = arg_gz_level.getValue();

		// Check files:
		if (!mrpt::system::fileExists(input_log))
			throw runtime_error(
				format("Input file doesn't exist: '%s'", input_log.c_str()));

		if (mrpt::system::fileExists(output_rawlog) && !overwrite)
			throw runtime_error(
				format(
					"Output file already exist: '%s' (Use --overwrite to "
					"override)",
					output_rawlog.c_str()));

		VERBOSE_COUT << "Input log        : " << input_log << endl;
		VERBOSE_COUT << "Output rawlog    : " << output_rawlog
					 << " (Compression level: " << compress_level << ")\n";

		// Open I/O streams:
		std::ifstream input_stream(input_log.c_str());
		if (!input_stream.is_open())
			throw runtime_error(
				format("Error opening for read: '%s'", input_log.c_str()));

		mrpt::utils::CFileGZOutputStream out_rawlog;
		if (!out_rawlog.open(output_rawlog, compress_level))
			throw runtime_error(
				format("Error opening for write: '%s'", output_rawlog.c_str()));

		// --------------------------------
		// The main loop
		// --------------------------------
		vector<CObservation::Ptr> importedObservations;
		map<TTimeStamp, TPose2D> groundTruthPoses;  // If found...
		unsigned int nSavedObs = 0;

		const mrpt::system::TTimeStamp base_timestamp = mrpt::system::now();

		const uint64_t totalInFileSize = mrpt::system::getFileSize(input_log);
		int decimateUpdateConsole = 0;

		while (carmen_log_parse_line(
			input_stream, importedObservations, base_timestamp))
		{
			for (size_t i = 0; i < importedObservations.size(); i++)
			{
				out_rawlog << *importedObservations[i];
				nSavedObs++;

				// by the way: if we have an "odometry" observation but it's not
				// alone, it's probably
				//  a "corrected" odometry from some SLAM program, so save it as
				//  ground truth:
				if (importedObservations.size() > 1 &&
					IS_CLASS(importedObservations[i], CObservationOdometry))
				{
					CObservationOdometry::Ptr odo =
						std::dynamic_pointer_cast<CObservationOdometry>(
							importedObservations[i]);
					groundTruthPoses[odo->timestamp] = TPose2D(odo->odometry);
				}
			}

			// Update progress in the console:
			// ----------------------------------
			if (verbose && ++decimateUpdateConsole > 10)
			{
				decimateUpdateConsole = 0;

				const std::streampos curPos = input_stream.tellg();
				const double progress_ratio =
					double(curPos) / double(totalInFileSize);
				static const int nBlocksTotal = 50;
				const int nBlocks = progress_ratio * nBlocksTotal;
				cout << "\rProgress: [" << string(nBlocks, '#')
					 << string(nBlocksTotal - nBlocks, ' ')
					 << format(
							"] %6.02f%% (%u objects)", progress_ratio * 100,
							nSavedObs);
				cout.flush();
			}
		};
		cout << "\n";

		// If we had ground-truth robot poses, save to file:
		if (!groundTruthPoses.empty())
		{
			const string gt_filename =
				mrpt::system::fileNameChangeExtension(output_rawlog, "gt.txt");
			cout << "Note: Saving ground truth pose information to '"
				 << gt_filename << "'\n";

			std::ofstream gt_file;
			gt_file.open(gt_filename.c_str());
			if (!gt_file.is_open())
				throw std::runtime_error(
					format(
						"Couldn't open output file for ground truth: '%s'",
						gt_filename.c_str()));
			gt_file
				<< "%          Ground truth positioning data \n"
				   "%  Timestamp (sec)       x (m)    y (m)    phi (rad)  \n"
				   "% ----------------------------------------------------\n";
			for (map<TTimeStamp, TPose2D>::const_iterator it =
					 groundTruthPoses.begin();
				 it != groundTruthPoses.end(); ++it)
				gt_file << format(
					"   %12.06f %9.03f %9.03f %9.04f\n",
					mrpt::system::timestampToDouble(it->first), it->second.x,
					it->second.y, it->second.phi);
		}

		// successful end of program.
		return 0;
	}
	catch (std::exception& e)
	{
		if (strlen(e.what())) std::cerr << e.what() << std::endl;
		return -1;
	}
}
Beispiel #23
0
			aes128round1 ( TCLAP::CmdLine& cmd, shared_ptr<KeyGenerators::base> _keygen ) : base ( cmd, _keygen ),
				whichsboxArg ( "b", "sbox", "From which SBOX output should I start to correlate?", false, 0, "0-15" ),
				sboxnumArg ( "v", "sboxnum", "How many consecutive SBOXes should I consider?", false, 1, "1-8" ) {
				cmd.add ( whichsboxArg );
				cmd.add ( sboxnumArg );
			}