// }}}
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;
} // }}}
// ======================================================================
// See TOutputRawlogCreator declaration
// ======================================================================
TOutputRawlogCreator::TOutputRawlogCreator()
{
if (!arg_output_file.isSet())
throw runtime_error("This operation requires an output file. Use '-o file' or '--output file'.");
out_rawlog_filename = arg_output_file.getValue();
if (fileExists(out_rawlog_filename) && !arg_overwrite.getValue() )
throw runtime_error(string("*ABORTING*: Output file already exists: ") + out_rawlog_filename + string("\n. Select a different output path, remove the file or force overwrite with '-w' or '--overwrite'.") );
if (!out_rawlog.open(out_rawlog_filename))
throw runtime_error(string("*ABORTING*: Cannot open output file: ") + out_rawlog_filename );
}
// 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;
} // }}}
//}}}
} // }}}
// ======================================================================
// See TOutputRawlogCreator declaration
// ======================================================================
TOutputRawlogCreator::TOutputRawlogCreator()
{
if (!arg_output_file.isSet())
throw runtime_error(
"This operation requires an output file. Use '-o file' or "
"'--output file'.");
out_rawlog_filename = arg_output_file.getValue();
if (fileExists(out_rawlog_filename) && !arg_overwrite.getValue())
throw runtime_error(
string("*ABORTING*: Output file already exists: ") +
out_rawlog_filename +
string("\n. Select a different output path, remove the file or "
"force overwrite with '-w' or '--overwrite'."));
if (!out_rawlog_io.open(out_rawlog_filename))
throw runtime_error(
string("*ABORTING*: Cannot open output file: ") +
out_rawlog_filename);
out_rawlog = std::make_unique<
mrpt::serialization::CArchiveStreamBase<mrpt::io::CFileGZOutputStream>>(
out_rawlog_io);
}
// ======================================================================
// 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()
// ======================================================================
// 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()
int main(int argc, char* argv[])
{
try
{
// init comandline parser
TCLAP::CmdLine cmd("Command description message", ' ', "1.0");
TCLAP::ValueArg<string> argOutputFile ("o", "output", "Output file", true, "", "string");
TCLAP::ValueArg<string> argFilter ("f", "filter", "Filter files according to pattern", false, ".SIFTComparison.feat.xml.gz","string");
TCLAP::ValueArg<int> argPRECLUSTER ("p", "precluster", "Number of descriptors to select in precluster-preprocessing (0 = no preclustering)",false,0 ,"int");
TCLAP::ValueArg<int> argBOWSIZE ("b", "bowsize", "Size of the BoW Dictionary", false, 1000, "int");
TCLAP::SwitchArg argBinaryInput ("i", "binary", "Read descriptors from binary archives", false);
TCLAP::SwitchArg argVerbose ("v", "verbose", "Provide additional debugging output", false);
TCLAP::UnlabeledValueArg<string> dirArg ( "directory", "Directory containing files with extracted features", true, "directory","string");
cmd.add( argOutputFile );
cmd.add( argFilter );
cmd.add( argPRECLUSTER );
cmd.add( argBOWSIZE );
cmd.add( argBinaryInput );
cmd.add( argVerbose );
cmd.add( dirArg );
// parse arguments
cmd.parse( argc, argv );
// enable/disable verbose output
VerboseOutput::verbose = argVerbose.getValue();
VerboseOutput::println(string("train"), "Create BoW of size %d", argBOWSIZE.getValue());
TermCriteria tcrit;
tcrit.epsilon = 10;
tcrit.maxCount = 10;
tcrit.type = 1;
BOWKMeansTrainer bowtrainer(argBOWSIZE.getValue(),tcrit,1,KMEANS_PP_CENTERS);
VerboseOutput::println(string("train"), "Creating Visual Bag of Words");
string filter = argFilter.getValue();
if (argBinaryInput.getValue())
{
filter = ".SIFTComparison.descriptors.dat";
}
vector<string> files = getdir(dirArg.getValue(), filter);
int i = 1;
VerboseOutput::println(string("train"), "Reading features of directory '%s'", dirArg.getValue().c_str());
for (vector<string>::iterator filename = files.begin(); filename!=files.end(); ++filename)
{
VerboseOutput::println("train", "[%i of %i in directory '%s']", i++, files.size(), dirArg.getValue().c_str());
Mat descriptors;
stringstream filePathss;
filePathss << dirArg.getValue() << "/" << *filename;
string filePath = filePathss.str();
VerboseOutput::println(string("train"), string("processing file '" + filePath + "'"));
if (argBinaryInput.getValue())
{
loadDescriptorsFromBinaryArchives(descriptors, filePath);
}
else
{
loadDescriptorsFromOpenCVFilestorage(descriptors, filePath);
}
if ((descriptors.rows == 0) || (descriptors.cols == 0))
{
throw runtime_error("No Descriptors read for file: ");
}
VerboseOutput::println(string("train"), "%i descriptors loaded", descriptors.rows);
if ((argPRECLUSTER.getValue() > 0) && (argPRECLUSTER.getValue() < descriptors.rows - 100))
{
VerboseOutput::println(string("train"), string("pre-clustering"));
Mat labels;
Mat centers;
kmeans(descriptors,argPRECLUSTER.getValue(),labels,tcrit,1, KMEANS_PP_CENTERS, centers);
VerboseOutput::println(string("train"), "...add cluster centers of pre-clustering to bow");
bowtrainer.add(centers);
}
else
{
VerboseOutput::println(string("train"), "...add descriptors to bow");
bowtrainer.add(descriptors);
}
VerboseOutput::println(string("train"), "...current bow-size: %i", bowtrainer.descripotorsCount());
}
// calculate vocabulary
VerboseOutput::println(string("train"), string("Creating Vocabulary"));
if (bowtrainer.descripotorsCount() <= argBOWSIZE.getValue())
{
throw runtime_error("BoW size higher than number of loaded descriptors!");
}
Mat vocab = bowtrainer.cluster();
// output results to file
VerboseOutput::println(string("train"), string("Storing BoW to File"));
writeVocabularyToFile(vocab, argOutputFile.getValue());
VerboseOutput::println(string("train"), string("Finished"));
}
catch (exception &e) // catch any exceptions
{
cout << "\n";
cout << "*** Training aborted!\n";
cout << " Reason: " << e.what() << "\n\n";
exit(1);
}
return 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;
}
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()
void execGraphSlamEngine(mrpt::system::COutputLogger* logger)
{
// Instance for managing the available graphslam deciders optimizers
TUserOptionsChecker<GRAPH_T> options_checker;
options_checker.createDeciderOptimizerMappings();
options_checker.populateDeciderOptimizerProperties();
// fetch the command line options
// ////////////////////////////////////////////////////////////
// decide whether to display the help messages for the deciders/optimizers
{
bool list_registrars = false;
if (list_all_registrars.getValue())
{
options_checker.dumpRegistrarsToConsole("all");
list_registrars = true;
}
if (list_node_registrars.getValue())
{
options_checker.dumpRegistrarsToConsole("node");
list_registrars = true;
}
if (list_edge_registrars.getValue())
{
options_checker.dumpRegistrarsToConsole("edge");
list_registrars = true;
}
if (list_optimizers.getValue())
{
options_checker.dumpOptimizersToConsole();
}
if (list_registrars || list_optimizers.getValue())
{
logger->logFmt(LVL_INFO, "Exiting.. ");
return;
}
}
// 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");
}
// 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();
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<GRAPH_T> graphslam_handler(
logger, &options_checker, !disable_visuals.getValue());
graphslam_handler.setFNames(ini_fname, rawlog_fname, ground_truth_fname);
graphslam_handler.initEngine(node_reg, edge_reg, optimizer);
graphslam_handler.printParams();
graphslam_handler.execute();
}
int main(const int argc, const char* argv[]) {
std::cout << "DatCC v0.2 created by pastelmind\n" << std::endl;
datcc::setCurrentProgramDir(argv[0]);
try {
const char exampleStr[] = "EXAMPLES:"
"\nDatCC -d -u ."
"\n\tDecompiles the default units.dat file."
"\nDatCC -c -w \"C:\\My Mod\\my weapons.ini\""
"\n\tCompiles \"C:\\My Mod\\my weapons.ini\" into \"C:\\My Mod\\my weapons.dat\""
"\nDatCC -c -t \"C:\\test\\tech.ini\" -b C:\\test\\techdata.dat"
"\n\tCompiles \"C:\\test\\tech.ini\" into \"C:\\test\\tech.dat\", using C:\\test\\techdata.dat as the base DAT file"
"\nDatCC -r -f \"example mod-flingy.dat\" output.ini"
"\n\tCompares \"example mod-flingy.dat\" with the default flingy.dat and save the differences to output.ini";
TCLAP::CmdLine cmd(exampleStr, ' ', "0.1");
TCLAP::SwitchArg isCompileModeArg ("c", "compile", "Compiles INI files to DAT files.");
TCLAP::SwitchArg isDecompileModeArg("d", "decompile", "Decompiles DAT files to INI files.");
TCLAP::SwitchArg isCompareModeArg ("r", "compare", "Compares the DAT file with the base DAT file and decompiles the differences to an INI file");
std::vector<TCLAP::Arg*> modeSwitchArgs;
modeSwitchArgs.push_back(&isCompileModeArg);
modeSwitchArgs.push_back(&isDecompileModeArg);
modeSwitchArgs.push_back(&isCompareModeArg);
cmd.xorAdd(modeSwitchArgs);
TCLAP::ValueArg<std::string> baseDatArg("b", "basedat",
"Base DAT file to use when compiling/comparing. If omitted, the default DAT files are used.",
false, ".", "base file");
cmd.add(baseDatArg);
TCLAP::UnlabeledValueArg<std::string> inputFileArg("input",
"In compile mode, specify the INI file to compile. In decompile or compare mode, specify the DAT file to decompile or compare. Use . to decompile the default DAT files.",
true, "", "input file");
cmd.add(inputFileArg);
TCLAP::UnlabeledValueArg<std::string> outputFileArg("output",
"Specify the output DAT file (in compile mode) or INI file (in decompile/compare mode). If omitted, the output file is named after the input file.",
false, "", "output file");
cmd.add(outputFileArg);
TCLAP::SwitchArg useUnitsDatArg ("u", "units", "Operate on units.dat");
TCLAP::SwitchArg useWeaponsDatArg ("w", "weapons", "Operate on weapons.dat");
TCLAP::SwitchArg useFlingyDatArg ("f", "flingy", "Operate on flingy.dat");
TCLAP::SwitchArg useSpritesDatArg ("s", "sprites", "Operate on sprites.dat");
TCLAP::SwitchArg useImagesDatArg ("i", "images", "Operate on images.dat");
TCLAP::SwitchArg useUpgradesDatArg("g", "upgrades", "Operate on upgrades.dat");
TCLAP::SwitchArg useTechdataDatArg("t", "techdata", "Operate on techdata.dat");
TCLAP::SwitchArg useSfxdataDatArg ("x", "sfxdata", "Operate on sfxdata.dat");
//TCLAP::SwitchArg usePortdataDatArg("p", "portdata", "Operate on portdata.dat (NOT SUPPORTED YET!)");
//TCLAP::SwitchArg useMapdataDatArg ("m", "mapdata", "Operate on mapdata.dat (NOT SUPPORTED YET)");
TCLAP::SwitchArg useOrdersDatArg ("o", "orders", "Operate on orders.dat");
std::vector<TCLAP::Arg*> datSwitchArgs;
datSwitchArgs.push_back(&useUnitsDatArg);
datSwitchArgs.push_back(&useWeaponsDatArg);
datSwitchArgs.push_back(&useFlingyDatArg);
datSwitchArgs.push_back(&useSpritesDatArg);
datSwitchArgs.push_back(&useImagesDatArg);
datSwitchArgs.push_back(&useUpgradesDatArg);
datSwitchArgs.push_back(&useTechdataDatArg);
datSwitchArgs.push_back(&useSfxdataDatArg);
//datSwitchArgs.push_back(&usePortdataDatArg);
//datSwitchArgs.push_back(&useMapdataDatArg);
datSwitchArgs.push_back(&useOrdersDatArg);
cmd.xorAdd(datSwitchArgs);
cmd.parse(argc, argv);
//-------- Main program logic start --------//
datcc::loadData();
if (isCompileModeArg.isSet()) {
//Compile mode
if (useUnitsDatArg.isSet())
datcc::compileUnits(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useWeaponsDatArg.isSet())
datcc::compileWeapons(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useFlingyDatArg.isSet())
datcc::compileFlingy(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useSpritesDatArg.isSet())
datcc::compileSprites(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useImagesDatArg.isSet())
datcc::compileImages(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useUpgradesDatArg.isSet())
datcc::compileUpgrades(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useTechdataDatArg.isSet())
datcc::compileTechdata(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useSfxdataDatArg.isSet())
datcc::compileSfxdata(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useOrdersDatArg.isSet())
datcc::compileOrders(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else
throw TCLAP::ArgException("Unsupported DAT file format, please wait for new version.",
"UnsupportedFormat", "Unsupported DAT format exception");
}
else if (isDecompileModeArg.isSet()) {
if (baseDatArg.isSet())
throw TCLAP::ArgException("Base DAT argument is unnecessary for decompile mode", "unused_basedat", "Unused base DAT argument");
//Decompile mode
if (useUnitsDatArg.isSet())
datcc::decompileUnits(inputFileArg.getValue(), outputFileArg.getValue());
else if (useWeaponsDatArg.isSet())
datcc::decompileWeapons(inputFileArg.getValue(), outputFileArg.getValue());
else if (useFlingyDatArg.isSet())
datcc::decompileFlingy(inputFileArg.getValue(), outputFileArg.getValue());
else if (useSpritesDatArg.isSet())
datcc::decompileSprites(inputFileArg.getValue(), outputFileArg.getValue());
else if (useImagesDatArg.isSet())
datcc::decompileImages(inputFileArg.getValue(), outputFileArg.getValue());
else if (useUpgradesDatArg.isSet())
datcc::decompileUpgrades(inputFileArg.getValue(), outputFileArg.getValue());
else if (useTechdataDatArg.isSet())
datcc::decompileTechdata(inputFileArg.getValue(), outputFileArg.getValue());
else if (useSfxdataDatArg.isSet())
datcc::decompileSfxdata(inputFileArg.getValue(), outputFileArg.getValue());
else if (useOrdersDatArg.isSet())
datcc::decompileOrders(inputFileArg.getValue(), outputFileArg.getValue());
else
throw TCLAP::ArgException("Unsupported DAT file format, please wait for new version.",
"UnsupportedFormat", "Unsupported DAT format exception");
}
else if (isCompareModeArg.isSet()) {
//Compare mode
if (useUnitsDatArg.isSet())
datcc::compareUnits(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useWeaponsDatArg.isSet())
datcc::compareWeapons(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useFlingyDatArg.isSet())
datcc::compareFlingy(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useSpritesDatArg.isSet())
datcc::compareSprites(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useImagesDatArg.isSet())
datcc::compareImages(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useUpgradesDatArg.isSet())
datcc::compareUpgrades(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useTechdataDatArg.isSet())
datcc::compareTechdata(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useSfxdataDatArg.isSet())
datcc::compareSfxdata(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else if (useOrdersDatArg.isSet())
datcc::compareOrders(inputFileArg.getValue(), outputFileArg.getValue(), baseDatArg.getValue());
else
throw TCLAP::ArgException("Unsupported DAT file format, please wait for new version.",
"UnsupportedFormat", "Unsupported DAT format exception");
}
else { //Should never reach here
throw TCLAP::ArgException("Cannot determine compile/decompile mode");
}
}
catch (TCLAP::ArgException &e) {
std::cerr << "Error: " << e.what() << std::endl;
}
return 0;
}
void thread_grabbing(TThreadParam& p)
{
try
{
CFileGZOutputStream f_out_rawlog;
if (arg_out_rawlog.isSet())
{
if (!f_out_rawlog.open(arg_out_rawlog.getValue()))
THROW_EXCEPTION_FMT(
"Error creating output rawlog file: %s",
arg_out_rawlog.getValue().c_str());
}
auto arch = mrpt::serialization::archiveFrom(f_out_rawlog);
mrpt::hwdrivers::CVelodyneScanner velodyne;
if (arg_verbose.isSet()) velodyne.enableVerbose(true);
// Set params:
velodyne.setModelName(mrpt::typemeta::TEnumType<
mrpt::hwdrivers::CVelodyneScanner::model_t>::
name2value(arg_model.getValue()));
if (arg_ip_filter.isSet())
velodyne.setDeviceIP(
arg_ip_filter.getValue()); // Default: from any IP
if (arg_in_pcap.isSet())
velodyne.setPCAPInputFile(arg_in_pcap.getValue());
if (arg_out_pcap.isSet())
velodyne.setPCAPOutputFile(arg_out_pcap.getValue());
// If you have a calibration file, better than default values:
if (arg_calib_file.isSet())
{
mrpt::obs::VelodyneCalibration calib;
if (!calib.loadFromXMLFile(arg_calib_file.getValue()))
throw std::runtime_error(
"Aborting: error loading calibration file.");
velodyne.setCalibration(calib);
}
// Open:
cout << "Calling CVelodyneScanner::initialize()...";
velodyne.initialize();
cout << "OK\n";
cout << "Waiting for first data packets (Press CTRL+C to abort)...\n";
CTicTac tictac;
int nScans = 0;
bool hard_error = false;
while (!hard_error && !p.quit)
{
// Grab new observations from the camera:
CObservationVelodyneScan::Ptr
obs; // (initially empty) Smart pointers to observations
CObservationGPS::Ptr obs_gps;
hard_error = !velodyne.getNextObservation(obs, obs_gps);
// Save to log file:
if (f_out_rawlog.fileOpenCorrectly())
{
if (obs) arch << *obs;
if (obs_gps) arch << *obs_gps;
}
if (obs)
{
std::atomic_store(&p.new_obs, obs);
nScans++;
}
if (obs_gps) std::atomic_store(&p.new_obs_gps, obs_gps);
if (p.pushed_key != 0)
{
switch (p.pushed_key)
{
case 27:
p.quit = true;
break;
}
// Clear pushed key flag:
p.pushed_key = 0;
}
if (nScans > 5)
{
p.Hz = nScans / tictac.Tac();
nScans = 0;
tictac.Tic();
}
}
}
catch (const std::exception& e)
{
cout << "Exception in Velodyne thread: " << mrpt::exception_to_str(e)
<< endl;
p.quit = true;
}
}
// 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;
}
void VTOutput::usage(TCLAP::CmdLineInterface& c)
{
std::string s = "";
std::list<TCLAP::Arg*> args = c.getArgList();
//prints unlabeled arument list first
for (TCLAP::ArgListIterator it = args.begin(); it != args.end(); it++)
{
if (typeid(**it)==typeid(TCLAP::UnlabeledValueArg<std::string>))
{
TCLAP::UnlabeledValueArg<std::string> *i = (TCLAP::UnlabeledValueArg<std::string> *) (*it);
s = i->getName();
}
else if (typeid(**it)==typeid(TCLAP::UnlabeledMultiArg<std::string>))
{
TCLAP::UnlabeledMultiArg<std::string> *i = (TCLAP::UnlabeledMultiArg<std::string> *) (*it);
s = i->getName();
}
}
std::clog << c.getProgramName() << " v" << c.getVersion() << "\n\n";
std::clog << "description : " << c.getMessage() << "\n\n";
std::clog << "usage : vt " << c.getProgramName() << " [options] " << s << "\n\n";
//prints rest of arguments
for (TCLAP::ArgListIterator it = args.begin(); it != args.end(); it++)
{
if (it==args.begin())
{
std::clog << "options : ";
}
else
{
std::clog << " ";
}
if (typeid(**it)==typeid(TCLAP::ValueArg<std::string>) ||
typeid(**it)==typeid(TCLAP::ValueArg<uint32_t>) ||
typeid(**it)==typeid(TCLAP::ValueArg<int32_t>) ||
typeid(**it)==typeid(TCLAP::ValueArg<double>) ||
typeid(**it)==typeid(TCLAP::ValueArg<float>))
{
TCLAP::ValueArg<std::string> *i = (TCLAP::ValueArg<std::string> *) (*it);
std::clog << "-" << i->getFlag()
<< " " << i->getDescription() << "\n";
}
else if (typeid(**it)==typeid(TCLAP::SwitchArg))
{
TCLAP::SwitchArg *i = (TCLAP::SwitchArg *) (*it);
std::clog << "-" << i->getFlag()
<< " " << i->getDescription() << "\n";
}
else if (typeid(**it)==typeid(TCLAP::UnlabeledValueArg<std::string>))
{
//ignored
}
else if (typeid(**it)==typeid(TCLAP::UnlabeledMultiArg<std::string>))
{
//ignored
}
else
{
std::clog << "oops, argument type not handled\n";
}
}
std::clog << "\n";
}
// -----------------------------------------------
// 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;
}
}
int main(int argc, char **argv) {
try {
TCLAP::CmdLine cmd("CEVAL Algorithm", ' ', "0.1");
cmd.add(poly);
cmd.add(x_min);
cmd.add(x_max);
cmd.add(y_min);
cmd.add(y_max);
cmd.add(use_rb);
cmd.add(display);
cmd.add(no_use_inclusion);
cmd.add(min_box_size);
cmd.add(max_box_size);
cmd.add(random_poly);
cmd.add(rand_degree);
cmd.add(rand_seed);
// Parse the args.
cmd.parse(argc, argv);
// Get the value parsed by each arg.
//string name = nameArg.getValue();
} catch (TCLAP::ArgException &e) {
cerr << "Error : " << e.error() << endl;
cerr << "Processing arg : " << e.argId() << endl;
return -1;
}
Polynomial<PolyType> a;
if (random_poly.getValue()) {
benchmark::GenerateRandom(&a, rand_degree.getValue(), 10 ,rand_seed.getValue());
} else {
benchmark::GetPoly(poly.getValue().c_str(), &a);
}
double max_box_size_d(max_box_size.getValue());
double min_box_size_d(min_box_size.getValue());
Complex min, max;
if (use_rb.getValue()) {
double min_r, max_r;
benchmark::GetRootBounds(a, &min_r, &max_r);
min = Complex(min_r, min_r);
max = Complex(max_r, max_r);
if (display.getValue()) {
cout << "Min : " << min << ",Max : " << max << endl;
}
} else {
double x_min_d(x_min.getValue()), x_max_d(x_max.getValue());
double y_min_d(y_min.getValue()), y_max_d(y_max.getValue());
min = Complex(x_min_d, y_min_d);
max = Complex(x_max_d, y_max_d);
}
Box *b = new Box(min, max);
Box *b_copy = new Box(min, max);
// start timing code.
struct timeval start;
struct timeval end;
gettimeofday(&start, NULL);
// end timing code.
Predicates p(a);
Algorithm algo(p, b, min_box_size_d, !no_use_inclusion.getValue(), display.getValue());
algo.Run();
if (no_use_inclusion.getValue()) {
algo.AttemptIsolation();
}
// start timing code.
gettimeofday(&end, NULL);
cout << ",time=" <<
(end.tv_sec - start.tv_sec)*1000000 + (end.tv_usec - start.tv_usec);
if (display.getValue()) {
cout << "Operated on Bounding box : " << min << "," << max << endl;
cout << "With polynomial : " << endl;
//a.dump();
cout << endl;
cout << "--------------------------" << endl;
cout << "Number of roots:" << algo.output()->size() << endl;
list<const Disk *>::const_iterator it = algo.output()->begin();
while (it != algo.output()->end()) {
cout << "m= " << (*it)->centre << ", r= " << (*it)->radius << endl;
++it;
}
display_funcs::SetDisplayParams(b_copy, algo.reject(), algo.output_boxes(),
algo.ambiguous());
startGlutLoop(argc, argv);
} else {
cout << ",output=" << algo.output()->size() << 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 {
// ------------------------------------------------------
// 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;
}
static std::unique_ptr<cMemorySettingsRepository> ParseArguments(int argc, char ** argv)
{
try
{
// Parse the comand line args:
TCLAP::CmdLine cmd("Cuberite");
TCLAP::ValueArg<int> slotsArg ("s", "max-players", "Maximum number of slots for the server to use, overrides setting in setting.ini", false, -1, "number", cmd);
TCLAP::MultiArg<int> portsArg ("p", "port", "The port number the server should listen to", false, "port", cmd);
TCLAP::SwitchArg commLogArg ("", "log-comm", "Log server client communications to file", cmd);
TCLAP::SwitchArg commLogInArg ("", "log-comm-in", "Log inbound server client communications to file", cmd);
TCLAP::SwitchArg commLogOutArg ("", "log-comm-out", "Log outbound server client communications to file", cmd);
TCLAP::SwitchArg crashDumpFull ("", "crash-dump-full", "Crashdumps created by the server will contain full server memory", cmd);
TCLAP::SwitchArg crashDumpGlobals("", "crash-dump-globals", "Crashdumps created by the server will contain the global variables' values", cmd);
TCLAP::SwitchArg noBufArg ("", "no-output-buffering", "Disable output buffering", cmd);
TCLAP::SwitchArg runAsServiceArg ("d", "service", "Run as a service on Windows, or daemon on UNIX like systems", cmd);
cmd.parse(argc, argv);
// Copy the parsed args' values into a settings repository:
auto repo = cpp14::make_unique<cMemorySettingsRepository>();
if (slotsArg.isSet())
{
int slots = slotsArg.getValue();
repo->AddValue("Server", "MaxPlayers", static_cast<Int64>(slots));
}
if (portsArg.isSet())
{
for (auto port: portsArg.getValue())
{
repo->AddValue("Server", "Port", static_cast<Int64>(port));
}
}
if (commLogArg.getValue())
{
g_ShouldLogCommIn = true;
g_ShouldLogCommOut = true;
}
else
{
g_ShouldLogCommIn = commLogInArg.getValue();
g_ShouldLogCommOut = commLogOutArg.getValue();
}
if (noBufArg.getValue())
{
setvbuf(stdout, nullptr, _IONBF, 0);
}
repo->SetReadOnly();
// Set the service flag directly to cRoot:
if (runAsServiceArg.getValue())
{
cRoot::m_RunAsService = true;
}
// Apply the CrashDump flags for platforms that support them:
#if defined(_WIN32) && !defined(_WIN64) && defined(_MSC_VER) // 32-bit Windows app compiled in MSVC
if (crashDumpGlobals.getValue())
{
g_DumpFlags = static_cast<MINIDUMP_TYPE>(g_DumpFlags | MiniDumpWithDataSegs);
}
if (crashDumpFull.getValue())
{
g_DumpFlags = static_cast<MINIDUMP_TYPE>(g_DumpFlags | MiniDumpWithFullMemory);
}
#endif // 32-bit Windows app compiled in MSVC
return repo;
}
catch (const TCLAP::ArgException & exc)
{
printf("Error reading command line %s for arg %s", exc.error().c_str(), exc.argId().c_str());
return cpp14::make_unique<cMemorySettingsRepository>();
}
}
// -----------------------------------------------
// 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;
}
}
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;
}