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") { // {{{
// 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;
}
// }}}
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;
} // }}}
//}}}
} // }}}
// }}}
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;
} // }}}
// -----------------------------------------------
// 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;
}
}
// ------------------------------------------------------
// 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;
}
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 {
// ======================================================================
// 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()
// ======================================================================
// 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;
}
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()
// 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;
}
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_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;
}
}
// ======================================================================
// 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 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;
}