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; }