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