diff --git a/.gitignore b/.gitignore index 362e2ed..956361b 100644 --- a/.gitignore +++ b/.gitignore @@ -17,6 +17,7 @@ libraries/setup/boost_1_58_0/ libraries/tbb43_20150611oss/ libraries/vicon/ libraries/*.log +modules/infinitam/ *.tags tags tests/mike/ diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..24412e1 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,69 @@ +{ + "files.associations": { + "xstring": "cpp", + "memory": "cpp", + "*.tpp": "cpp", + "*.tcu": "cpp", + "algorithm": "cpp", + "atomic": "cpp", + "bit": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "compare": "cpp", + "concepts": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "deque": "cpp", + "exception": "cpp", + "fstream": "cpp", + "functional": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "ios": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "iterator": "cpp", + "limits": "cpp", + "list": "cpp", + "map": "cpp", + "new": "cpp", + "numeric": "cpp", + "ostream": "cpp", + "queue": "cpp", + "set": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "string": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "typeinfo": "cpp", + "unordered_map": "cpp", + "utility": "cpp", + "vector": "cpp", + "xfacet": "cpp", + "xhash": "cpp", + "xiosbase": "cpp", + "xlocale": "cpp", + "xlocinfo": "cpp", + "xlocmon": "cpp", + "xlocnum": "cpp", + "xloctime": "cpp", + "xmemory": "cpp", + "xstddef": "cpp", + "xtr1common": "cpp", + "xtree": "cpp", + "xutility": "cpp", + "ratio": "cpp", + "stop_token": "cpp", + "thread": "cpp" + } +} \ No newline at end of file diff --git a/apps/spaintgui/Application.cpp b/apps/spaintgui/Application.cpp index 9caa1b9..33d8f20 100644 --- a/apps/spaintgui/Application.cpp +++ b/apps/spaintgui/Application.cpp @@ -51,8 +51,9 @@ using namespace tvgutil; //#################### CONSTRUCTORS #################### -Application::Application(const MultiScenePipeline_Ptr& pipeline, bool renderFiducials) -: m_activeSubwindowIndex(0), +Application::Application(const MultiScenePipeline_Ptr& pipeline, bool renderFiducials, std::string name) +: m_name(name), + m_activeSubwindowIndex(0), m_batchModeEnabled(false), m_commandManager(10), m_pauseBetweenFrames(true), @@ -141,7 +142,9 @@ bool Application::run() if(m_saveMeshOnExit) save_mesh(); // If desired, save a model of each scene before the application terminates. - if(m_saveModelsOnExit) save_models(); + // if(m_saveModelsOnExit) save_models(); + // set save model as default to save voxel scene for relocalisation later + save_models(); return true; } @@ -1032,14 +1035,8 @@ void Application::save_mesh() const void Application::save_models() const { - // Find the models directory and make sure it exists. - boost::filesystem::path modelsSubdir = find_subdir_from_executable("models"); - boost::filesystem::create_directories(modelsSubdir); - - // Determine the directory to use for saving the models, based on either the experiment tag (if specified) or the current timestamp (otherwise). - const Settings_CPtr& settings = m_pipeline->get_model()->get_settings(); - std::string modelName = settings->get_first_value("experimentTag", TimeUtil::get_iso_timestamp()); - boost::filesystem::path outputDir = modelsSubdir / modelName; + std::string modelName = m_name; + boost::filesystem::path outputDir = modelName; // Save the models to disk. m_pipeline->save_models(outputDir); diff --git a/apps/spaintgui/Application.h b/apps/spaintgui/Application.h index 6599b78..23f33dc 100644 --- a/apps/spaintgui/Application.h +++ b/apps/spaintgui/Application.h @@ -45,6 +45,9 @@ class Application //#################### PRIVATE VARIABLES #################### private: + /** Save model and relocalisation to this file. */ + std::string m_name; + /** The index of the sub-window with which the user is interacting. */ size_t m_activeSubwindowIndex; @@ -113,7 +116,7 @@ class Application * \param pipeline The multi-scene pipeline that the application should use. * \param renderFiducials Whether or not to render the fiducials (if any) that have been detected in the 3D scene. */ - Application(const MultiScenePipeline_Ptr& pipeline, bool renderFiducials = false); + Application(const MultiScenePipeline_Ptr& pipeline, bool renderFiducials = false, std::string name = ""); //#################### PUBLIC MEMBER FUNCTIONS #################### public: diff --git a/apps/spaintgui/core/CollaborativePipeline.cpp b/apps/spaintgui/core/CollaborativePipeline.cpp index 7e651f0..95c32ae 100644 --- a/apps/spaintgui/core/CollaborativePipeline.cpp +++ b/apps/spaintgui/core/CollaborativePipeline.cpp @@ -51,26 +51,6 @@ CollaborativePipeline::CollaborativePipeline(const Settings_Ptr& settings, const } } - // Finally, we add a collaborative component to handle relocalisation between the different scenes. - /* std::cout << "after SLAMComponent allocate\n"; - for(int i = 0; i < 3; ++i) - { - // Set the GPU as active. - ORcudaSafeCall(cudaSetDevice(i)); - - // Look up its memory usage. - size_t freeMemory, totalMemory; - ORcudaSafeCall(cudaMemGetInfo(&freeMemory, &totalMemory)); - - // Convert the memory usage to MB. - const size_t bytesPerMb = 1024 * 1024; - const size_t freeMb = freeMemory / bytesPerMb; - const size_t usedMb = (totalMemory - freeMemory) / bytesPerMb; - const size_t totalMb = totalMemory / bytesPerMb; - - // Save the memory usage to the output stream. - std::cout << i << " freeMb: " << freeMb << ";" << " usedMb: " << usedMb << ";" << " totalMb: "<< totalMb << "\n"; - }*/ m_collaborativeComponent.reset(new CollaborativeComponent(m_model, collaborationMode)); } diff --git a/apps/spaintgui/core/MultiScenePipeline.cpp b/apps/spaintgui/core/MultiScenePipeline.cpp index 46a2611..d61db28 100644 --- a/apps/spaintgui/core/MultiScenePipeline.cpp +++ b/apps/spaintgui/core/MultiScenePipeline.cpp @@ -132,7 +132,7 @@ void MultiScenePipeline::save_models(const bf::path& outputDir) const // Save the models for each scene into a separate subdirectory. for(std::map::const_iterator it = m_slamComponents.begin(), iend = m_slamComponents.end(); it != iend; ++it) { - const bf::path scenePath = outputDir / it->first; + const bf::path scenePath = outputDir / "model"; std::cout << "Saving models for " << it->first << " in: " << scenePath << '\n'; it->second->save_models(scenePath.string()); } diff --git a/apps/spaintgui/main.cpp b/apps/spaintgui/main.cpp index 3ffd613..8f84f50 100644 --- a/apps/spaintgui/main.cpp +++ b/apps/spaintgui/main.cpp @@ -10,21 +10,22 @@ #include #include -// Note: This must appear before anything that could include SDL.h, since it includes boost/asio.hpp, a header that has a WinSock conflict with SDL.h. +// Note: This must appear before anything that could include SDL.h, since it includes boost/asio.hpp, a header that has +// a WinSock conflict with SDL.h. #include "Application.h" #if defined(WITH_ARRAYFIRE) && defined(WITH_CUDA) - #ifdef _MSC_VER - // Suppress a VC++ warning that is produced when including ArrayFire headers. - #pragma warning(disable:4275) - #endif +#ifdef _MSC_VER +// Suppress a VC++ warning that is produced when including ArrayFire headers. +#pragma warning(disable : 4275) +#endif - #include +#include - #ifdef _MSC_VER - // Reenable the suppressed warning for the rest of the translation unit. - #pragma warning(default:4275) - #endif +#ifdef _MSC_VER +// Reenable the suppressed warning for the rest of the translation unit. +#pragma warning(default : 4275) +#endif #endif #include @@ -57,8 +58,8 @@ #include "core/CollaborativePipeline.h" #include "core/ObjectivePipeline.h" -#include "core/SemanticPipeline.h" #include "core/SLAMPipeline.h" +#include "core/SemanticPipeline.h" #include "sequences/SpaintSequence.h" using namespace InputSource; @@ -81,6 +82,7 @@ struct CommandLineArguments //~~~~~~~~~~~~~~~~~~~~ PUBLIC VARIABLES ~~~~~~~~~~~~~~~~~~~~ // User-specifiable arguments + std::string name; bool batch; std::string calibrationFilename; bool cameraAfterDisk; @@ -133,7 +135,7 @@ struct CommandLineArguments * * \param settings The settings object. */ - void add_to_settings(const Settings_Ptr& settings) + void add_to_settings(const Settings_Ptr &settings) { std::vector diskTrackerConfigs; for(size_t i = 0, size = sequences.size(); i < size; ++i) @@ -141,50 +143,54 @@ struct CommandLineArguments diskTrackerConfigs.push_back(sequences[i]->make_disk_tracker_config()); } - #define ADD_SETTING(arg) settings->add_value(#arg, boost::lexical_cast(arg)) - #define ADD_SETTINGS(arg) for(size_t i = 0; i < arg.size(); ++i) { settings->add_value(#arg, boost::lexical_cast(arg[i])); } - ADD_SETTING(batch); - ADD_SETTING(calibrationFilename); - ADD_SETTING(collaborationMode); - ADD_SETTINGS(depthImageMasks); - ADD_SETTINGS(depthNoiseSigmas); - ADD_SETTING(detectFiducials); - ADD_SETTINGS(diskTrackerConfigs); - ADD_SETTING(experimentTag); - ADD_SETTING(fiducialDetectorType); - ADD_SETTING(globalPosesSpecifier); - ADD_SETTING(headless); - ADD_SETTING(host); - ADD_SETTINGS(initialFrameNumbers); - ADD_SETTING(leapFiducialID); - ADD_SETTING(mapSurfels); - ADD_SETTINGS(missingDepthFractions); - ADD_SETTING(modelSpecifier); - ADD_SETTING(noRelocaliser); - ADD_SETTING(openNIDeviceURI); - ADD_SETTING(pipelineType); - ADD_SETTING(port); - ADD_SETTINGS(poseFileMasks); - ADD_SETTING(prefetchBufferCapacity); - ADD_SETTING(profileMemory); - ADD_SETTING(relocaliserType); - ADD_SETTING(renderFiducials); - ADD_SETTINGS(rgbImageMasks); - ADD_SETTING(runServer); - ADD_SETTING(saveMeshOnExit); - ADD_SETTING(saveModelsOnExit); - ADD_SETTINGS(semanticImageMasks); - ADD_SETTINGS(sequenceSpecifiers); - ADD_SETTINGS(sequenceTypes); - ADD_SETTING(subwindowConfigurationIndex); - ADD_SETTINGS(trackerSpecifiers); - ADD_SETTING(trackObject); - ADD_SETTING(trackSurfels); - ADD_SETTING(useVicon); - ADD_SETTING(verbose); - ADD_SETTING(viconHost); - #undef ADD_SETTINGS - #undef ADD_SETTING +#define ADD_SETTING(arg) settings->add_value(#arg, boost::lexical_cast(arg)) +#define ADD_SETTINGS(arg) \ + for(size_t i = 0; i < arg.size(); ++i) \ + { \ + settings->add_value(#arg, boost::lexical_cast(arg[i])); \ + } + ADD_SETTING(batch); + ADD_SETTING(calibrationFilename); + ADD_SETTING(collaborationMode); + ADD_SETTINGS(depthImageMasks); + ADD_SETTINGS(depthNoiseSigmas); + ADD_SETTING(detectFiducials); + ADD_SETTINGS(diskTrackerConfigs); + ADD_SETTING(experimentTag); + ADD_SETTING(fiducialDetectorType); + ADD_SETTING(globalPosesSpecifier); + ADD_SETTING(headless); + ADD_SETTING(host); + ADD_SETTINGS(initialFrameNumbers); + ADD_SETTING(leapFiducialID); + ADD_SETTING(mapSurfels); + ADD_SETTINGS(missingDepthFractions); + ADD_SETTING(modelSpecifier); + ADD_SETTING(noRelocaliser); + ADD_SETTING(openNIDeviceURI); + ADD_SETTING(pipelineType); + ADD_SETTING(port); + ADD_SETTINGS(poseFileMasks); + ADD_SETTING(prefetchBufferCapacity); + ADD_SETTING(profileMemory); + ADD_SETTING(relocaliserType); + ADD_SETTING(renderFiducials); + ADD_SETTINGS(rgbImageMasks); + ADD_SETTING(runServer); + ADD_SETTING(saveMeshOnExit); + ADD_SETTING(saveModelsOnExit); + ADD_SETTINGS(semanticImageMasks); + ADD_SETTINGS(sequenceSpecifiers); + ADD_SETTINGS(sequenceTypes); + ADD_SETTING(subwindowConfigurationIndex); + ADD_SETTINGS(trackerSpecifiers); + ADD_SETTING(trackObject); + ADD_SETTING(trackSurfels); + ADD_SETTING(useVicon); + ADD_SETTING(verbose); + ADD_SETTING(viconHost); +#undef ADD_SETTINGS +#undef ADD_SETTING } }; @@ -196,11 +202,11 @@ struct CommandLineArguments * \param parsedOptions The set of parsed options. * \param settings The settings object. */ -void add_unregistered_options_to_settings(const po::parsed_options& parsedOptions, const Settings_Ptr& settings) +void add_unregistered_options_to_settings(const po::parsed_options &parsedOptions, const Settings_Ptr &settings) { for(size_t i = 0, optionCount = parsedOptions.options.size(); i < optionCount; ++i) { - const po::basic_option& option = parsedOptions.options[i]; + const po::basic_option &option = parsedOptions.options[i]; if(option.unregistered) { // Add all the specified values for the option in the correct order. @@ -227,17 +233,20 @@ ImageSourceEngine *check_camera_subengine(ImageSourceEngine *cameraSubengine) delete cameraSubengine; return NULL; } - else return cameraSubengine; + else + return cameraSubengine; } /** - * \brief Copies any (voxel) scene parameters that have been specified in the configuration file across to the actual scene parameters object. + * \brief Copies any (voxel) scene parameters that have been specified in the configuration file across to the actual + * scene parameters object. * * \param settings The settings for the application. */ -void copy_scene_params(const Settings_Ptr& settings) +void copy_scene_params(const Settings_Ptr &settings) { -#define COPY_PARAM(type, name, defaultValue) settings->sceneParams.name = settings->get_first_value("SceneParams."#name, defaultValue) +#define COPY_PARAM(type, name, defaultValue) \ + settings->sceneParams.name = settings->get_first_value("SceneParams." #name, defaultValue) // Note: The default values are taken from InfiniTAM. COPY_PARAM(int, maxW, 100); @@ -251,13 +260,15 @@ void copy_scene_params(const Settings_Ptr& settings) } /** - * \brief Copies any surfel scene parameters that have been specified in the configuration file across to the actual surfel scene parameters object. + * \brief Copies any surfel scene parameters that have been specified in the configuration file across to the actual + * surfel scene parameters object. * * \param settings The settings for the application. */ -void copy_surfel_scene_params(const Settings_Ptr& settings) +void copy_surfel_scene_params(const Settings_Ptr &settings) { -#define COPY_PARAM(type, name, defaultValue) settings->surfelSceneParams.name = settings->get_first_value("SurfelSceneParams."#name, defaultValue) +#define COPY_PARAM(type, name, defaultValue) \ + settings->surfelSceneParams.name = settings->get_first_value("SurfelSceneParams." #name, defaultValue) // Note: The default values are taken from InfiniTAM. COPY_PARAM(float, deltaRadius, 0.5f); @@ -279,12 +290,13 @@ void copy_surfel_scene_params(const Settings_Ptr& settings) } /** - * \brief Determines the sequences (if any) that the user wants to load from disk, based on the program's command-line arguments. + * \brief Determines the sequences (if any) that the user wants to load from disk, based on the program's command-line + * arguments. * * \param args The program's command-line arguments. * \return The sequences (if any) that the user wants to load from disk. */ -std::vector determine_sequences(const CommandLineArguments& args) +std::vector determine_sequences(const CommandLineArguments &args) { std::vector sequences; @@ -301,7 +313,7 @@ std::vector determine_sequences(const CommandLineArguments& args) double missingDepthFraction = i < args.missingDepthFractions.size() ? args.missingDepthFractions[i] : 0.0; float depthNoiseSigma = i < args.depthNoiseSigmas.size() ? args.depthNoiseSigmas[i] : 0.0f; - const std::string& sequenceSpecifier = args.sequenceSpecifiers[i]; + const std::string &sequenceSpecifier = args.sequenceSpecifiers[i]; if(!bf::is_regular_file(sequenceSpecifier)) { // Determine the sequence type. @@ -309,13 +321,16 @@ std::vector determine_sequences(const CommandLineArguments& args) // Determine the directory containing the sequence. bf::path dir = bf::is_directory(sequenceSpecifier) - ? sequenceSpecifier - : find_subdir_from_executable(sequenceType + "s") / sequenceSpecifier; + ? sequenceSpecifier + : find_subdir_from_executable(sequenceType + "s") / sequenceSpecifier; // Add the sequence to the list. - sequences.push_back(Sequence_CPtr(new SpaintSequence(dir, initialFrameNumber, missingDepthFraction, depthNoiseSigma))); + sequences.push_back( + Sequence_CPtr(new SpaintSequence(dir, initialFrameNumber, missingDepthFraction, depthNoiseSigma))); } - else throw std::runtime_error("Error: The sequence specifier '" + sequenceSpecifier + "' denotes a file rather than a directory"); + else + throw std::runtime_error("Error: The sequence specifier '" + sequenceSpecifier + + "' denotes a file rather than a directory"); } // Add any sequence that the user specifies implicitly via depth/RGB/pose masks. @@ -331,7 +346,13 @@ std::vector determine_sequences(const CommandLineArguments& args) double missingDepthFraction = j < args.missingDepthFractions.size() ? args.missingDepthFractions[j] : 0.0; float depthNoiseSigma = j < args.depthNoiseSigmas.size() ? args.depthNoiseSigmas[j] : 0.0f; - sequences.push_back(Sequence_CPtr(new SpaintSequence(depthImageMask, rgbImageMask, poseFileMask, semanticImageMask, initialFrameNumber, missingDepthFraction, depthNoiseSigma))); + sequences.push_back(Sequence_CPtr(new SpaintSequence(depthImageMask, + rgbImageMask, + poseFileMask, + semanticImageMask, + initialFrameNumber, + missingDepthFraction, + depthNoiseSigma))); } return sequences; @@ -343,13 +364,15 @@ std::vector determine_sequences(const CommandLineArguments& args) * \param globalPosesSpecifier The global poses specifier. * \return The global poses from the file, if possible, or an empty map otherwise. */ -std::map load_global_poses(const std::string& globalPosesSpecifier) +std::map load_global_poses(const std::string &globalPosesSpecifier) { - std::map globalPoses; + std::map globalPoses; // Determine the file from which to load the global poses. const std::string dirName = "global_poses"; - const bf::path p = bf::is_regular(globalPosesSpecifier) ? globalPosesSpecifier : find_subdir_from_executable(dirName) / (globalPosesSpecifier + ".txt"); + const bf::path p = bf::is_regular(globalPosesSpecifier) + ? globalPosesSpecifier + : find_subdir_from_executable(dirName) / (globalPosesSpecifier + ".txt"); // Try to read the poses from the file. If we can't, throw. std::ifstream fs(p.string().c_str()); @@ -371,7 +394,7 @@ std::map load_global_poses(const std::string& globalPoses * \param args The program's command-line arguments. * \return The camera subengine, if a suitable camera is attached, or NULL otherwise. */ -ImageSourceEngine *make_camera_subengine(const CommandLineArguments& args) +ImageSourceEngine *make_camera_subengine(const CommandLineArguments &args) { ImageSourceEngine *cameraSubengine = NULL; @@ -380,14 +403,19 @@ ImageSourceEngine *make_camera_subengine(const CommandLineArguments& args) if(cameraSubengine == NULL) { std::cout << "[spaint] Probing OpenNI camera: " << args.openNIDeviceURI << '\n'; - boost::optional uri = args.openNIDeviceURI == "Default" ? boost::none : boost::optional(args.openNIDeviceURI); + boost::optional uri = + args.openNIDeviceURI == "Default" ? boost::none : boost::optional(args.openNIDeviceURI); bool useInternalCalibration = !uri; // if reading from a file, assume that the provided calibration is to be used - cameraSubengine = check_camera_subengine(new OpenNIEngine(args.calibrationFilename.c_str(), uri ? uri->c_str() : NULL, useInternalCalibration + cameraSubengine = check_camera_subengine(new OpenNIEngine( + args.calibrationFilename.c_str(), + uri ? uri->c_str() : NULL, + useInternalCalibration #if USE_LOW_USB_BANDWIDTH_MODE - // If there is insufficient USB bandwidth available to support 640x480 RGB input, use 320x240 instead. - , Vector2i(320, 240) + // If there is insufficient USB bandwidth available to support 640x480 RGB input, use 320x240 instead. + , + Vector2i(320, 240) #endif - )); + )); } #endif @@ -421,11 +449,12 @@ ImageSourceEngine *make_camera_subengine(const CommandLineArguments& args) return cameraSubengine; } -boost::shared_ptr make_image_source_engine(const CommandLineArguments& args) +boost::shared_ptr make_image_source_engine(const CommandLineArguments &args) { boost::shared_ptr imageSourceEngine(new CompositeImageSourceEngine); - // If a model was specified without either a disk sequence or the camera following it, add an idle subengine to allow the model to still be viewed. + // If a model was specified without either a disk sequence or the camera following it, add an idle subengine to allow + // the model to still be viewed. if(args.modelDir && args.sequences.empty() && !args.cameraAfterDisk) { const std::string calibrationFilename = (*args.modelDir / "calib.txt").string(); @@ -439,13 +468,17 @@ boost::shared_ptr make_image_source_engine(const Com // FIXME: It would be better to use the correct calibration file for each disk sequence, but our pipeline doesn't // yet support changing the camera calibration parameters during reconstruction. const bf::path calibrationPath = args.sequences[0]->default_calib_path(); - const std::string calibrationFilename = (args.calibrationFilename != "" || !bf::exists(calibrationPath)) ? args.calibrationFilename : calibrationPath.string(); + const std::string calibrationFilename = (args.calibrationFilename != "" || !bf::exists(calibrationPath)) + ? args.calibrationFilename + : calibrationPath.string(); std::cout << "[spaint] Reading images from disk: " << *args.sequences[i] << '\n'; - imageSourceEngine->addSubengine(new AsyncImageSourceEngine(args.sequences[i]->make_image_source_engine(calibrationFilename), args.prefetchBufferCapacity)); + imageSourceEngine->addSubengine(new AsyncImageSourceEngine( + args.sequences[i]->make_image_source_engine(calibrationFilename), args.prefetchBufferCapacity)); } - // If no model and no disk sequences were specified, or we want to switch to the camera once all the disk sequences finish, add a camera subengine. + // If no model and no disk sequences were specified, or we want to switch to the camera once all the disk sequences + // finish, add a camera subengine. if((!args.modelDir && args.sequences.empty()) || args.cameraAfterDisk) { ImageSourceEngine *cameraSubengine = make_camera_subengine(args); @@ -456,17 +489,18 @@ boost::shared_ptr make_image_source_engine(const Com } /** - * \brief Makes the overall tracker configuration based on any tracker specifiers that were passed in on the command line. + * \brief Makes the overall tracker configuration based on any tracker specifiers that were passed in on the command + * line. * * \param args The program's command-line arguments. * \return The overall tracker configuration. */ -std::string make_tracker_config(const CommandLineArguments& args) +std::string make_tracker_config(const CommandLineArguments &args) { std::string result; // If the user wants to use global poses for the scenes, load them from disk. - std::map globalPoses; + std::map globalPoses; if(args.globalPosesSpecifier != "") globalPoses = load_global_poses(args.globalPosesSpecifier); // Determine the number of different trackers that will be needed. size_t trackerCount = args.sequences.size(); @@ -478,7 +512,8 @@ std::string make_tracker_config(const CommandLineArguments& args) // For each tracker that is needed: for(size_t i = 0; i < trackerCount; ++i) { - // Look to see if the user specified an explicit tracker specifier for it on the command line; if not, use a default tracker specifier. + // Look to see if the user specified an explicit tracker specifier for it on the command line; if not, use a default + // tracker specifier. const std::string trackerSpecifier = i < args.trackerSpecifiers.size() ? args.trackerSpecifiers[i] : "InfiniTAM"; // Separate the tracker specifier into chunks. typedef boost::char_separator sep; @@ -506,19 +541,23 @@ std::string make_tracker_config(const CommandLineArguments& args) // Try to find the global pose for this scene based on the sequence ID. const std::string sequenceID = args.sequences[i]->id(); - std::map::const_iterator it = globalPoses.find(sequenceID); + std::map::const_iterator it = globalPoses.find(sequenceID); // If that doesn't work, try to find the global pose based on the scene ID. if(it == globalPoses.end()) { - // FIXME: We shouldn't hard-code "Local" here - it's based on knowing how CollaborativePipeline assigns scene names. - const std::string sceneID = i == 0 ? Model::get_world_scene_id() : "Local" + boost::lexical_cast(i); + // FIXME: We shouldn't hard-code "Local" here - it's based on knowing how CollaborativePipeline assigns + // scene names. + const std::string sceneID = + i == 0 ? Model::get_world_scene_id() : "Local" + boost::lexical_cast(i); it = globalPoses.find(sceneID); } // If we now have a global pose, specify the creation of a global tracker that uses it. If not, throw. - if(it != globalPoses.end()) result += "" + boost::lexical_cast(it->second) + ""; - else throw std::runtime_error("Error: Global pose for sequence '" + sequenceID + "' not found"); + if(it != globalPoses.end()) + result += "" + boost::lexical_cast(it->second) + ""; + else + throw std::runtime_error("Error: Global pose for sequence '" + sequenceID + "' not found"); } // Specify the creation of a disk-based tracker that reads poses from disk. @@ -551,7 +590,10 @@ std::string make_tracker_config(const CommandLineArguments& args) * \param vm The variables map for the application. * \param settings The settings for the application. */ -void parse_configuration_file(const std::string& filename, const po::options_description& options, po::variables_map& vm, const Settings_Ptr& settings) +void parse_configuration_file(const std::string &filename, + const po::options_description &options, + po::variables_map &vm, + const Settings_Ptr &settings) { // Parse the options in the configuration file. po::parsed_options parsedConfigFileOptions = po::parse_config_file(filename.c_str(), options, true); @@ -571,15 +613,22 @@ void parse_configuration_file(const std::string& filename, const po::options_des * \param vm The variables map for the application. * \param settings The settings for the application. */ -void postprocess_arguments(CommandLineArguments& args, const po::options_description& options, po::variables_map& vm, const Settings_Ptr& settings) +void postprocess_arguments(CommandLineArguments &args, + const po::options_description &options, + po::variables_map &vm, + const Settings_Ptr &settings) { - // Determine the sequences (if any) that the user wants to load from disk, based on the program's command-line arguments. + // Determine the sequences (if any) that the user wants to load from disk, based on the program's command-line + // arguments. args.sequences = determine_sequences(args); - // If the user specified a model to load, determine the model directory and parse the model's configuration file (if present). + // If the user specified a model to load, determine the model directory and parse the model's configuration file (if + // present). if(args.modelSpecifier != "") { - args.modelDir = bf::is_directory(args.modelSpecifier) ? args.modelSpecifier : find_subdir_from_executable("models") / args.modelSpecifier / Model::get_world_scene_id(); + args.modelDir = bf::is_directory(args.modelSpecifier) + ? args.modelSpecifier + : find_subdir_from_executable("models") / args.modelSpecifier / Model::get_world_scene_id(); const bf::path configPath = *args.modelDir / "settings.ini"; if(bf::is_regular_file(configPath)) @@ -590,7 +639,8 @@ void postprocess_arguments(CommandLineArguments& args, const po::options_descrip } } - // If the user wants to use global poses for the scenes, make sure that each disk sequence has a tracker specifier set to Disk. + // If the user wants to use global poses for the scenes, make sure that each disk sequence has a tracker specifier set + // to Disk. if(args.globalPosesSpecifier != "") { args.trackerSpecifiers.resize(args.sequenceSpecifiers.size()); @@ -614,7 +664,8 @@ void postprocess_arguments(CommandLineArguments& args, const po::options_descrip // (there is no way to control the application without the UI anyway). if(args.headless) args.batch = true; - // If the user wants to use a Vicon fiducial detector or a Vicon-based tracker, make sure that the Vicon system it needs is enabled. + // If the user wants to use a Vicon fiducial detector or a Vicon-based tracker, make sure that the Vicon system it + // needs is enabled. if(args.fiducialDetectorType == "vicon") { args.useVicon = true; @@ -660,65 +711,81 @@ void postprocess_arguments(CommandLineArguments& args, const po::options_descrip * \param settings The application settings. * \return true, if the program should continue after parsing the command-line arguments, or false otherwise. */ -bool parse_command_line(int argc, char *argv[], CommandLineArguments& args, const Settings_Ptr& settings) +bool parse_command_line(int argc, char *argv[], CommandLineArguments &args, const Settings_Ptr &settings) { // Specify the possible options. po::options_description genericOptions("Generic options"); - genericOptions.add_options() - ("help", "produce help message") - ("batch", po::bool_switch(&args.batch), "enable batch mode") - ("calib,c", po::value(&args.calibrationFilename)->default_value(""), "calibration filename") - ("cameraAfterDisk", po::bool_switch(&args.cameraAfterDisk), "switch to the camera after a disk sequence") - ("collaborationMode", po::value(&args.collaborationMode)->default_value("batch"), "collaboration mode (batch|live)") - ("configFile,f", po::value(), "additional parameters filename") - ("depthNoiseSigma", po::value >(&args.depthNoiseSigmas)->multitoken(), "depth noise sigma") - ("detectFiducials", po::bool_switch(&args.detectFiducials), "enable fiducial detection") - ("experimentTag", po::value(&args.experimentTag)->default_value(Settings::NOT_SET), "experiment tag") - ("fiducialDetectorType", po::value(&args.fiducialDetectorType)->default_value("aruco"), "fiducial detector type (aruco|vicon)") - ("globalPosesSpecifier,g", po::value(&args.globalPosesSpecifier)->default_value(""), "global poses specifier") - ("headless", po::bool_switch(&args.headless), "run in headless mode") - ("host,h", po::value(&args.host)->default_value(""), "remote mapping host") - ("leapFiducialID", po::value(&args.leapFiducialID)->default_value(""), "the ID of the fiducial to use for the Leap Motion") - ("mapSurfels", po::bool_switch(&args.mapSurfels), "enable surfel mapping") - ("missingDepthFraction", po::value >(&args.missingDepthFractions)->multitoken(), "missing depth fraction [0,1]") - ("modelSpecifier,m", po::value(&args.modelSpecifier)->default_value(""), "model specifier") - ("noRelocaliser", po::bool_switch(&args.noRelocaliser), "don't use the relocaliser") - ("pipelineType", po::value(&args.pipelineType)->default_value("semantic"), "pipeline type") - ("port", po::value(&args.port)->default_value("7851"), "remote mapping port") - ("profileMemory", po::bool_switch(&args.profileMemory)->default_value(false), "whether or not to profile the memory usage") - ("relocaliserType", po::value(&args.relocaliserType)->default_value("forest"), "relocaliser type") - ("renderFiducials", po::bool_switch(&args.renderFiducials), "enable fiducial rendering") - ("runServer", po::bool_switch(&args.runServer), "run a remote mapping server") - ("saveMeshOnExit", po::bool_switch(&args.saveMeshOnExit), "save a mesh of the scene on exiting the application") - ("saveModelsOnExit", po::bool_switch(&args.saveModelsOnExit), "save a model of each voxel scene on exiting the application") - ("subwindowConfigurationIndex", po::value(&args.subwindowConfigurationIndex)->default_value("1"), "subwindow configuration index") - ("trackerSpecifier,t", po::value >(&args.trackerSpecifiers)->multitoken(), "tracker specifier") - ("trackSurfels", po::bool_switch(&args.trackSurfels), "enable surfel mapping and tracking") - ("useVicon", po::bool_switch(&args.useVicon)->default_value(false), "whether or not to use the Vicon system") - ("verbose,v", po::bool_switch(&args.verbose), "enable verbose output") - ("viconHost", po::value(&args.viconHost)->default_value("192.168.0.101"), "Vicon host") - ; + genericOptions.add_options()("help", + "produce help message")("batch", po::bool_switch(&args.batch), "enable batch mode")( + "calib,c", po::value(&args.calibrationFilename)->default_value(""), "calibration filename")( + "name", po::value(&args.name), "sequence name and save directory name")( + "cameraAfterDisk", po::bool_switch(&args.cameraAfterDisk), "switch to the camera after a disk sequence")( + "collaborationMode", + po::value(&args.collaborationMode)->default_value("batch"), + "collaboration mode (batch|live)")("configFile,f", po::value(), "additional parameters filename")( + "depthNoiseSigma", po::value>(&args.depthNoiseSigmas)->multitoken(), "depth noise sigma")( + "detectFiducials", po::bool_switch(&args.detectFiducials), "enable fiducial detection")( + "experimentTag", po::value(&args.experimentTag)->default_value(Settings::NOT_SET), "experiment tag")( + "fiducialDetectorType", + po::value(&args.fiducialDetectorType)->default_value("aruco"), + "fiducial detector type (aruco|vicon)")("globalPosesSpecifier,g", + po::value(&args.globalPosesSpecifier)->default_value(""), + "global poses specifier")( + "headless", po::bool_switch(&args.headless)->default_value(true), "run in headless mode")( + "host,h", po::value(&args.host)->default_value(""), "remote mapping host")( + "leapFiducialID", + po::value(&args.leapFiducialID)->default_value(""), + "the ID of the fiducial to use for the Leap Motion")( + "mapSurfels", po::bool_switch(&args.mapSurfels), "enable surfel mapping")( + "missingDepthFraction", + po::value>(&args.missingDepthFractions)->multitoken(), + "missing depth fraction [0,1]")( + "modelSpecifier,m", po::value(&args.modelSpecifier)->default_value(""), "model specifier")( + "noRelocaliser", po::bool_switch(&args.noRelocaliser), "don't use the relocaliser")( + "pipelineType", po::value(&args.pipelineType)->default_value("semantic"), "pipeline type")( + "port", po::value(&args.port)->default_value("7851"), "remote mapping port")( + "profileMemory", + po::bool_switch(&args.profileMemory)->default_value(false), + "whether or not to profile the memory usage")( + "relocaliserType", po::value(&args.relocaliserType)->default_value("forest"), "relocaliser type")( + "renderFiducials", po::bool_switch(&args.renderFiducials), "enable fiducial rendering")( + "runServer", po::bool_switch(&args.runServer), "run a remote mapping server")( + "saveMeshOnExit", po::bool_switch(&args.saveMeshOnExit), "save a mesh of the scene on exiting the application")( + "saveModelsOnExit", + po::bool_switch(&args.saveModelsOnExit), + "save a model of each voxel scene on exiting the application")( + "subwindowConfigurationIndex", + po::value(&args.subwindowConfigurationIndex)->default_value("1"), + "subwindow configuration index")("trackerSpecifier,t", + po::value>(&args.trackerSpecifiers)->multitoken(), + "tracker specifier")( + "trackSurfels", po::bool_switch(&args.trackSurfels), "enable surfel mapping and tracking")( + "useVicon", po::bool_switch(&args.useVicon)->default_value(false), "whether or not to use the Vicon system")( + "verbose,v", po::bool_switch(&args.verbose), "enable verbose output")( + "viconHost", po::value(&args.viconHost)->default_value("192.168.0.101"), "Vicon host"); po::options_description cameraOptions("Camera options"); - cameraOptions.add_options() - ("uri,u", po::value(&args.openNIDeviceURI)->default_value("Default"), "OpenNI device URI") - ; + cameraOptions.add_options()( + "uri,u", po::value(&args.openNIDeviceURI)->default_value("Default"), "OpenNI device URI"); po::options_description diskSequenceOptions("Disk sequence options"); - diskSequenceOptions.add_options() - ("depthMask,d", po::value >(&args.depthImageMasks)->multitoken(), "depth image mask") - ("initialFrame,n", po::value >(&args.initialFrameNumbers)->multitoken(), "initial frame numbers") - ("poseMask,p", po::value >(&args.poseFileMasks)->multitoken(), "pose file mask") - ("prefetchBufferCapacity,b", po::value(&args.prefetchBufferCapacity)->default_value(60), "capacity of the prefetch buffer") - ("rgbMask,r", po::value >(&args.rgbImageMasks)->multitoken(), "RGB image mask") - ("sequenceSpecifier,s", po::value >(&args.sequenceSpecifiers)->multitoken(), "sequence specifier") - ("sequenceType", po::value >(&args.sequenceTypes)->multitoken(), "sequence type") - ; + diskSequenceOptions.add_options()( + "depthMask,d", po::value>(&args.depthImageMasks)->multitoken(), "depth image mask")( + "initialFrame,n", + po::value>(&args.initialFrameNumbers)->multitoken(), + "initial frame numbers")("poseMask,p", + po::value>(&args.poseFileMasks)->multitoken(), + "pose file mask")("prefetchBufferCapacity,b", + po::value(&args.prefetchBufferCapacity)->default_value(60), + "capacity of the prefetch buffer")( + "rgbMask,r", po::value>(&args.rgbImageMasks)->multitoken(), "RGB image mask")( + "sequenceSpecifier,s", + po::value>(&args.sequenceSpecifiers)->multitoken(), + "sequence specifier")( + "sequenceType", po::value>(&args.sequenceTypes)->multitoken(), "sequence type"); po::options_description objectivePipelineOptions("Objective pipeline options"); - objectivePipelineOptions.add_options() - ("trackObject", po::bool_switch(&args.trackObject), "track the object") - ; + objectivePipelineOptions.add_options()("trackObject", po::bool_switch(&args.trackObject), "track the object"); po::options_description options; options.add(genericOptions); @@ -763,7 +830,7 @@ bool parse_command_line(int argc, char *argv[], CommandLineArguments& args, cons * \param message The error message. * \param code The exit code. */ -void quit(const std::string& message, int code = EXIT_FAILURE) +void quit(const std::string &message, int code = EXIT_FAILURE) { std::cerr << message << '\n'; SDL_Quit(); @@ -779,7 +846,7 @@ try Settings_Ptr settings(new Settings); settings->trackerConfig = NULL; - // Parse the command-line arguments. + // Parse the command-line arguments. (set headless mode as default) CommandLineArguments args; if(!parse_command_line(argc, argv, args, settings)) { @@ -795,15 +862,15 @@ try quit("Error: Failed to initialise SDL."); } - #ifdef WITH_GLUT +#ifdef WITH_GLUT // Initialise GLUT (used for text rendering only). glutInit(&argc, argv); - #endif +#endif - #ifdef WITH_OVR +#ifdef WITH_OVR // If we built with Rift support, initialise the Rift SDK. ovr_Initialize(); - #endif +#endif } // Find all available joysticks and report the number found to the user. @@ -827,7 +894,8 @@ try afcu::setNativeId(0); #endif - // Copy any scene parameters that have been set in the configuration file across to the actual scene parameters objects. + // Copy any scene parameters that have been set in the configuration file across to the actual scene parameters + // objects. copy_scene_params(settings); copy_surfel_scene_params(settings); @@ -841,123 +909,42 @@ try MappingServer_Ptr mappingServer; if(args.runServer) { - const MappingServer::Mode mode = args.pipelineType == "collaborative" ? MappingServer::SM_MULTI_CLIENT : MappingServer::SM_SINGLE_CLIENT; + const MappingServer::Mode mode = + args.pipelineType == "collaborative" ? MappingServer::SM_MULTI_CLIENT : MappingServer::SM_SINGLE_CLIENT; mappingServer.reset(new MappingServer(mode)); mappingServer->start(); } // Construct the pipeline. MultiScenePipeline_Ptr pipeline; - if(args.pipelineType != "collaborative") - { - const size_t maxLabelCount = 10; - SLAMComponent::MappingMode mappingMode = args.mapSurfels ? SLAMComponent::MAP_BOTH : SLAMComponent::MAP_VOXELS_ONLY; - SLAMComponent::TrackingMode trackingMode = args.trackSurfels ? SLAMComponent::TRACK_SURFELS : SLAMComponent::TRACK_VOXELS; - - if(args.pipelineType == "slam") - { - pipeline.reset(new SLAMPipeline( - settings, - Application::resources_dir().string(), - make_image_source_engine(args), - make_tracker_config(args), - mappingMode, - trackingMode, - args.modelDir, - args.detectFiducials - )); - } - else if(args.pipelineType == "semantic") - { - const unsigned int seed = 12345; - pipeline.reset(new SemanticPipeline( - settings, - Application::resources_dir().string(), - maxLabelCount, - make_image_source_engine(args), - seed, - make_tracker_config(args), - mappingMode, - trackingMode, - args.modelDir, - args.detectFiducials - )); - } - else if(args.pipelineType == "objective") - { - pipeline.reset(new ObjectivePipeline( - settings, - Application::resources_dir().string(), - maxLabelCount, - make_image_source_engine(args), - make_tracker_config(args), - mappingMode, - trackingMode, - args.detectFiducials, - !args.trackObject - )); - } - else throw std::runtime_error("Unknown pipeline type: " + args.pipelineType); - } - else - { - // Set a reasonable default for the voxel size (this can be overridden using a configuration file). - if(!settings->has_values("SceneParams.voxelSize")) - { - settings->sceneParams.voxelSize = 0.015f; - settings->sceneParams.mu = settings->sceneParams.voxelSize * 4; - } - - // Set up the image source engines, mapping modes, tracking modes and tracker configurations. - std::vector imageSourceEngines; - std::vector mappingModes; - std::vector trackingModes; - std::vector trackerConfigs; - - // Add an image source engine for each disk sequence specified. - for(size_t i = 0; i < args.sequences.size(); ++i) - { - const bf::path calibrationPath = args.sequences[i]->default_calib_path(); - const std::string calibrationFilename = bf::exists(calibrationPath) ? calibrationPath.string() : args.calibrationFilename; - - std::cout << "[spaint] Adding local agent for disk sequence: " << *args.sequences[i] << '\n'; - CompositeImageSourceEngine_Ptr imageSourceEngine(new CompositeImageSourceEngine); - imageSourceEngine->addSubengine(new AsyncImageSourceEngine(args.sequences[i]->make_image_source_engine(calibrationFilename), args.prefetchBufferCapacity)); - - imageSourceEngines.push_back(imageSourceEngine); - } - - // Set up the mapping modes, tracking modes and tracker configurations. - for(size_t i = 0, size = imageSourceEngines.size(); i < size; ++i) - { - mappingModes.push_back(args.mapSurfels ? SLAMComponent::MAP_BOTH : SLAMComponent::MAP_VOXELS_ONLY); - trackingModes.push_back(args.trackSurfels ? SLAMComponent::TRACK_SURFELS : SLAMComponent::TRACK_VOXELS); - - // FIXME: We don't always want to read the poses from disk - make it possible to run the normal tracker instead. - trackerConfigs.push_back(args.sequences[i]->make_disk_tracker_config()); - } - - // Construct the pipeline itself. - const CollaborationMode collaborationMode = args.collaborationMode == "batch" ? CM_BATCH : CM_LIVE; - pipeline.reset(new CollaborativePipeline( - settings, - Application::resources_dir().string(), - imageSourceEngines, - trackerConfigs, - mappingModes, - trackingModes, - args.detectFiducials, - mappingServer, - collaborationMode - )); - } + // Set a reasonable default for the voxel size (this can be overridden using a configuration file). + if(!settings->has_values("SceneParams.voxelSize")) + { + settings->sceneParams.voxelSize = 0.015f; + settings->sceneParams.mu = settings->sceneParams.voxelSize * 4; + } + const size_t maxLabelCount = 10; + SLAMComponent::MappingMode mappingMode = args.mapSurfels ? SLAMComponent::MAP_BOTH : SLAMComponent::MAP_VOXELS_ONLY; + SLAMComponent::TrackingMode trackingMode = + args.trackSurfels ? SLAMComponent::TRACK_SURFELS : SLAMComponent::TRACK_VOXELS; + pipeline.reset(new SLAMPipeline(settings, + Application::resources_dir().string(), + make_image_source_engine(args), + make_tracker_config(args), + mappingMode, + trackingMode, + args.modelDir, + args.detectFiducials)); // If a remote host was specified, set up a mapping client for the world scene. if(args.host != "") { std::cout << "Setting mapping client for host '" << args.host << "' and port '" << args.port << "'\n"; - const pooled_queue::PoolEmptyStrategy poolEmptyStrategy = settings->get_first_value("MappingClient.poolEmptyStrategy", pooled_queue::PES_DISCARD); - pipeline->set_mapping_client(Model::get_world_scene_id(), MappingClient_Ptr(new MappingClient(args.host, args.port, poolEmptyStrategy))); + const pooled_queue::PoolEmptyStrategy poolEmptyStrategy = + settings->get_first_value("MappingClient.poolEmptyStrategy", + pooled_queue::PES_DISCARD); + pipeline->set_mapping_client(Model::get_world_scene_id(), + MappingClient_Ptr(new MappingClient(args.host, args.port, poolEmptyStrategy))); } #ifdef WITH_LEAP @@ -966,7 +953,7 @@ try #endif // Configure and run the application. - Application app(pipeline, args.renderFiducials); + Application app(pipeline, args.renderFiducials, args.name); if(args.batch) app.set_batch_mode_enabled(true); if(args.runServer) app.set_server_mode_enabled(true); app.set_save_memory_usage(args.profileMemory); @@ -980,10 +967,10 @@ try // If we're not running in headless mode, shut down the GUI-only subsystems. if(!args.headless) { - #ifdef WITH_OVR +#ifdef WITH_OVR // If we built with Rift support, shut down the Rift SDK. ovr_Shutdown(); - #endif +#endif // Shut down SDL. SDL_Quit(); @@ -991,7 +978,7 @@ try return runSucceeded ? EXIT_SUCCESS : EXIT_FAILURE; } -catch(std::exception& e) +catch(std::exception &e) { std::cerr << e.what() << '\n'; return EXIT_FAILURE; diff --git a/modules/grove/src/ransac/interface/PreemptiveRansac.cpp b/modules/grove/src/ransac/interface/PreemptiveRansac.cpp index d6ba86f..423c6b4 100644 --- a/modules/grove/src/ransac/interface/PreemptiveRansac.cpp +++ b/modules/grove/src/ransac/interface/PreemptiveRansac.cpp @@ -13,11 +13,17 @@ using namespace tvgutil; #include #include +#include +#include +#include + #include #ifdef WITH_OPENMP #include #endif +#include +#include #include #include @@ -126,15 +132,26 @@ void PreemptiveRansac::update_candidate_poses() // Note: This is a default implementation of the abstract function - it is intended to be called / overridden by derived classes. const int nbPoseCandidates = static_cast(m_poseCandidates->dataSize); + std::unordered_set threadCnt{}; + boost::mutex m; + int ttid = syscall(SYS_gettid); #ifdef WITH_OPENMP - #pragma omp parallel for schedule(dynamic) + #pragma omp parallel for schedule(dynamic) num_threads(24) #endif for(int i = 0; i < nbPoseCandidates; ++i) { + int tid = syscall(SYS_gettid); + // m.lock(); + // threadCnt.insert(tid); + // m.unlock(); + // std::cout << tid << ": in update_candidate_poses openmp\n"; update_candidate_pose(i); } + // std::cout << ttid << ": this is cnt size in update_candidate_poses: " < PreemptiveRansac::estimate_pose(const Keypoint3DColourImage_CPtr& keypointsImage, const ScorePredictionsImage_CPtr& predictionsImage) @@ -322,10 +339,12 @@ void PreemptiveRansac::compute_candidate_poses_kabsch() #if 0 std::cout << "Generated " << nbPoseCandidates << " candidates." << std::endl; #endif - + boost::mutex m; + std::unordered_set threadCnt{}; + int ttid = syscall(SYS_gettid); // For each candidate: #ifdef WITH_OPENMP - #pragma omp parallel for + #pragma omp parallel for num_threads(16) #endif for(int candidateIdx = 0; candidateIdx < nbPoseCandidates; ++candidateIdx) { @@ -336,13 +355,19 @@ void PreemptiveRansac::compute_candidate_poses_kabsch() Eigen::Matrix3f worldPoints; for(int i = 0; i < PoseCandidate::KABSCH_CORRESPONDENCES_NEEDED; ++i) { +// int tid = syscall(SYS_gettid); +// m.lock(); +// threadCnt.insert(tid); +// m.unlock(); + //std::cout << tid << ": in compute_candidate_poses_kabsch openmp\n"; cameraPoints.col(i) = Eigen::Map(candidate.pointsCamera[i].v); worldPoints.col(i) = Eigen::Map(candidate.pointsWorld[i].v); } - // Run the Kabsch algorithm and store the resulting camera -> world transformation in the candidate's cameraPose matrix. Eigen::Map(candidate.cameraPose.m) = GeometryUtil::estimate_rigid_transform(cameraPoints, worldPoints); } + // std::cout << ttid << ": this is cnt size in compute_candidate_poses_kabsch: " << threadCnt.size() <<"\n"; + //sleep(35); } void PreemptiveRansac::reset_inliers(bool resetMask) diff --git a/modules/grove/src/relocalisation/interface/ScoreForestRelocaliser.cpp b/modules/grove/src/relocalisation/interface/ScoreForestRelocaliser.cpp index 0436a24..00408a1 100644 --- a/modules/grove/src/relocalisation/interface/ScoreForestRelocaliser.cpp +++ b/modules/grove/src/relocalisation/interface/ScoreForestRelocaliser.cpp @@ -37,7 +37,8 @@ ScoreForestRelocaliser::ScoreForestRelocaliser(const SettingsContainer_CPtr& set } else { - const std::string modelFilename = m_settings->get_first_value(settingsNamespace + "modelFilename", (find_subdir_from_executable("resources") / "DefaultRelocalisationForest.rf").string()); + // const std::string modelFilename = m_settings->get_first_value(settingsNamespace + "modelFilename", (find_subdir_from_executable("resources") / "DefaultRelocalisationForest.rf").string()); + const std::string modelFilename = "randomForest/DefaultRelocalisationForest.rf"; std::cout << "Loading relocalisation forest from: " << modelFilename << '\n'; m_scoreForest = DecisionForestFactory::make_forest(modelFilename, deviceType); } diff --git a/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp b/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp index 9c0204e..a3f5f09 100644 --- a/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp +++ b/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp @@ -8,6 +8,9 @@ using namespace ORUtils; using namespace tvgutil; #include +#include +#include +#include namespace bf = boost::filesystem; #include @@ -68,8 +71,25 @@ ScoreRelocaliser::ScoreRelocaliser(const SettingsContainer_CPtr& settings, const } //#################### DESTRUCTOR #################### - -ScoreRelocaliser::~ScoreRelocaliser() {} +int totalRelocalise = 0; +double totalRelocaliset0 = 0.0; +double totalRelocaliset1 = 0.0; +double totalRelocaliset2 = 0.0; +int totalGetBestPose = 0; +double totalGetBestPoset = 0.0; +ScoreRelocaliser::~ScoreRelocaliser() { + std::cout << "in ~ScoreRelocaliser()\n"; + if (totalRelocalise!=0) { + std::cout << "totalRelocalise: " << totalRelocalise << "\n"; + std::cout << "average t0: " << totalRelocaliset0/(double)totalRelocalise << "ms\n"; + std::cout << "average t1: " << totalRelocaliset1/(double)totalRelocalise << "ms\n"; + std::cout << "average t2: " << totalRelocaliset2/(double)totalRelocalise << "ms\n"; + } + if (totalGetBestPose!=0) { + std::cout << "totalGetBestPose: " << totalGetBestPose << "\n"; + std::cout << "average t3: " << totalGetBestPoset/(double)totalGetBestPose << "ms\n"; + } +} //#################### PUBLIC MEMBER FUNCTIONS #################### @@ -123,6 +143,7 @@ void ScoreRelocaliser::load_from_disk(const std::string& inputFolder) m_relocaliserState->load_from_disk(inputFolder); } + std::vector ScoreRelocaliser::relocalise(const ORUChar4Image *colourImage, const ORFloatImage *depthImage, const Vector4f& depthIntrinsics) const { boost::lock_guard lock(m_mutex); @@ -134,14 +155,29 @@ std::vector ScoreRelocaliser::relocalise(const ORUChar4Imag { // Step 1: Extract keypoints from the RGB-D image and compute descriptors for them. // FIXME: We only need to compute the descriptors if we're using the forest. + totalRelocalise++; + struct timeval t0, t1; + double t_cost; + gettimeofday(&t0,NULL); m_featureCalculator->compute_keypoints_and_features(colourImage, depthImage, depthIntrinsics, m_keypointsImage.get(), m_descriptorsImage.get()); - + gettimeofday(&t1,NULL); + t_cost = (t1.tv_sec - t0.tv_sec)*1000.0 + (double)(t1.tv_usec - t0.tv_usec)/1000.0; + // std::cout << "1. compute_keypoints_and_features time cost: " << t_cost << "ms\n"; + totalRelocaliset0 += t_cost; // Step 2: Create a single SCoRe prediction (a single set of clusters) for each keypoint. + gettimeofday(&t0,NULL); make_predictions(colourImage); - + gettimeofday(&t1,NULL); + t_cost = (t1.tv_sec - t0.tv_sec)*1000.0 + (double)(t1.tv_usec - t0.tv_usec)/1000.0; + // std::cout << "2. make_predictions time cost: " << t_cost << "ms\n"; + totalRelocaliset1 += t_cost; // Step 3: Perform P-RANSAC to try to estimate the camera pose. + gettimeofday(&t0,NULL); boost::optional poseCandidate = m_preemptiveRansac->estimate_pose(m_keypointsImage, m_predictionsImage); - + gettimeofday(&t1,NULL); + t_cost = (t1.tv_sec - t0.tv_sec)*1000.0 + (double)(t1.tv_usec - t0.tv_usec)/1000.0; + //std::cout << "3. estimate_pose time cost: " << t_cost << "ms\n"; + totalRelocaliset2 += t_cost; // Step 4: If we succeeded in estimating a camera pose: if(poseCandidate) { @@ -157,8 +193,13 @@ std::vector ScoreRelocaliser::relocalise(const ORUChar4Imag { // Get all of the candidates that survived the initial culling process during P-RANSAC. std::vector candidates; - m_preemptiveRansac->get_best_poses(candidates); - + totalGetBestPose++; + gettimeofday(&t0,NULL); + m_preemptiveRansac->get_best_poses(candidates); + gettimeofday(&t1,NULL); + t_cost = (t1.tv_sec - t0.tv_sec)*1000.0 + (double)(t1.tv_usec - t0.tv_usec)/1000.0; + std::cout << "4. get_best_poses time cost: " << t_cost << "ms\n"; + totalGetBestPoset += t_cost; // Add the best candidates to the results (skipping the first one, since it's the same one returned by estimate_pose above). const size_t maxElements = std::min(candidates.size(), m_maxRelocalisationsToOutput); for(size_t i = 1; i < maxElements; ++i) diff --git a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h index 407029a..899fea7 100644 --- a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h +++ b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h @@ -10,6 +10,9 @@ #include #include +#include +#include + #include #include @@ -21,6 +24,9 @@ namespace spaint { +const int relocalisationThreadsCount = 4; +const int bestCandidateMaxCount = 4; + /** * \brief An instance of this pipeline component can be used to determine the relative poses between agents participating in collaborative SLAM. */ @@ -31,6 +37,8 @@ class CollaborativeComponent /** The best relocalisation candidate, as chosen by the scheduler. This will be the next relocalisation attempted. */ boost::shared_ptr m_bestCandidate; + std::list> m_bestCandidates = std::list>(); + /** The timer used to compute the time spent collaborating. */ boost::optional m_collaborationTimer; @@ -61,6 +69,8 @@ class CollaborativeComponent /** The thread on which relocalisations should be attempted. */ boost::thread m_relocalisationThread; + std::vector m_relocalisationThreads = std::vector(relocalisationThreadsCount); + /** The results of every relocalisation that has been attempted. */ std::deque m_results; @@ -148,7 +158,8 @@ class CollaborativeComponent /** * \brief Runs the relocalisation thread, repeatedly attempting scheduled relocalisations until the collaborative component is destroyed. */ - void run_relocalisation(); + // void run_relocalisation(); + void run_relocalisation(cpu_set_t mask); /** * \brief Scores all of the specified candidate relocalisations to allow one of them to be chosen for a relocalisation attempt. diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index 7676af3..99aab89 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -8,6 +8,9 @@ using namespace ITMLib; using namespace ORUtils; using namespace itmx; +#include +#include + #include #include @@ -21,8 +24,45 @@ using boost::bind; #include using namespace orx; +#include +#include +#include + #define DEBUGGING 0 +int parseLine(char *line) { + // This assumes that a digit will be found and the line ends in " Kb". + int i = strlen(line); + const char *p = line; + while (*p < '0' || *p > '9') p++; + line[i - 3] = '\0'; + i = atoi(p); + return i; +} +typedef struct { + uint32_t virtualMem; + uint32_t physicalMem; +} processMem_t; +processMem_t GetProcessMemory() { + FILE *file = fopen("/proc/self/status", "r"); + char line[128]; + processMem_t processMem; + + while (fgets(line, 128, file) != NULL) { + if (strncmp(line, "VmSize:", 7) == 0) { + processMem.virtualMem = parseLine(line); + break; + } + + if (strncmp(line, "VmRSS:", 6) == 0) { + processMem.physicalMem = parseLine(line); + break; + } + } + fclose(file); + return processMem; +} + namespace spaint { //#################### CONSTRUCTORS #################### @@ -42,10 +82,8 @@ CollaborativeComponent::CollaborativeComponent(const CollaborativeContext_Ptr& c m_stopAtFirstConsistentReconstruction = settings->get_first_value(settingsNamespace + "stopAtFirstConsistentReconstruction", false); m_timeCollaboration = settings->get_first_value(settingsNamespace + "timeCollaboration", false); - m_relocalisationThread = boost::thread(boost::bind(&CollaborativeComponent::run_relocalisation, this)); - const std::string globalPosesSpecifier = settings->get_first_value("globalPosesSpecifier", ""); - m_context->get_collaborative_pose_optimiser()->start(globalPosesSpecifier); + } //#################### DESTRUCTOR #################### @@ -53,8 +91,12 @@ CollaborativeComponent::CollaborativeComponent(const CollaborativeContext_Ptr& c CollaborativeComponent::~CollaborativeComponent() { m_stopRelocalisationThread = true; - m_readyToRelocalise.notify_one(); - m_relocalisationThread.join(); + // m_readyToRelocalise.notify_one(); + m_readyToRelocalise.notify_all(); + for (int i=0; i lock(m_mutex); - while(!m_bestCandidate) + // while(!m_bestCandidate) + while (m_bestCandidates.empty()) { m_readyToRelocalise.wait(lock); @@ -370,27 +422,31 @@ void CollaborativeComponent::run_relocalisation() if(m_stopRelocalisationThread) return; } } - - std::cout << "Attempting to relocalise frame " << m_bestCandidate->m_frameIndexJ << " of " << m_bestCandidate->m_sceneJ << " against " << m_bestCandidate->m_sceneI << "..."; + m_mutex.lock(); + auto now_bestCandidate = m_bestCandidates.front(); + m_bestCandidates.pop_front(); + m_mutex.unlock(); + std::cout << tid <<" : Attempting to relocalise frame " << now_bestCandidate->m_frameIndexJ << " of " << now_bestCandidate->m_sceneJ << " against " << now_bestCandidate->m_sceneI << "..."; + // std::cout << "Attempting to relocalise frame " << m_bestCandidate->m_frameIndexJ << " of " << m_bestCandidate->m_sceneJ << " against " << m_bestCandidate->m_sceneI << "..."; // Render synthetic images of the source scene from the relevant pose and copy them across to the GPU for use by the relocaliser. // The synthetic images have the size of the images in the target scene and are generated using the target scene's intrinsics. - const SLAMState_CPtr slamStateI = m_context->get_slam_state(m_bestCandidate->m_sceneI); - const SLAMState_CPtr slamStateJ = m_context->get_slam_state(m_bestCandidate->m_sceneJ); + const SLAMState_CPtr slamStateI = m_context->get_slam_state(now_bestCandidate->m_sceneI); + const SLAMState_CPtr slamStateJ = m_context->get_slam_state(now_bestCandidate->m_sceneJ); const View_CPtr viewI = slamStateI->get_view(); ORFloatImage_Ptr depth(new ORFloatImage(slamStateI->get_depth_image_size(), true, true)); ORUChar4Image_Ptr rgb(new ORUChar4Image(slamStateI->get_rgb_image_size(), true, true)); - VoxelRenderState_Ptr& renderStateD = m_depthRenderStates[m_bestCandidate->m_sceneI]; + VoxelRenderState_Ptr& renderStateD = m_depthRenderStates[now_bestCandidate->m_sceneI]; m_visualisationGenerator->generate_depth_from_voxels( - depth, slamStateJ->get_voxel_scene(), m_bestCandidate->m_localPoseJ, viewI->calib.intrinsics_d, + depth, slamStateJ->get_voxel_scene(), now_bestCandidate->m_localPoseJ, viewI->calib.intrinsics_d, renderStateD, DepthVisualiser::DT_ORTHOGRAPHIC ); - VoxelRenderState_Ptr& renderStateRGB = m_rgbRenderStates[m_bestCandidate->m_sceneI]; + VoxelRenderState_Ptr& renderStateRGB = m_rgbRenderStates[now_bestCandidate->m_sceneI]; m_visualisationGenerator->generate_voxel_visualisation( - rgb, slamStateJ->get_voxel_scene(), m_bestCandidate->m_localPoseJ, viewI->calib.intrinsics_rgb, + rgb, slamStateJ->get_voxel_scene(), now_bestCandidate->m_localPoseJ, viewI->calib.intrinsics_rgb, renderStateRGB, VisualisationGenerator::VT_SCENE_COLOUR, boost::none ); @@ -410,12 +466,13 @@ void CollaborativeComponent::run_relocalisation() #endif // Attempt to relocalise the synthetic images using the relocaliser for the target scene. - Relocaliser_CPtr relocaliserI = m_context->get_relocaliser(m_bestCandidate->m_sceneI); - std::vector results = relocaliserI->relocalise(rgb.get(), depth.get(), m_bestCandidate->m_depthIntrinsicsI); + Relocaliser_CPtr relocaliserI = m_context->get_relocaliser(now_bestCandidate->m_sceneI); + std::cout << "b5\n"; + std::vector results = relocaliserI->relocalise(rgb.get(), depth.get(), now_bestCandidate->m_depthIntrinsicsI); boost::optional result = results.empty() ? boost::none : boost::optional(results[0]); // If the relocaliser returned a result, store the initial relocalisation quality for later examination. - if(result) m_bestCandidate->m_initialRelocalisationQuality = result->quality; + if(result) now_bestCandidate->m_initialRelocalisationQuality = result->quality; // If relocalisation succeeded, verify the result by thresholding the difference between the // source depth image and a rendered depth image of the target scene at the relevant pose. @@ -465,19 +522,20 @@ void CollaborativeComponent::run_relocalisation() #endif // Determine the average depth difference for valid pixels in the source and target depth images. - m_bestCandidate->m_meanDepthDiff = cv::mean(cvMaskedDepthDiff); + now_bestCandidate->m_meanDepthDiff = cv::mean(cvMaskedDepthDiff); #if DEBUGGING - std::cout << "\nMean Depth Difference: " << m_bestCandidate->m_meanDepthDiff << std::endl; + std::cout << "\nMean Depth Difference: " << now_bestCandidate->m_meanDepthDiff << std::endl; #endif // Compute the fraction of the target depth image that is valid. - m_bestCandidate->m_targetValidFraction = static_cast(cv::countNonZero(cvTargetMask == 255)) / (cvTargetMask.size().width * cvTargetMask.size().height); + now_bestCandidate->m_targetValidFraction = static_cast(cv::countNonZero(cvTargetMask == 255)) / (cvTargetMask.size().width * cvTargetMask.size().height); #if DEBUGGING std::cout << "Valid Target Pixels: " << cv::countNonZero(cvTargetMask == 255) << std::endl; #endif // Decide whether or not to verify the relocalisation, based on the average depth difference and the fraction of the target depth image that is valid. - verified = is_verified(*m_bestCandidate); + // verified = is_verified(*m_bestCandidate); + verified = is_verified(*now_bestCandidate); #else // If we didn't build with OpenCV, we can't do any verification, so just mark the relocalisation as verified and hope for the best. verified = true; @@ -489,8 +547,8 @@ void CollaborativeComponent::run_relocalisation() if(verified) { // cjTwi^-1 * cjTwj = wiTcj * cjTwj = wiTwj - m_bestCandidate->m_relativePose = ORUtils::SE3Pose(result->pose.GetInvM() * m_bestCandidate->m_localPoseJ.GetM()); - m_context->get_collaborative_pose_optimiser()->add_relative_transform_sample(m_bestCandidate->m_sceneI, m_bestCandidate->m_sceneJ, *m_bestCandidate->m_relativePose, m_mode); + now_bestCandidate->m_relativePose = ORUtils::SE3Pose(result->pose.GetInvM() * now_bestCandidate->m_localPoseJ.GetM()); + m_context->get_collaborative_pose_optimiser()->add_relative_transform_sample(now_bestCandidate->m_sceneI, now_bestCandidate->m_sceneJ, *now_bestCandidate->m_relativePose, m_mode); std::cout << "succeeded!" << std::endl; #if defined(WITH_OPENCV) && DEBUGGING @@ -512,9 +570,9 @@ void CollaborativeComponent::run_relocalisation() #if DEBUGGING m_results.push_back(*m_bestCandidate); #endif - m_bestCandidate.reset(); + now_bestCandidate.reset(); + // m_bestCandidate.reset(); } - // In live mode, allow a bit of extra time for training before running the next relocalisation. // FIXME: This is a bit hacky - we might want to improve this in the future. if(m_mode == CM_LIVE) boost::this_thread::sleep_for(boost::chrono::milliseconds(100)); @@ -569,8 +627,11 @@ void CollaborativeComponent::try_schedule_relocalisation() boost::unique_lock lock(m_mutex); // If an existing relocalisation attempt is in progress, early out. - if(m_bestCandidate) return; - + // if(m_bestCandidate) return; + if (m_bestCandidates.size()>=bestCandidateMaxCount) { + m_readyToRelocalise.notify_one(); + return; + } #if 1 // Randomly generate a list of candidate relocalisations. const size_t desiredCandidateCount = 10; @@ -599,13 +660,17 @@ void CollaborativeComponent::try_schedule_relocalisation() #endif // Schedule the best candidate for relocalisation. - m_bestCandidate.reset(new CollaborativeRelocalisation(candidates.back())); - + // m_bestCandidate.reset(new CollaborativeRelocalisation(candidates.back())); + // auto now_bestCandidate = new CollaborativeRelocalisation(candidates.back()); + boost::shared_ptr now_bestCandidate(new CollaborativeRelocalisation(candidates.back())); + m_bestCandidates.push_back(now_bestCandidate); // If we're in batch mode, record the index of the frame we're trying in case we want to avoid frames with similar poses later. if(m_mode == CM_BATCH) { - std::set& triedFrameIndices = m_triedFrameIndices[std::make_pair(m_bestCandidate->m_sceneI, m_bestCandidate->m_sceneJ)]; - triedFrameIndices.insert(m_bestCandidate->m_frameIndexJ); + // std::set& triedFrameIndices = m_triedFrameIndices[std::make_pair(m_bestCandidate->m_sceneI, m_bestCandidate->m_sceneJ)]; + // triedFrameIndices.insert(m_bestCandidate->m_frameIndexJ); + std::set& triedFrameIndices = m_triedFrameIndices[std::make_pair(now_bestCandidate->m_sceneI, now_bestCandidate->m_sceneJ)]; + triedFrameIndices.insert(now_bestCandidate->m_frameIndexJ); } } diff --git a/tmp b/tmp new file mode 100644 index 0000000..70054d2 --- /dev/null +++ b/tmp @@ -0,0 +1,47 @@ +#include +int totalRelocalise = 0; +double totalRelocaliset0 = 0.0; +double totalRelocaliset1 = 0.0; +double totalRelocaliset2 = 0.0; +int totalGetBestPose = 0; +double totalGetBestPoset = 0.0; +std::cout << "in ~ScoreRelocaliser()\n"; + if (totalRelocalise!=0) { + std::cout << "totalRelocalise: " << totalRelocalise << "\n"; + std::cout << "average t0: " << totalRelocaliset0/(double)totalRelocalise << "ms\n"; + std::cout << "average t1: " << totalRelocaliset1/(double)totalRelocalise << "ms\n"; + std::cout << "average t2: " << totalRelocaliset2/(double)totalRelocalise << "ms\n"; + } + if (totalGetBestPose!=0) { + std::cout << "totalGetBestPose: " << totalGetBestPose << "\n"; + std::cout << "average t3: " << totalGetBestPoset/(double)totalGetBestPose << "ms\n"; + } + + totalRelocalise++; + struct timeval t0, t1; + double t_cost; + gettimeofday(&t0,NULL); + + t_cost = (t1.tv_sec - t0.tv_sec)*1000.0 + (double)(t1.tv_usec - t0.tv_usec)/1000.0; + std::cout << "1. compute_keypoints_and_features time cost: " << t_cost << "ms\n"; + totalRelocaliset0 += t_cost; + + + gettimeofday(&t1,NULL); + t_cost = (t1.tv_sec - t0.tv_sec)*1000.0 + (double)(t1.tv_usec - t0.tv_usec)/1000.0; + std::cout << "2. make_predictions time cost: " << t_cost << "ms\n"; + totalRelocaliset1 += t_cost; + + + gettimeofday(&t1,NULL); + t_cost = (t1.tv_sec - t0.tv_sec)*1000.0 + (double)(t1.tv_usec - t0.tv_usec)/1000.0; + std::cout << "3. estimate_pose time cost: " << t_cost << "ms\n"; + totalRelocaliset2 += t_cost; + + + totalGetBestPose++; + gettimeofday(&t0,NULL); + gettimeofday(&t1,NULL); + t_cost = (t1.tv_sec - t0.tv_sec)*1000.0 + (double)(t1.tv_usec - t0.tv_usec)/1000.0; + std::cout << "4. get_best_poses time cost: " << t_cost << "ms\n"; + totalGetBestPoset += t_cost;