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..e72afc6 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,73 @@ +{ + "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", + "condition_variable": "cpp", + "cstdarg": "cpp", + "mutex": "cpp", + "unordered_set": "cpp" + } +} \ No newline at end of file diff --git a/apps/spaintgui/Application.cpp b/apps/spaintgui/Application.cpp index 9caa1b9..d2bc0f1 100644 --- a/apps/spaintgui/Application.cpp +++ b/apps/spaintgui/Application.cpp @@ -5,6 +5,11 @@ #include "Application.h" using namespace tvginput; +#include +#include +#include +#include +#include #include #include @@ -51,13 +56,14 @@ using namespace tvgutil; //#################### CONSTRUCTORS #################### -Application::Application(const MultiScenePipeline_Ptr& pipeline, bool renderFiducials) +Application::Application(const MultiScenePipeline_Ptr& pipeline, const std::map &sceneID2Name, bool renderFiducials) : m_activeSubwindowIndex(0), m_batchModeEnabled(false), m_commandManager(10), m_pauseBetweenFrames(true), m_paused(true), m_pipeline(pipeline), + m_sceneID2Name(sceneID2Name), m_renderFiducials(renderFiducials), m_saveModelsOnExit(false), m_usePoseMirroring(true), @@ -84,8 +90,16 @@ Application::Application(const MultiScenePipeline_Ptr& pipeline, bool renderFidu bool Application::run() { + time_t start, end; + double cost = 0; + time(&start); for(int i=0;;i++) - { + { + time(&end); + cost = difftime(end, start); + if (cost>30.0) { + break; + } // Check to see if the user wants to quit the application, and quit if necessary. Note that if we // are running in batch mode, we quit directly, rather than saving a mesh of the scene on exit. bool eventQuit = !process_events(); @@ -98,7 +112,6 @@ bool Application::run() // Take action as relevant based on the current input state. process_input(); - // If the application is unpaused, process a new frame. if(!m_paused) { @@ -118,8 +131,9 @@ bool Application::run() break; } } - + usleep(100000); // Render the scene. + /* m_renderer->render(m_fracWindowPos, m_renderFiducials); // If we're running a mapping server and we want to render any scene images requested by remote clients, do so. @@ -135,13 +149,14 @@ bool Application::run() // If desired, pause at the end of each frame for debugging purposes. if(m_pauseBetweenFrames) m_paused = true; + */ } // If desired, save a mesh of the scene before the application terminates. 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(); return true; } @@ -959,16 +974,23 @@ void Application::save_mesh() const const std::vector sceneIDs = model->get_scene_ids(); // Determine the (base) filename to use for the mesh, based on either the experiment tag (if specified) or the current timestamp (otherwise). - std::string meshBaseName = settings->get_first_value("experimentTag", "spaint-" + TimeUtil::get_iso_timestamp()); + // std::string meshBaseName = settings->get_first_value("experimentTag", "spaint-" + TimeUtil::get_iso_timestamp()); // Determine the directory into which to save the meshes, and make sure that it exists. + /* boost::filesystem::path dir = find_subdir_from_executable("meshes"); if(sceneIDs.size() > 1) dir = dir / meshBaseName; boost::filesystem::create_directories(dir); + */ + if (!(model->get_collaborative_pose_optimiser()->isSuccess())) { + return; + } // Mesh each scene independently. + std::vector mergeFilesName; + std::vector filesRootName; for(size_t sceneIdx = 0; sceneIdx < sceneIDs.size(); ++sceneIdx) - { + { const std::string& sceneID = sceneIDs[sceneIdx]; std::cout << "Meshing " << sceneID << " scene.\n"; SpaintVoxelScene_CPtr scene = model->get_slam_state(sceneID)->get_voxel_scene(); @@ -1011,6 +1033,7 @@ void Application::save_mesh() const // Next, transform each triangle using the relative transform determined above. const Matrix4f transform = *relativeTransform; + std::cout << "transform:\n" << transform << "\n"; Triangle *trianglesData = triangles->GetData(MEMORYDEVICE_CPU); for(size_t triangleIdx = 0; triangleIdx < mesh->noTotalTriangles; ++triangleIdx) { @@ -1024,10 +1047,37 @@ void Application::save_mesh() const } // Save the mesh to disk. - const boost::filesystem::path meshPath = dir / (meshBaseName + "_" + sceneID + ".ply"); + // const boost::filesystem::path meshPath = dir / (meshBaseName + "_" + sceneID + ".ply"); + auto sceneName = m_sceneID2Name.find(sceneID); + const boost::filesystem::path meshPath = sceneName->second + "-" + TimeUtil::get_iso_timestamp() + ".ply"; + mergeFilesName.push_back(meshPath.string()); std::cout << "Saving mesh to: " << meshPath << '\n'; mesh->WritePLY(meshPath.string().c_str()); } + std::string worldScene = m_sceneID2Name.find("World")->second; + std::string localScene = m_sceneID2Name.find("Local1")->second; + std::string outputFile = worldScene + "-" + localScene + ".ply"; + pid_t pid = fork(); + if (pid < 0) { + printf("error create merge program\n"); + exit(0); + } + if (pid == 0) { + std::cout << "fork python program " << mergeFilesName[0] << " " << mergeFilesName[1] << "\n"; + char *argv[] = {"python3","mergeMesh.py", + "--file1", const_cast(mergeFilesName[0].c_str()), + "--file2",const_cast(mergeFilesName[1].c_str()), + "--pose", "worldPose.txt", + "--output", const_cast(outputFile.c_str()), + NULL}; + execvp("python3", argv); + std::cout << "after execlp python program\n"; + }else { + std::cout << "this is origin program\n"; + int status; + waitpid(pid, &status, 0); + std::cout << "after waitpid\n"; + } } void Application::save_models() const @@ -1170,3 +1220,4 @@ void Application::toggle_recording(const std::string& type, boost::optionalget_base_dir() << "...\n"; } } + diff --git a/apps/spaintgui/Application.h b/apps/spaintgui/Application.h index 6599b78..eeba3e0 100644 --- a/apps/spaintgui/Application.h +++ b/apps/spaintgui/Application.h @@ -6,6 +6,8 @@ #ifndef H_SPAINTGUI_APPLICATION #define H_SPAINTGUI_APPLICATION +#include + #include #include @@ -105,6 +107,9 @@ class Application /** The stream of commands being sent from the voice command server. */ boost::asio::ip::tcp::iostream m_voiceCommandStream; + /** scene name and scene ID map*/ + std::map m_sceneID2Name; + //#################### CONSTRUCTORS #################### public: /** @@ -113,7 +118,9 @@ 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, + const std::map &sceneID2Name, + bool renderFiducials = false); //#################### PUBLIC MEMBER FUNCTIONS #################### public: @@ -356,3 +363,4 @@ class Application }; #endif + diff --git a/apps/spaintgui/core/CollaborativePipeline.cpp b/apps/spaintgui/core/CollaborativePipeline.cpp index 7e651f0..a118684 100644 --- a/apps/spaintgui/core/CollaborativePipeline.cpp +++ b/apps/spaintgui/core/CollaborativePipeline.cpp @@ -11,67 +11,49 @@ using namespace spaint; //#################### CONSTRUCTORS #################### -CollaborativePipeline::CollaborativePipeline(const Settings_Ptr& settings, const std::string& resourcesDir, - const std::vector& imageSourceEngines, - const std::vector& trackerConfigs, - const std::vector& mappingModes, - const std::vector& trackingModes, - bool detectFiducials, const MappingServer_Ptr& mappingServer, - CollaborationMode collaborationMode) +CollaborativePipeline::CollaborativePipeline(const Settings_Ptr &settings, + const std::string &resourcesDir, + const std::map &scenesPoseCnt, + const std::map &sceneID2Name, + const std::vector &imageSourceEngines, + const std::vector &trackerConfigs, + const std::vector &mappingModes, + const std::vector &trackingModes, + bool detectFiducials, + const MappingServer_Ptr &mappingServer, + CollaborationMode collaborationMode, + const std::map &sceneDirs) // Note: A minimum of 2 labels is required (background and foreground). -: MultiScenePipeline("collaborative", settings, resourcesDir, 2, mappingServer), - m_collaborationStarted(false), - m_detectFiducials(detectFiducials), - m_worldIsRemote(imageSourceEngines.empty()) + : MultiScenePipeline("collaborative", settings, resourcesDir, 2, mappingServer), + m_collaborationStarted(false), + m_detectFiducials(detectFiducials), + // m_worldIsRemote(imageSourceEngines.empty()), + m_worldIsRemote(false), + m_sceneDirs(sceneDirs) { - if(imageSourceEngines.empty()) + // If local scenes were specified when the pipeline was instantiated, we add a SLAM component for each such scene. + /* for(size_t i = 0, size = imageSourceEngines.size(); i < size; ++i) { - // The current implementation of spaintgui relies on the existence of a scene called "World". - // If no local scenes were specified when the pipeline was instantiated, then we check to - // see if a mapping server exists: - if(mappingServer) - { - // If it does, we wait for the first remote client to join instead, and call its scene "World" when it - // connects. The creation of the SLAM component for the remote client will block until it does so. - add_remote_slam_component(Model::get_world_scene_id(), 0); - } - else - { - // Otherwise, no scene called "World" will ever exist, so we throw. - throw std::runtime_error("Error: Cannot run a collaborative pipeline without a scene called 'World' (did you mean to specify --runServer?)"); - } - } - else + const std::string sceneID = i == 0 ? Model::get_world_scene_id() : "Local" + boost::lexical_cast(i); + m_slamComponents[sceneID].reset(new SLAMComponent(m_model, + sceneID, + imageSourceEngines[i], + trackerConfigs[i], + mappingModes[i], + trackingModes[i], + detectFiducials)); + }*/ + std::map trackingControllers; + for(size_t i = 0, size = imageSourceEngines.size(); i < size; ++i) { - // If local scenes were specified when the pipeline was instantiated, we add a SLAM component for each such scene. - for(size_t i = 0, size = imageSourceEngines.size(); i < size; ++i) - { - const std::string sceneID = i == 0 ? Model::get_world_scene_id() : "Local" + boost::lexical_cast(i); - m_slamComponents[sceneID].reset(new SLAMComponent(m_model, sceneID, imageSourceEngines[i], trackerConfigs[i], mappingModes[i], trackingModes[i], detectFiducials)); - } + const std::string sceneID = i == 0 ? Model::get_world_scene_id() : "Local" + boost::lexical_cast(i); + m_slamComponents[sceneID].reset(new SLAMComponent( + m_model, sceneID, imageSourceEngines[i], trackerConfigs[i], mappingModes[i], trackingModes[i], detectFiducials)); + load_models(m_slamComponents[sceneID], m_sceneDirs[sceneID]); + trackingControllers[sceneID] = m_slamComponents[sceneID]->get_tracking_controller(); } - // 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)); + m_collaborativeComponent.reset(new CollaborativeComponent(m_model, collaborationMode, scenesPoseCnt, trackingControllers, sceneID2Name)); } //#################### PUBLIC MEMBER FUNCTIONS #################### @@ -79,6 +61,7 @@ CollaborativePipeline::CollaborativePipeline(const Settings_Ptr& settings, const std::set CollaborativePipeline::run_main_section() { // If we're running a mapping server, add SLAM components for any newly-connected remote clients. + /* if(m_model->get_mapping_server()) check_for_new_clients(); // Run the main section of the pipeline. @@ -87,8 +70,17 @@ std::set CollaborativePipeline::run_main_section() // Provided at least one of the scenes has started fusion, run the collaborative pose estimation process. m_collaborationStarted = m_collaborationStarted || !scenesProcessed.empty(); if(m_collaborationStarted) m_collaborativeComponent->run_collaborative_pose_estimation(); - - return scenesProcessed; + */ + std::set result; + for(std::map::const_iterator it = m_slamComponents.begin(), iend = m_slamComponents.end(); it != iend; ++it) + { + bool isConsistent = m_collaborativeComponent->run_collaborative_pose_estimation(); + if (!isConsistent) + { + result.insert(it->first); + } + } + return result; } void CollaborativePipeline::set_mode(Mode mode) @@ -99,17 +91,20 @@ void CollaborativePipeline::set_mode(Mode mode) //#################### PRIVATE MEMBER FUNCTIONS #################### -void CollaborativePipeline::add_remote_slam_component(const std::string& sceneID, int remoteClientID) +void CollaborativePipeline::add_remote_slam_component(const std::string &sceneID, int remoteClientID) { - std::cout << "Instantiating a SLAM component for client '" << remoteClientID << "', with local scene ID '" << sceneID << "'" << std::endl; + std::cout << "Instantiating a SLAM component for client '" << remoteClientID << "', with local scene ID '" << sceneID + << "'" << std::endl; // Note: We create remote clients that are voxel-only and have no support for fiducials. - const MappingServer_Ptr& mappingServer = m_model->get_mapping_server(); + const MappingServer_Ptr &mappingServer = m_model->get_mapping_server(); const ImageSourceEngine_Ptr imageSourceEngine(new RemoteImageSourceEngine(mappingServer, remoteClientID)); - const std::string trackerConfig = "" + boost::lexical_cast(remoteClientID) + ""; + const std::string trackerConfig = + "" + boost::lexical_cast(remoteClientID) + ""; const SLAMComponent::MappingMode mappingMode = SLAMComponent::MAP_VOXELS_ONLY; const SLAMComponent::TrackingMode trackingMode = SLAMComponent::TRACK_VOXELS; - m_slamComponents[sceneID].reset(new SLAMComponent(m_model, sceneID, imageSourceEngine, trackerConfig, mappingMode, trackingMode, m_detectFiducials)); + m_slamComponents[sceneID].reset(new SLAMComponent( + m_model, sceneID, imageSourceEngine, trackerConfig, mappingMode, trackingMode, m_detectFiducials)); mappingServer->set_scene_id(remoteClientID, sceneID); } @@ -122,7 +117,9 @@ void CollaborativePipeline::check_for_new_clients() const int clientID = activeClients[i]; // Determine what the scene for this client should be called. - const std::string sceneID = m_worldIsRemote && clientID == 0 ? Model::get_world_scene_id() : "Remote" + boost::lexical_cast(clientID); + const std::string sceneID = m_worldIsRemote && clientID == 0 + ? Model::get_world_scene_id() + : "Remote" + boost::lexical_cast(clientID); // If a SLAM component for this client does not yet exist, add one now. if(m_slamComponents.find(sceneID) == m_slamComponents.end()) diff --git a/apps/spaintgui/core/CollaborativePipeline.h b/apps/spaintgui/core/CollaborativePipeline.h index 8ffeb62..c02fe4f 100644 --- a/apps/spaintgui/core/CollaborativePipeline.h +++ b/apps/spaintgui/core/CollaborativePipeline.h @@ -8,6 +8,7 @@ #include #include +#include #include "MultiScenePipeline.h" @@ -18,6 +19,9 @@ class CollaborativePipeline : public MultiScenePipeline { //#################### PRIVATE VARIABLES #################### private: + /** A map for scene model dir and scene id */ + std::map m_sceneDirs; + /** A flag indicating whether or not we should start running collaborative pose estimation. */ bool m_collaborationStarted; @@ -32,13 +36,17 @@ class CollaborativePipeline : public MultiScenePipeline //#################### CONSTRUCTORS #################### public: - CollaborativePipeline(const Settings_Ptr& settings, const std::string& resourcesDir, + CollaborativePipeline(const Settings_Ptr& settings, + const std::string& resourcesDir, + const std::map& scenesPoseCnt, + const std::map &sceneID2Name, const std::vector& imageSourceEngines, const std::vector& trackerConfigs, const std::vector& mappingModes, const std::vector& trackingModes, bool detectFiducials = false, const itmx::MappingServer_Ptr& mappingServer = itmx::MappingServer_Ptr(), - spaint::CollaborationMode collaborationMode = spaint::CM_LIVE); + spaint::CollaborationMode collaborationMode = spaint::CM_LIVE, + const std::map &sceneDirs = std::map()); //#################### PUBLIC MEMBER FUNCTIONS #################### public: diff --git a/apps/spaintgui/core/MultiScenePipeline.cpp b/apps/spaintgui/core/MultiScenePipeline.cpp index 46a2611..85951fe 100644 --- a/apps/spaintgui/core/MultiScenePipeline.cpp +++ b/apps/spaintgui/core/MultiScenePipeline.cpp @@ -79,7 +79,8 @@ std::set MultiScenePipeline::run_main_section() std::set result; for(std::map::const_iterator it = m_slamComponents.begin(), iend = m_slamComponents.end(); it != iend; ++it) { - if(it->second->process_frame()) result.insert(it->first); + if(it->second->process_frame()) result.insert(it->first); + // if(it->second->process_frame_pose()) result.insert(it->first); } return result; } diff --git a/apps/spaintgui/main.cpp b/apps/spaintgui/main.cpp index 3ffd613..c1fbd0c 100644 --- a/apps/spaintgui/main.cpp +++ b/apps/spaintgui/main.cpp @@ -6,25 +6,28 @@ #include #include #include +#include +#include #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 +60,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 +84,7 @@ struct CommandLineArguments //~~~~~~~~~~~~~~~~~~~~ PUBLIC VARIABLES ~~~~~~~~~~~~~~~~~~~~ // User-specifiable arguments + std::string scene1, scene2; bool batch; std::string calibrationFilename; bool cameraAfterDisk; @@ -133,7 +137,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 +145,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 +204,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 +235,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 +262,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 +292,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 +315,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 +323,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 +348,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 +366,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 +396,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 +405,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 +451,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 +470,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); @@ -455,18 +490,42 @@ boost::shared_ptr make_image_source_engine(const Com return imageSourceEngine; } +std::vector make_image_source_engines(const CommandLineArguments &args) +{ + std::vector ImageSourceEngines; + if(args.scene1!="" && args.scene2!="") + { + boost::filesystem::path current_path = boost::filesystem::current_path(); + + CompositeImageSourceEngine_Ptr imageSourceEngine1(new CompositeImageSourceEngine); + boost::filesystem::path calibrationPath = args.sequences[0]->default_calib_path(); + const std::string calibrationFilename1 = calibrationPath.string(); + std::cout << "this is calibrationFilename1: " << calibrationFilename1 << "\n"; + imageSourceEngine1->addSubengine(new IdleImageSourceEngine(calibrationFilename1.c_str())); + ImageSourceEngines.push_back(imageSourceEngine1); + + CompositeImageSourceEngine_Ptr imageSourceEngine2(new CompositeImageSourceEngine); + calibrationPath = args.sequences[1]->default_calib_path(); + const std::string calibrationFilename2 = calibrationPath.string(); + imageSourceEngine2->addSubengine(new IdleImageSourceEngine(calibrationFilename2.c_str())); + ImageSourceEngines.push_back(imageSourceEngine2); + } + return ImageSourceEngines; +} + /** - * \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 +537,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 +566,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 +615,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 +638,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 +664,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 +689,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 +736,82 @@ 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")( + "scene1", po::value(&args.scene1), "the one scene prepare to relocalise")( + "scene2", po::value(&args.scene2), "the other scene prepare to relocalise")( + "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)->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 +856,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(); @@ -795,15 +888,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 +920,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,18 +935,20 @@ 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") + /*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; + SLAMComponent::TrackingMode trackingMode = args.trackSurfels ? SLAMComponent::TRACK_SURFELS : + SLAMComponent::TRACK_VOXELS; if(args.pipelineType == "slam") { @@ -867,132 +963,128 @@ try 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")) { - // 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; - } + 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; + // Set up the image source engines, mapping modes, tracking modes and tracker configurations. + std::vector imageSourceEngines = make_image_source_engines(args); + 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; + // 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)); + 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); - } + 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()); - } + // 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); - // 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 - )); + // 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()); } - // If a remote host was specified, set up a mapping client for the world scene. - if(args.host != "") + // Construct the pipeline itself. + // const CollaborationMode collaborationMode = args.collaborationMode == "batch" ? CM_BATCH : CM_LIVE; + // set batch mode as default + const CollaborationMode collaborationMode = CM_BATCH; + std::vector sceneIDs; + sceneIDs.push_back("World"); + sceneIDs.push_back("Local1"); + + std::map sceneDirs; + boost::filesystem::path current_path = boost::filesystem::current_path(); + sceneDirs.insert(std::pair(sceneIDs[0], (current_path / args.scene1 / "model").string())); + sceneDirs.insert(std::pair(sceneIDs[1], (current_path / args.scene2 / "model").string())); + + // only two scene to relocalise + std::map scenesPoseCnt; + for(size_t i = 0, size = args.sequenceSpecifiers.size(); i < size; ++i) { - 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 std::string &sequenceSpecifier = args.sequenceSpecifiers[i]; + bf::path dir = sequenceSpecifier; + bf::directory_iterator endIter; + int count = 0; + for (bf::directory_iterator iter(dir); iter!=endIter; iter++) + { + count++; + } + scenesPoseCnt.insert(std::pair(sceneIDs[i], count-1)); } + std::map sceneID2Name; + sceneID2Name.insert(std::pair("World", args.scene1)); + sceneID2Name.insert(std::pair("Local1", args.scene2)); + + pipeline.reset(new CollaborativePipeline(settings, + Application::resources_dir().string(), + scenesPoseCnt, + sceneID2Name, + imageSourceEngines, + trackerConfigs, + mappingModes, + trackingModes, + args.detectFiducials, + mappingServer, + collaborationMode, + sceneDirs)); + // } + #ifdef WITH_LEAP - // Set the ID of the fiducial to use for the Leap Motion (if any). - pipeline->get_model()->set_leap_fiducial_id(args.leapFiducialID); + // Set the ID of the fiducial to use for the Leap Motion (if any). + pipeline->get_model()->set_leap_fiducial_id(args.leapFiducialID); #endif - // Configure and run the application. - Application app(pipeline, args.renderFiducials); - 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); - app.set_save_mesh_on_exit(args.saveMeshOnExit); - app.set_save_models_on_exit(args.saveModelsOnExit); - bool runSucceeded = app.run(); + // Configure and run the application. + Application app(pipeline, sceneID2Name, args.renderFiducials); + 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); + app.set_save_mesh_on_exit(args.saveMeshOnExit); + app.set_save_models_on_exit(args.saveModelsOnExit); + bool runSucceeded = app.run(); - // Close all open joysticks. - joysticks.clear(); + // Close all open joysticks. + joysticks.clear(); - // If we're not running in headless mode, shut down the GUI-only subsystems. - if(!args.headless) - { - #ifdef WITH_OVR - // If we built with Rift support, shut down the Rift SDK. - ovr_Shutdown(); - #endif + // If we're not running in headless mode, shut down the GUI-only subsystems. + if(!args.headless) + { + #ifdef WITH_OVR + // If we built with Rift support, shut down the Rift SDK. + ovr_Shutdown(); + #endif - // Shut down SDL. - SDL_Quit(); - } + // Shut down SDL. + SDL_Quit(); + } - return runSucceeded ? EXIT_SUCCESS : EXIT_FAILURE; + 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/apps/spaintgui/sequences/SpaintSequence.cpp b/apps/spaintgui/sequences/SpaintSequence.cpp index aeefd44..05dd086 100644 --- a/apps/spaintgui/sequences/SpaintSequence.cpp +++ b/apps/spaintgui/sequences/SpaintSequence.cpp @@ -21,9 +21,9 @@ SpaintSequence::SpaintSequence(const bf::path& dir, size_t initialFrameNumber, d : Sequence(initialFrameNumber), m_depthNoiseSigma(depthNoiseSigma), m_dir(dir), m_missingDepthFraction(missingDepthFraction) { // Try to figure out the format of the sequence stored in the directory (we only check the depth images, since the colour ones might be missing). + /* const bool sevenScenesNaming = bf::is_regular_file(dir / "frame-000000.depth.png"); const bool spaintNaming = bf::is_regular_file(dir / "depthm000000.pgm"); - // Determine the depth/RGB/pose masks. if(sevenScenesNaming && spaintNaming) { @@ -45,8 +45,14 @@ SpaintSequence::SpaintSequence(const bf::path& dir, size_t initialFrameNumber, d } else { - throw std::runtime_error("Error: The directory '" + dir.string() + "' does not contain depth images that follow a known naming convention."); - } + */ + // throw std::runtime_error("Error: The directory '" + dir.string() + "' does not contain depth images that follow a known naming convention."); + //only need pose file + m_depthImageMask = ""; + m_poseFileMask = (dir / "frame-%06i.pose.txt").string(); + m_rgbImageMask = ""; + m_semanticImageMask = ""; + //} } SpaintSequence::SpaintSequence(const std::string& depthImageMask, const std::string& rgbImageMask, const std::string& poseFileMask, const std::string& semanticImageMask, diff --git a/libraries/build-opencv-3.1.0-nix.sh b/libraries/build-opencv-3.1.0-nix.sh index 6373450..a37638b 100755 --- a/libraries/build-opencv-3.1.0-nix.sh +++ b/libraries/build-opencv-3.1.0-nix.sh @@ -50,7 +50,7 @@ else CMAKE_EXE_LINKER_FLAGS="" fi - cmake -Wno-dev -DENABLE_PRECOMPILED_HEADERS=OFF -DCMAKE_INSTALL_PREFIX=$INSTALL_PREFIX -DOPENCV_EXTRA_MODULES_PATH=../../opencv_contrib-3.1.0/modules -DBUILD_DOCS=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DWITH_CUDA=OFF -DCMAKE_CXX_FLAGS=$CMAKE_CXX_FLAGS -DCMAKE_EXE_LINKER_FLAGS=$CMAKE_EXE_LINKER_FLAGS -DBUILD_opencv_bgsegm=OFF -DBUILD_opencv_bioinspired=OFF -DBUILD_opencv_ccalib=OFF -DBUILD_opencv_datasets=OFF -DBUILD_opencv_dnn=OFF -DBUILD_opencv_dpm=OFF -DBUILD_opencv_face=OFF -DBUILD_opencv_fuzzy=OFF -DBUILD_opencv_hdf=OFF -DBUILD_opencv_java=OFF -DBUILD_opencv_line_descriptor=OFF -DBUILD_opencv_matlab=OFF -DBUILD_opencv_optflow=OFF -DBUILD_opencv_plot=OFF -DBUILD_opencv_python=OFF -DBUILD_opencv_python2=OFF -DBUILD_opencv_python3=OFF -DBUILD_opencv_reg=OFF -DBUILD_opencv_rgbd=OFF -DBUILD_opencv_saliency=OFF -DBUILD_opencv_stereo=OFF -DBUILD_opencv_structured_light=OFF -DBUILD_opencv_surface_matching=OFF -DBUILD_opencv_text=OFF -DBUILD_opencv_tracking=OFF -DBUILD_opencv_xfeatures2d=OFF -DBUILD_opencv_ximgproc=OFF -DBUILD_opencv_xobjdetect=OFF -DBUILD_opencv_xphoto=OFF .. # > $LOG 2>&1 + cmake -Wno-dev -DENABLE_PRECOMPILED_HEADERS=OFF -DCMAKE_INSTALL_PREFIX=$INSTALL_PREFIX -DOPENCV_EXTRA_MODULES_PATH=../../opencv_contrib-3.1.0/modules -DBUILD_DOCS=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DWITH_CUDA=OFF -DCMAKE_CXX_FLAGS=$CMAKE_CXX_FLAGS -DCMAKE_EXE_LINKER_FLAGS=$CMAKE_EXE_LINKER_FLAGS -DBUILD_opencv_bgsegm=OFF -DBUILD_opencv_bioinspired=OFF -DBUILD_opencv_ccalib=OFF -DBUILD_opencv_datasets=OFF -DBUILD_opencv_dnn=OFF -DBUILD_opencv_dpm=OFF -DBUILD_opencv_face=OFF -DBUILD_opencv_fuzzy=OFF -DBUILD_opencv_hdf=OFF -DBUILD_opencv_java=OFF -DBUILD_opencv_line_descriptor=OFF -DBUILD_opencv_matlab=OFF -DBUILD_opencv_optflow=OFF -DBUILD_opencv_plot=OFF -DBUILD_opencv_python=OFF -DBUILD_opencv_python2=OFF -DBUILD_opencv_python3=OFF -DBUILD_opencv_reg=OFF -DBUILD_opencv_rgbd=OFF -DBUILD_opencv_saliency=OFF -DBUILD_opencv_stereo=OFF -DBUILD_opencv_structured_light=OFF -DBUILD_opencv_surface_matching=ON -DBUILD_opencv_text=OFF -DBUILD_opencv_tracking=OFF -DBUILD_opencv_xfeatures2d=ON -DBUILD_opencv_ximgproc=ON -DBUILD_opencv_xobjdetect=OFF -DBUILD_opencv_xphoto=OFF .. # > $LOG 2>&1 echo "[spaint] ...Running build..." make -j96 # >> $LOG 2>&1 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/collaboration/CollaborativePoseOptimiser.h b/modules/spaint/include/spaint/collaboration/CollaborativePoseOptimiser.h index 314db90..186bb19 100644 --- a/modules/spaint/include/spaint/collaboration/CollaborativePoseOptimiser.h +++ b/modules/spaint/include/spaint/collaboration/CollaborativePoseOptimiser.h @@ -7,6 +7,7 @@ #define H_SPAINT_COLLABORATIVEPOSEOPTIMISER #include +#include #include #include @@ -30,6 +31,9 @@ class CollaborativePoseOptimiser //#################### PRIVATE VARIABLES #################### private: + /** The map of the scene ID and Scene Name*/ + std::map m_sceneID2Name; + /** Estimates of the poses of the different scenes in the global coordinate system. */ std::map m_estimatedGlobalPoses; @@ -90,6 +94,13 @@ class CollaborativePoseOptimiser //#################### PUBLIC MEMBER FUNCTIONS #################### public: + /** + * \brief set map for SceneID and its name + * + * \param sceneID2Name The map to set. + */ + void set_sceneID2Name(const std::map& sceneID2Name); + /** * \brief Adds a sample of the transformation from the coordinate system of scene j to that of scene i. * @@ -115,6 +126,11 @@ class CollaborativePoseOptimiser */ void terminate(); + /** + * \brief Terminates the pose graph optimiser. + */ + bool isSuccess() const; + /** * \brief Attempts to get the estimated global pose (if any) of the specified scene. * diff --git a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h index 407029a..15b4bd4 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 = 1; +const int bestCandidateMaxCount = 1; + /** * \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; @@ -46,6 +54,9 @@ class CollaborativeComponent /** The current frame index (in practice, the number of times that run_collaborative_pose_estimation has been called). */ int m_frameIndex; + /** The number of times that run_collaborative_pose_estimation has been called. */ + int m_collaborativeCnt; + /** The mode in which the collaboration reconstruction should run. */ CollaborationMode m_mode; @@ -61,6 +72,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; @@ -79,6 +92,9 @@ class CollaborativeComponent /** Whether or not to compute the time spent collaborating. */ bool m_timeCollaboration; + /** Tracking controller to update pose */ + std::map m_trackingControllers; + /** The trajectories followed by the cameras that reconstructed each of the different scenes (only poses where tracking succeeded are stored). */ std::map > m_trajectories; @@ -96,7 +112,10 @@ class CollaborativeComponent * \param context The shared context needed for collaborative SLAM. * \param mode The mode in which the collaborative reconstruction should run. */ - CollaborativeComponent(const CollaborativeContext_Ptr& context, CollaborationMode mode); + CollaborativeComponent(const CollaborativeContext_Ptr& context, CollaborationMode mode, + const std::map &scenesPoseCnt, + const std::map &trackingControllers, + const std::map &sceneID2Name); //#################### DESTRUCTOR #################### public: @@ -110,7 +129,7 @@ class CollaborativeComponent /** * \brief Attempts to estimate the poses of the different scenes involved in the collaborative reconstruction. */ - void run_collaborative_pose_estimation(); + bool run_collaborative_pose_estimation(); //#################### PRIVATE MEMBER FUNCTIONS #################### private: @@ -148,7 +167,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/include/spaint/pipelinecomponents/CollaborativeContext.h b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeContext.h index 2d3ba26..69f6ca4 100644 --- a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeContext.h +++ b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeContext.h @@ -36,6 +36,7 @@ class CollaborativeContext virtual orx::Relocaliser_CPtr get_relocaliser(const std::string& sceneID) const = 0; virtual const std::vector& get_scene_ids() const = 0; virtual const Settings_CPtr& get_settings() const = 0; + virtual const SLAMState_Ptr& get_slam_state(const std::string& sceneID) = 0; virtual SLAMState_CPtr get_slam_state(const std::string& sceneID) const = 0; //#################### PUBLIC MEMBER FUNCTIONS #################### diff --git a/modules/spaint/include/spaint/pipelinecomponents/SLAMComponent.h b/modules/spaint/include/spaint/pipelinecomponents/SLAMComponent.h index 81a2dae..c92cc5d 100644 --- a/modules/spaint/include/spaint/pipelinecomponents/SLAMComponent.h +++ b/modules/spaint/include/spaint/pipelinecomponents/SLAMComponent.h @@ -58,6 +58,9 @@ class SLAMComponent /** The shared context needed for SLAM. */ SLAMContext_Ptr m_context; + /** Pose count for tracker*/ + int m_poseCnt; + /** The dense surfel mapper. */ DenseSurfelMapper_Ptr m_denseSurfelMapper; @@ -150,7 +153,8 @@ class SLAMComponent * \param trackingMode The tracking mode to use. * \param detectFiducials Whether or not to initially detect fiducials in the scene. */ - SLAMComponent(const SLAMContext_Ptr& context, const std::string& sceneID, const ImageSourceEngine_Ptr& imageSourceEngine, const std::string& trackerConfig, + SLAMComponent(const SLAMContext_Ptr& context, const std::string& sceneID, + const ImageSourceEngine_Ptr& imageSourceEngine, const std::string& trackerConfig, MappingMode mappingMode = MAP_VOXELS_ONLY, TrackingMode trackingMode = TRACK_VOXELS, bool detectFiducials = false); //#################### PUBLIC MEMBER FUNCTIONS #################### @@ -169,6 +173,13 @@ class SLAMComponent */ const std::string& get_scene_id() const; + /** + * \brief Gets the ID of the scene being reconstructed by this SLAM component. + * + * \return The ID of the scene being reconstructed by this SLAM component. + */ + TrackingController_Ptr& get_tracking_controller(); + /** * \brief Replaces the SLAM component's voxel (and surfel model, if available) with ones loaded from the specified directory on disk. * @@ -194,6 +205,13 @@ class SLAMComponent */ bool process_frame(); + /** + * \brief Attempts to run the SLAM component for a single frame pose. + * + * \return true, if a frame was processed, or false otherwise. + */ + bool process_frame_pose(); + /** * \brief Resets the reconstructed scene. */ diff --git a/modules/spaint/include/spaint/slamstate/SLAMState.h b/modules/spaint/include/spaint/slamstate/SLAMState.h index 131a298..e270598 100644 --- a/modules/spaint/include/spaint/slamstate/SLAMState.h +++ b/modules/spaint/include/spaint/slamstate/SLAMState.h @@ -66,6 +66,9 @@ class SLAMState /** The current reconstructed surfel scene. */ SpaintSurfelScene_Ptr m_surfelScene; + /** The tracking controller. */ + TrackingController_Ptr m_trackingController; + /** The current tracking state (containing the camera pose and additional tracking information used by InfiniTAM). */ TrackingState_Ptr m_trackingState; @@ -287,6 +290,13 @@ class SLAMState */ void set_surfel_scene(const SpaintSurfelScene_Ptr& surfelScene); + /** + * \brief Sets the current tracking controller. + * + * \param trackingState The new current tracking controller. + */ + void set_tracking_controller(const TrackingController_Ptr& trackingController); + /** * \brief Sets the current tracking state. * diff --git a/modules/spaint/src/collaboration/CollaborativePoseOptimiser.cpp b/modules/spaint/src/collaboration/CollaborativePoseOptimiser.cpp index 9c2727f..f37193f 100644 --- a/modules/spaint/src/collaboration/CollaborativePoseOptimiser.cpp +++ b/modules/spaint/src/collaboration/CollaborativePoseOptimiser.cpp @@ -91,6 +91,11 @@ void CollaborativePoseOptimiser::add_relative_transform_sample(const std::string #endif } +void CollaborativePoseOptimiser::set_sceneID2Name(const std::map& sceneID2Name) +{ + m_sceneID2Name = sceneID2Name; +} + void CollaborativePoseOptimiser::start(const std::string& globalPosesSpecifier) { m_globalPosesSpecifier = globalPosesSpecifier; @@ -111,6 +116,12 @@ void CollaborativePoseOptimiser::terminate() save_global_poses(); } +bool CollaborativePoseOptimiser::isSuccess() const +{ + if(m_estimatedGlobalPoses.empty()) return false; + return true; +} + boost::optional CollaborativePoseOptimiser::try_get_estimated_global_pose(const std::string& sceneID) const { boost::lock_guard lock(m_mutex); @@ -399,7 +410,9 @@ void CollaborativePoseOptimiser::save_global_poses() const // Determine the file to which to save the poses. const std::string dirName = "global_poses"; const std::string globalPosesSpecifier = m_globalPosesSpecifier != "" ? m_globalPosesSpecifier : TimeUtil::get_iso_timestamp(); - const bf::path p = find_subdir_from_executable(dirName) / (globalPosesSpecifier + ".txt"); + // const bf::path p = find_subdir_from_executable(dirName) / (globalPosesSpecifier + ".txt"); + const bf::path p("global_poses/" + globalPosesSpecifier + ".txt"); + std::cout << "this is global pose file name: " << p << "\n"; // Try to ensure that the directory into which we want to save the file exists. If we can't, early out. try @@ -422,8 +435,9 @@ void CollaborativePoseOptimiser::save_global_poses() const for(std::map::const_iterator it = m_estimatedGlobalPoses.begin(), iend = m_estimatedGlobalPoses.end(); it != iend; ++it) { + auto sceneName = m_sceneID2Name.find(it->first); DualQuatd dq = GeometryUtil::pose_to_dual_quat(it->second); - fs << it->first << ' ' << dq << '\n'; + fs << sceneName->second << ' ' << dq << '\n'; } } diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index 7676af3..e0d13ae 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -8,8 +8,22 @@ using namespace ITMLib; using namespace ORUtils; using namespace itmx; +#include +#include +#include + #include +#include +#include +#include +#include +#include + +using namespace std; +using namespace cv; +using namespace cv::xfeatures2d; + #include using boost::bind; @@ -21,15 +35,57 @@ 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 #################### -CollaborativeComponent::CollaborativeComponent(const CollaborativeContext_Ptr& context, CollaborationMode mode) +CollaborativeComponent::CollaborativeComponent(const CollaborativeContext_Ptr& context, CollaborationMode mode, + const std::map &scenesPoseCnt, + const std::map &trackingControllers, + const std::map &sceneID2Name) : m_context(context), + m_trackingControllers(trackingControllers), m_frameIndex(0), + m_collaborativeCnt(0), m_mode(mode), m_reconstructionIsConsistent(false), m_rng(12345), @@ -40,12 +96,42 @@ CollaborativeComponent::CollaborativeComponent(const CollaborativeContext_Ptr& c const std::string settingsNamespace = "CollaborativeComponent."; m_considerPoorRelocalisations = settings->get_first_value(settingsNamespace + "considerPoorRelocalisations", mode == CM_LIVE); 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)); + // m_timeCollaboration = settings->get_first_value(settingsNamespace + "timeCollaboration", false); + m_timeCollaboration = true; + + int cpuCnt = sysconf(_SC_NPROCESSORS_CONF); + int average = cpuCnt/relocalisationThreadsCount; + for (int i=0; iget_slam_state(sceneID); + const View_Ptr &view = slamState->get_view(); + const TrackingState_Ptr &trackingState = slamState->get_tracking_state(); + + TrackingController_Ptr tracker = m_trackingControllers[sceneID]; + for (size_t i = 0, poseSize = it.second; iTrack(trackingState.get(), view.get()); + m_trajectories[sceneID].push_back(*trackingState->pose_d); + } + } - const std::string globalPosesSpecifier = settings->get_first_value("globalPosesSpecifier", ""); - m_context->get_collaborative_pose_optimiser()->start(globalPosesSpecifier); + m_context->get_collaborative_pose_optimiser()->set_sceneID2Name(sceneID2Name); + std::string scene1 = sceneID2Name.find("World")->second; + std::string scene2 = sceneID2Name.find("Local1")->second; + // const std::string globalPosesSpecifier = settings->get_first_value("globalPosesSpecifier", ""); + m_context->get_collaborative_pose_optimiser()->start(scene1 + "-" + scene2); } //#################### DESTRUCTOR #################### @@ -53,8 +139,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 sceneIDs = m_context->get_scene_ids(); + m_reconstructionIsConsistent = true; + for(size_t sceneIdx = 0; sceneIdx < sceneIDs.size(); ++sceneIdx) + { + if(!m_context->get_collaborative_pose_optimiser()->try_get_estimated_global_pose(sceneIDs[sceneIdx])) + { + m_reconstructionIsConsistent = false; + break; + } + } + + if(m_reconstructionIsConsistent) std::cout << "The reconstruction became consistent at frame: " << m_frameIndex << '\n'; + } + // If the reconstruction is consistent and we're stopping at the first consistent reconstruction: + if(m_reconstructionIsConsistent && m_stopAtFirstConsistentReconstruction) + { + // Stop the collaboration timer if necessary. + if(m_collaborationTimer) m_collaborationTimer->stop(); + // Early out to prevent any more relocalisation attempts being scheduled. + return m_reconstructionIsConsistent; + } + + // Otherwise, try to schedule a relocalisation attempt. + if (m_bestCandidates.size() < bestCandidateMaxCount) { + try_schedule_relocalisation(); + } + + ++m_frameIndex; + + return m_reconstructionIsConsistent; + /* bool fusionMayStillRun = update_trajectories(); if(!fusionMayStillRun) m_mode = CM_BATCH; if(m_frameIndex > 0 && (!fusionMayStillRun || (m_mode == CM_LIVE && m_frameIndex % 50 == 0))) @@ -122,6 +253,7 @@ void CollaborativeComponent::run_collaborative_pose_estimation() #if defined(WITH_OPENCV) && 0 cv::waitKey(1); #endif + */ } //#################### PRIVATE MEMBER FUNCTIONS #################### @@ -214,11 +346,17 @@ bool CollaborativeComponent::is_verified(const CollaborativeRelocalisation& cand const float depthDiffThreshold = m_mode == CM_LIVE ? 10.0f : 5.0f; return candidate.m_meanDepthDiff(0) < depthDiffThreshold && candidate.m_targetValidFraction >= 0.5f; + // return candidate.m_meanDepthDiff(0) < 2.5f && candidate.m_targetValidFraction >= 0.75f; #else return true; #endif } +bool verifying(cv::Scalar meanDepthDiff, float targetValidFraction) +{ + return meanDepthDiff(0) < 5.0f && targetValidFraction >= 0.5f; +} + void CollaborativeComponent::output_results() const { int good = 0, bad = 0, unknown = 0; @@ -355,42 +493,72 @@ void CollaborativeComponent::output_results() const } } -void CollaborativeComponent::run_relocalisation() +double getPSNR(const cv::Mat& depthImage1, const cv::Mat& depthImage2) { + cv::Mat diff; + cv::absdiff(depthImage1, depthImage2, diff); + diff = diff.mul(diff); + double mse = cv::mean(diff)[0]; + + double max_pixel_value = 255.0; + double psnr = 10.0 * std::log10((max_pixel_value * max_pixel_value) / mse); + return psnr; +} + +// void CollaborativeComponent::run_relocalisation() +void CollaborativeComponent::run_relocalisation(cpu_set_t mask) +{ + auto tid = syscall(SYS_gettid); + std::cout << tid << "\n"; + + if (sched_setaffinity(tid, sizeof(mask), &mask) < 0) { + std::cout << "set thread affinity failed\n"; + }else { + std::cout << "set " << tid << " to cpu set \n"; + } + int run_times = 0; + int save_cnt1 = 0; + int save_cnt2 = 0; + int save_cnt3 = 0; while(!m_stopRelocalisationThread) { // Wait for a relocalisation to be scheduled. { boost::unique_lock lock(m_mutex); - while(!m_bestCandidate) + // while(!m_bestCandidate) + while (m_bestCandidates.empty()) { m_readyToRelocalise.wait(lock); - // If the collaborative component is terminating, stop attempting relocalisations and let the thread terminate. 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(); + run_times++; + auto now_bestCandidate = m_bestCandidates.front(); + m_bestCandidates.pop_front(); + m_mutex.unlock(); + std::cout << tid <<" : " << run_times << " 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 ); @@ -401,7 +569,7 @@ void CollaborativeComponent::run_relocalisation() // Make OpenCV copies of the synthetic images we're trying to relocalise (these may be needed later). cv::Mat3b cvSourceRGB = OpenCVUtil::make_rgb_image(rgb->GetData(MEMORYDEVICE_CPU), rgb->noDims.x, rgb->noDims.y); cv::Mat1b cvSourceDepth = OpenCVUtil::make_greyscale_image(depth->GetData(MEMORYDEVICE_CPU), depth->noDims.x, depth->noDims.y, OpenCVUtil::ROW_MAJOR, 100.0f); - + // cv::imwrite("source.png", cvSourceRGB); #if DEBUGGING // If we're debugging, show the synthetic images of the source scene to the user. cv::imshow("Source Depth", cvSourceDepth); @@ -410,12 +578,12 @@ 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::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. @@ -437,13 +605,14 @@ void CollaborativeComponent::run_relocalisation() // Make OpenCV copies of the synthetic images of the target scene. cv::Mat3b cvTargetRGB = OpenCVUtil::make_rgb_image(rgb->GetData(MEMORYDEVICE_CPU), rgb->noDims.x, rgb->noDims.y); cv::Mat1b cvTargetDepth = OpenCVUtil::make_greyscale_image(depth->GetData(MEMORYDEVICE_CPU), depth->noDims.x, depth->noDims.y, OpenCVUtil::ROW_MAJOR, 100.0f); + // cv::imwrite("target.png", cvTargetRGB); + // std::cout << "save target png\n"; #if DEBUGGING // If we're debugging, show the synthetic images of the target scene to the user. cv::imshow("Target RGB", cvTargetRGB); cv::imshow("Target Depth", cvTargetDepth); #endif - // Compute a binary mask showing which pixels are valid in both the source and target depth images. cv::Mat cvSourceMask; cv::inRange(cvSourceDepth, cv::Scalar(0,0,0), cv::Scalar(0,0,0), cvSourceMask); @@ -465,19 +634,30 @@ 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); + /* + if (verified) { + printf("save cnt: %d\n", save_cnt1); + cv::imwrite("verifiedSource"+std::to_string(now_bestCandidate->m_frameIndexJ)+".png", cvSourceRGB); + cv::imwrite("verifiedTarget"+std::to_string(now_bestCandidate->m_frameIndexJ)+".png", cvTargetRGB); + cv::imwrite("verifiedSourcedep"+std::to_string(now_bestCandidate->m_frameIndexJ)+".png", cvSourceDepth); + cv::imwrite("verifiedTargetdep"+std::to_string(now_bestCandidate->m_frameIndexJ)+".png", cvTargetDepth); + save_cnt1++; + } + */ #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; @@ -488,11 +668,145 @@ void CollaborativeComponent::run_relocalisation() // relative transform between the source and target scenes to the pose graph optimiser. if(verified) { + now_bestCandidate->m_relativePose = ORUtils::SE3Pose(result->pose.GetInvM() * now_bestCandidate->m_localPoseJ.GetM()); + std::string sceneJ = now_bestCandidate->m_sceneJ; + int sceneJFrameSize = m_trajectories[sceneJ].size(); + int tmpIndexJ = now_bestCandidate->m_frameIndexJ; + double similarity = 0.0; + int count = 0; + int successTimes = 0; + for (int i = 1; i<=3; i++) { + // use rgb render picture to calculate similarity + int checkNextFrameIndex = now_bestCandidate->m_frameIndexJ + i*10; + if (checkNextFrameIndex < sceneJFrameSize) { + printf("this is another frame: %d\n", checkNextFrameIndex); + count++; + ORUtils::SE3Pose nextFrameLocalPose = m_trajectories[sceneJ][checkNextFrameIndex]; + ORUtils::SE3Pose nextFramePredPose = ORUtils::SE3Pose(nextFrameLocalPose.GetM() * now_bestCandidate->m_relativePose->GetInvM()); + /* + m_visualisationGenerator->generate_voxel_visualisation( + rgb, slamStateI->get_voxel_scene(), nextFramePredPose, viewI->calib.intrinsics_rgb, + renderStateRGB, VisualisationGenerator::VT_SCENE_COLOUR, boost::none + ); + cv::Mat3b cvNextFrameTargetRGB = OpenCVUtil::make_rgb_image(rgb->GetData(MEMORYDEVICE_CPU), rgb->noDims.x, rgb->noDims.y); + m_visualisationGenerator->generate_voxel_visualisation( + rgb, slamStateJ->get_voxel_scene(), nextFrameLocalPose, viewI->calib.intrinsics_rgb, + renderStateRGB, VisualisationGenerator::VT_SCENE_COLOUR, boost::none + ); + cv::Mat3b cvNextFrameSourceRGB = OpenCVUtil::make_rgb_image(rgb->GetData(MEMORYDEVICE_CPU), rgb->noDims.x, rgb->noDims.y); + cv::imwrite("Source-"+ std::to_string(tmpIndexJ) + "-"+ std::to_string(checkNextFrameIndex)+".png", cvNextFrameSourceRGB); + cv::imwrite("Target-"+ std::to_string(tmpIndexJ) + "-"+ std::to_string(checkNextFrameIndex)+".png", cvNextFrameTargetRGB); + */ + m_visualisationGenerator->generate_depth_from_voxels( + depth, slamStateI->get_voxel_scene(), nextFramePredPose, viewI->calib.intrinsics_d, + renderStateD, DepthVisualiser::DT_ORTHOGRAPHIC + ); + cv::Mat1b cvNextFrameTargetDepth = OpenCVUtil::make_greyscale_image(depth->GetData(MEMORYDEVICE_CPU), depth->noDims.x, depth->noDims.y, OpenCVUtil::ROW_MAJOR, 100.0f); + m_visualisationGenerator->generate_depth_from_voxels( + depth, slamStateJ->get_voxel_scene(), nextFrameLocalPose, viewI->calib.intrinsics_d, + renderStateD, DepthVisualiser::DT_ORTHOGRAPHIC + ); + cv::Mat1b cvNextFrameSourceDepth = OpenCVUtil::make_greyscale_image(depth->GetData(MEMORYDEVICE_CPU), depth->noDims.x, depth->noDims.y, OpenCVUtil::ROW_MAJOR, 100.0f); + + cv::Mat cvNextSourceMask; + cv::inRange(cvNextFrameSourceDepth, cv::Scalar(0,0,0), cv::Scalar(0,0,0), cvNextSourceMask); + cv::bitwise_not(cvNextSourceMask, cvNextSourceMask); + + cv::Mat cvNextTargetMask; + cv::inRange(cvNextFrameTargetDepth, cv::Scalar(0,0,0), cv::Scalar(0,0,0), cvNextTargetMask); + cv::bitwise_not(cvNextTargetMask, cvNextTargetMask); + + cv::Mat cvNextCombinedMask; + cv::bitwise_and(cvNextSourceMask, cvNextTargetMask, cvNextCombinedMask); + + // Compute the difference between the source and target depth images, and mask it using the combined mask. + cv::Mat cvNextDepthDiff, cvNextMaskedDepthDiff; + cv::absdiff(cvNextFrameSourceDepth, cvNextFrameTargetDepth, cvNextDepthDiff); + cvNextDepthDiff.copyTo(cvNextMaskedDepthDiff, cvNextCombinedMask); + + // Determine the average depth difference for valid pixels in the source and target depth images. + cv::Scalar meanNextDepthDiff = cv::mean(cvNextMaskedDepthDiff); + + + // Compute the fraction of the target depth image that is valid. + float targetNextValidFraction = static_cast(cv::countNonZero(cvNextTargetMask == 255)) / (cvNextTargetMask.size().width * cvNextTargetMask.size().height); + + if (!verifying(meanNextDepthDiff, targetNextValidFraction)) { + printf("next frame Failed!!!!!!!!!\n"); + }else { + successTimes++; + } + } + int checkPrevFrameIndex = now_bestCandidate->m_frameIndexJ - i*10; + if (checkPrevFrameIndex >= 0) { + count++; + printf("this is another frame: %d\n", checkPrevFrameIndex); + ORUtils::SE3Pose prevFrameLocalPose = m_trajectories[sceneJ][checkPrevFrameIndex]; + ORUtils::SE3Pose prevFramePredPose = ORUtils::SE3Pose(prevFrameLocalPose.GetM() * now_bestCandidate->m_relativePose->GetInvM()); + /* + m_visualisationGenerator->generate_voxel_visualisation( + rgb, slamStateI->get_voxel_scene(), prevFramePredPose, viewI->calib.intrinsics_rgb, + renderStateRGB, VisualisationGenerator::VT_SCENE_COLOUR, boost::none + ); + cv::Mat3b cvPrevFrameTargetRGB = OpenCVUtil::make_rgb_image(rgb->GetData(MEMORYDEVICE_CPU), rgb->noDims.x, rgb->noDims.y); + m_visualisationGenerator->generate_voxel_visualisation( + rgb, slamStateJ->get_voxel_scene(), prevFrameLocalPose, viewI->calib.intrinsics_rgb, + renderStateRGB, VisualisationGenerator::VT_SCENE_COLOUR, boost::none + ); + cv::Mat3b cvPrevFrameSourceRGB = OpenCVUtil::make_rgb_image(rgb->GetData(MEMORYDEVICE_CPU), rgb->noDims.x, rgb->noDims.y); + cv::imwrite("Source"+ std::to_string(tmpIndexJ) + "-"+ std::to_string(checkPrevFrameIndex)+".png", cvPrevFrameSourceRGB); + cv::imwrite("Target"+ std::to_string(tmpIndexJ) + "-"+ std::to_string(checkPrevFrameIndex)+".png", cvPrevFrameTargetRGB); + */ + m_visualisationGenerator->generate_depth_from_voxels( + depth, slamStateI->get_voxel_scene(), prevFramePredPose, viewI->calib.intrinsics_d, + renderStateD, DepthVisualiser::DT_ORTHOGRAPHIC + ); + cv::Mat1b cvPrevFrameTargetDepth = OpenCVUtil::make_greyscale_image(depth->GetData(MEMORYDEVICE_CPU), depth->noDims.x, depth->noDims.y, OpenCVUtil::ROW_MAJOR, 100.0f); + m_visualisationGenerator->generate_depth_from_voxels( + depth, slamStateJ->get_voxel_scene(), prevFrameLocalPose, viewI->calib.intrinsics_d, + renderStateD, DepthVisualiser::DT_ORTHOGRAPHIC + ); + cv::Mat1b cvPrevFrameSourceDepth = OpenCVUtil::make_greyscale_image(depth->GetData(MEMORYDEVICE_CPU), depth->noDims.x, depth->noDims.y, OpenCVUtil::ROW_MAJOR, 100.0f); + + cv::Mat cvPrevSourceMask; + cv::inRange(cvPrevFrameSourceDepth, cv::Scalar(0,0,0), cv::Scalar(0,0,0), cvPrevSourceMask); + cv::bitwise_not(cvPrevSourceMask, cvPrevSourceMask); + + cv::Mat cvPrevTargetMask; + cv::inRange(cvPrevFrameTargetDepth, cv::Scalar(0,0,0), cv::Scalar(0,0,0), cvPrevTargetMask); + cv::bitwise_not(cvPrevTargetMask, cvPrevTargetMask); + + cv::Mat cvPrevCombinedMask; + cv::bitwise_and(cvPrevSourceMask, cvPrevTargetMask, cvPrevCombinedMask); + + // Compute the difference between the source and target depth images, and mask it using the combined mask. + cv::Mat cvPrevDepthDiff, cvPrevMaskedDepthDiff; + cv::absdiff(cvPrevFrameSourceDepth, cvPrevFrameTargetDepth, cvPrevDepthDiff); + cvPrevDepthDiff.copyTo(cvPrevMaskedDepthDiff, cvPrevCombinedMask); + + // Determine the average depth difference for valid pixels in the source and target depth images. + cv::Scalar meanPrevDepthDiff = cv::mean(cvPrevMaskedDepthDiff); + + + // Compute the fraction of the target depth image that is valid. + float targetPrevValidFraction = static_cast(cv::countNonZero(cvPrevTargetMask == 255)) / (cvPrevTargetMask.size().width * cvPrevTargetMask.size().height); + + if (!verifying(meanPrevDepthDiff, targetPrevValidFraction)) { + printf("Prev frame Failed!!!!!!!!!\n"); + }else { + successTimes++; + } + } + } // 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()); + double rate = (double)successTimes/(double)count; + if (rate > 0.6) { + 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; - + }else { + std::cout << "failed :( because next prev not match" << std::endl; + } #if defined(WITH_OPENCV) && DEBUGGING cv::waitKey(1); #endif @@ -512,9 +826,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 +883,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 +916,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); } } @@ -636,3 +957,5 @@ bool CollaborativeComponent::update_trajectories() } } + + diff --git a/modules/spaint/src/pipelinecomponents/SLAMComponent.cpp b/modules/spaint/src/pipelinecomponents/SLAMComponent.cpp index 82e8b08..0c6a42c 100644 --- a/modules/spaint/src/pipelinecomponents/SLAMComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/SLAMComponent.cpp @@ -8,8 +8,9 @@ using namespace orx; #include #include -#include #include +#include +#include namespace bf = boost::filesystem; #include @@ -43,20 +44,26 @@ namespace spaint { //#################### CONSTRUCTORS #################### -SLAMComponent::SLAMComponent(const SLAMContext_Ptr& context, const std::string& sceneID, const ImageSourceEngine_Ptr& imageSourceEngine, - const std::string& trackerConfig, MappingMode mappingMode, TrackingMode trackingMode, bool detectFiducials) -: m_context(context), - m_detectFiducials(detectFiducials), - m_fallibleTracker(NULL), - m_imageSourceEngine(imageSourceEngine), - m_initialFramesToFuse(50), // FIXME: This value should be passed in rather than hard-coded. - m_mappingMode(mappingMode), - m_relocaliserTrainingCount(0), - m_relocaliserTrainingSkip(0), - m_sceneID(sceneID), - m_settingsNamespace("SLAMComponent."), - m_trackerConfig(trackerConfig), - m_trackingMode(trackingMode) +SLAMComponent::SLAMComponent(const SLAMContext_Ptr &context, + const std::string &sceneID, + const ImageSourceEngine_Ptr &imageSourceEngine, + const std::string &trackerConfig, + MappingMode mappingMode, + TrackingMode trackingMode, + bool detectFiducials) + : m_context(context) + , m_detectFiducials(detectFiducials) + , m_fallibleTracker(NULL) + , m_imageSourceEngine(imageSourceEngine) + , m_initialFramesToFuse(50) + , // FIXME: This value should be passed in rather than hard-coded. + m_mappingMode(mappingMode) + , m_relocaliserTrainingCount(0) + , m_relocaliserTrainingSkip(0) + , m_sceneID(sceneID) + , m_settingsNamespace("SLAMComponent.") + , m_trackerConfig(trackerConfig) + , m_trackingMode(trackingMode) { // Determine the RGB and depth image sizes. Vector2i rgbImageSize = m_imageSourceEngine->getRGBImageSize(); @@ -64,12 +71,12 @@ SLAMComponent::SLAMComponent(const SLAMContext_Ptr& context, const std::string& if(depthImageSize.x == -1 || depthImageSize.y == -1) depthImageSize = rgbImageSize; // Set up the RGB and raw depth images into which input is to be read each frame. - const SLAMState_Ptr& slamState = context->get_slam_state(sceneID); + const SLAMState_Ptr &slamState = context->get_slam_state(sceneID); slamState->set_input_rgb_image(ORUChar4Image_Ptr(new ORUChar4Image(rgbImageSize, true, true))); slamState->set_input_raw_depth_image(ORShortImage_Ptr(new ORShortImage(depthImageSize, true, true))); // Set up the low-level engine. - const Settings_CPtr& settings = context->get_settings(); + const Settings_CPtr &settings = context->get_settings(); m_lowLevelEngine.reset(ITMLowLevelEngineFactory::MakeLowLevelEngine(settings->deviceType)); // Set up the view builder. @@ -77,15 +84,16 @@ SLAMComponent::SLAMComponent(const SLAMContext_Ptr& context, const std::string& // Set up the scenes. MemoryDeviceType memoryType = settings->GetMemoryType(); - slamState->set_voxel_scene(SpaintVoxelScene_Ptr(new SpaintVoxelScene(&settings->sceneParams, settings->swappingMode == ITMLibSettings::SWAPPINGMODE_ENABLED, memoryType))); + slamState->set_voxel_scene(SpaintVoxelScene_Ptr(new SpaintVoxelScene( + &settings->sceneParams, settings->swappingMode == ITMLibSettings::SWAPPINGMODE_ENABLED, memoryType))); if(mappingMode != MAP_VOXELS_ONLY) { slamState->set_surfel_scene(SpaintSurfelScene_Ptr(new SpaintSurfelScene(&settings->surfelSceneParams, memoryType))); } // Set up the dense mappers. - const SpaintVoxelScene_Ptr& voxelScene = slamState->get_voxel_scene(); - m_denseVoxelMapper.reset(new ITMDenseMapper(settings.get())); + const SpaintVoxelScene_Ptr &voxelScene = slamState->get_voxel_scene(); + m_denseVoxelMapper.reset(new ITMDenseMapper(settings.get())); if(mappingMode != MAP_VOXELS_ONLY) { m_denseSurfelMapper.reset(new ITMDenseSurfelMapper(depthImageSize, settings->deviceType)); @@ -95,16 +103,19 @@ SLAMComponent::SLAMComponent(const SLAMContext_Ptr& context, const std::string& setup_tracker(); m_trackingController.reset(new ITMTrackingController(m_tracker.get(), settings.get())); const Vector2i trackedImageSize = m_trackingController->GetTrackedImageSize(rgbImageSize, depthImageSize); + slamState->set_tracking_controller(m_trackingController); slamState->set_tracking_state(TrackingState_Ptr(new ITMTrackingState(trackedImageSize, memoryType))); // Set up the relocaliser. setup_relocaliser(); // Set up the live render states. - slamState->set_live_voxel_render_state(VoxelRenderState_Ptr(ITMRenderStateFactory::CreateRenderState(trackedImageSize, voxelScene->sceneParams, memoryType))); + slamState->set_live_voxel_render_state(VoxelRenderState_Ptr( + ITMRenderStateFactory::CreateRenderState(trackedImageSize, voxelScene->sceneParams, memoryType))); if(mappingMode != MAP_VOXELS_ONLY) { - slamState->set_live_surfel_render_state(SurfelRenderState_Ptr(new ITMSurfelRenderState(trackedImageSize, settings->surfelSceneParams.supersamplingFactor))); + slamState->set_live_surfel_render_state(SurfelRenderState_Ptr( + new ITMSurfelRenderState(trackedImageSize, settings->surfelSceneParams.supersamplingFactor))); } // Set up the scene. @@ -122,36 +133,33 @@ SLAMComponent::SLAMComponent(const SLAMContext_Ptr& context, const std::string& //#################### PUBLIC MEMBER FUNCTIONS #################### -bool SLAMComponent::get_fusion_enabled() const -{ - return m_fusionEnabled; -} +bool SLAMComponent::get_fusion_enabled() const { return m_fusionEnabled; } -const std::string& SLAMComponent::get_scene_id() const -{ - return m_sceneID; -} +const std::string &SLAMComponent::get_scene_id() const { return m_sceneID; } -void SLAMComponent::load_models(const std::string& inputDir) +TrackingController_Ptr& SLAMComponent::get_tracking_controller() { return m_trackingController; } + +void SLAMComponent::load_models(const std::string &inputDir) { // Reset the scene. reset_scene(); // Load the voxel model. Note that we have to add '/' to the directory in order to force // InfiniTAM's loading function to load the files from *inside* the specified folder. - const SLAMState_Ptr& slamState = m_context->get_slam_state(m_sceneID); + const SLAMState_Ptr &slamState = m_context->get_slam_state(m_sceneID); slamState->get_voxel_scene()->LoadFromDirectory(inputDir + "/"); - // TODO: If we support surfel model loading at some point in the future, the surfel model should be loaded here as well. + // TODO: If we support surfel model loading at some point in the future, the surfel model should be loaded here as + // well. // Load the relocaliser. m_context->get_relocaliser(m_sceneID)->load_from_disk(inputDir); // Set up the view to allow the scene to be rendered without any frames needing to be processed. // We are aiming to roughly mirror what would happen if we reconstructed the scene frame-by-frame. - const ORShortImage_Ptr& inputRawDepthImage = slamState->get_input_raw_depth_image(); - const ORUChar4Image_Ptr& inputRGBImage = slamState->get_input_rgb_image(); - const View_Ptr& view = slamState->get_view(); + const ORShortImage_Ptr &inputRawDepthImage = slamState->get_input_raw_depth_image(); + const ORUChar4Image_Ptr &inputRGBImage = slamState->get_input_rgb_image(); + const View_Ptr &view = slamState->get_view(); ITMView *newView = view.get(); inputRGBImage->Clear(); @@ -165,14 +173,11 @@ void SLAMComponent::load_models(const std::string& inputDir) set_fusion_enabled(false); } -void SLAMComponent::mirror_pose_of(const std::string& mirrorSceneID) -{ - m_mirrorSceneID = mirrorSceneID; -} +void SLAMComponent::mirror_pose_of(const std::string &mirrorSceneID) { m_mirrorSceneID = mirrorSceneID; } bool SLAMComponent::process_frame() { - const SLAMState_Ptr& slamState = m_context->get_slam_state(m_sceneID); + const SLAMState_Ptr &slamState = m_context->get_slam_state(m_sceneID); if(m_imageSourceEngine->hasImagesNow()) { @@ -180,10 +185,13 @@ bool SLAMComponent::process_frame() } else { - const SLAMState::InputStatus inputStatus = m_imageSourceEngine->hasMoreImages() ? SLAMState::IS_IDLE : SLAMState::IS_TERMINATED; + const SLAMState::InputStatus inputStatus = + m_imageSourceEngine->hasMoreImages() ? SLAMState::IS_IDLE : SLAMState::IS_TERMINATED; - // If finish training is enabled and no more images are expected, let the relocaliser know that no more calls will be made to its train or update functions. - if(m_finishTrainingEnabled && inputStatus == SLAMState::IS_TERMINATED && slamState->get_input_status() != SLAMState::IS_TERMINATED) + // If finish training is enabled and no more images are expected, let the relocaliser know that no more calls will + // be made to its train or update functions. + if(m_finishTrainingEnabled && inputStatus == SLAMState::IS_TERMINATED && + slamState->get_input_status() != SLAMState::IS_TERMINATED) { m_context->get_relocaliser(m_sceneID)->finish_training(); } @@ -193,14 +201,14 @@ bool SLAMComponent::process_frame() return false; } - const ORShortImage_Ptr& inputRawDepthImage = slamState->get_input_raw_depth_image(); - const ORUChar4Image_Ptr& inputRGBImage = slamState->get_input_rgb_image(); - const SurfelRenderState_Ptr& liveSurfelRenderState = slamState->get_live_surfel_render_state(); - const VoxelRenderState_Ptr& liveVoxelRenderState = slamState->get_live_voxel_render_state(); - const SpaintSurfelScene_Ptr& surfelScene = slamState->get_surfel_scene(); - const TrackingState_Ptr& trackingState = slamState->get_tracking_state(); - const View_Ptr& view = slamState->get_view(); - const SpaintVoxelScene_Ptr& voxelScene = slamState->get_voxel_scene(); + const ORShortImage_Ptr &inputRawDepthImage = slamState->get_input_raw_depth_image(); + const ORUChar4Image_Ptr &inputRGBImage = slamState->get_input_rgb_image(); + const SurfelRenderState_Ptr &liveSurfelRenderState = slamState->get_live_surfel_render_state(); + const VoxelRenderState_Ptr &liveVoxelRenderState = slamState->get_live_voxel_render_state(); + const SpaintSurfelScene_Ptr &surfelScene = slamState->get_surfel_scene(); + const TrackingState_Ptr &trackingState = slamState->get_tracking_state(); + const View_Ptr &view = slamState->get_view(); + const SpaintVoxelScene_Ptr &voxelScene = slamState->get_voxel_scene(); // Get the next frame. ITMView *newView = view.get(); @@ -215,7 +223,8 @@ bool SLAMComponent::process_frame() if(inputMask && inputMask->noDims == view->depth->noDims) { view->depth->UpdateHostFromDevice(); - maskedDepthImage = SegmentationUtil::apply_mask(inputMask, ORFloatImage_CPtr(view->depth, boost::serialization::null_deleter()), -1.0f); + maskedDepthImage = SegmentationUtil::apply_mask( + inputMask, ORFloatImage_CPtr(view->depth, boost::serialization::null_deleter()), -1.0f); maskedDepthImage->UpdateDeviceFromHost(); view->depth->Swap(*maskedDepthImage); } @@ -243,29 +252,26 @@ bool SLAMComponent::process_frame() // Determine the tracking quality, taking into account the failure mode being used. switch(m_context->get_settings()->behaviourOnFailure) { - case ITMLibSettings::FAILUREMODE_RELOCALISE: - { - // Allow the relocaliser to either improve the pose, store a new keyframe or update its model. - process_relocalisation(); - break; - } - case ITMLibSettings::FAILUREMODE_STOP_INTEGRATION: - { - // Since we're not using relocalisation, treat tracking failures like poor tracking, - // on the basis that it's better to try to keep going than to fail completely. - if(trackingState->trackerResult == ITMTrackingState::TRACKING_FAILED) - { - trackingState->trackerResult = ITMTrackingState::TRACKING_POOR; - } - break; - } - case ITMLibSettings::FAILUREMODE_IGNORE: - default: + case ITMLibSettings::FAILUREMODE_RELOCALISE: { + // Allow the relocaliser to either improve the pose, store a new keyframe or update its model. + process_relocalisation(); + break; + } + case ITMLibSettings::FAILUREMODE_STOP_INTEGRATION: { + // Since we're not using relocalisation, treat tracking failures like poor tracking, + // on the basis that it's better to try to keep going than to fail completely. + if(trackingState->trackerResult == ITMTrackingState::TRACKING_FAILED) { - // If we're completely ignoring poor or failed tracking, treat the tracking quality as good. - trackingState->trackerResult = ITMTrackingState::TRACKING_GOOD; - break; + trackingState->trackerResult = ITMTrackingState::TRACKING_POOR; } + break; + } + case ITMLibSettings::FAILUREMODE_IGNORE: + default: { + // If we're completely ignoring poor or failed tracking, treat the tracking quality as good. + trackingState->trackerResult = ITMTrackingState::TRACKING_GOOD; + break; + } } // Decide whether or not fusion should be run. @@ -284,22 +290,24 @@ bool SLAMComponent::process_frame() if(runFusion) { // Run the fusion process. - m_denseVoxelMapper->ProcessFrame(view.get(), trackingState.get(), voxelScene.get(), liveVoxelRenderState.get(), resetVisibleList); + m_denseVoxelMapper->ProcessFrame( + view.get(), trackingState.get(), voxelScene.get(), liveVoxelRenderState.get(), resetVisibleList); if(m_mappingMode != MAP_VOXELS_ONLY) { - m_denseSurfelMapper->ProcessFrame(view.get(), trackingState.get(), surfelScene.get(), liveSurfelRenderState.get()); + m_denseSurfelMapper->ProcessFrame( + view.get(), trackingState.get(), surfelScene.get(), liveSurfelRenderState.get()); } // If a mapping client is active: - const MappingClient_Ptr& mappingClient = m_context->get_mapping_client(m_sceneID); + const MappingClient_Ptr &mappingClient = m_context->get_mapping_client(m_sceneID); if(mappingClient) { // Send the current frame to the remote mapping server. MappingClient::RGBDFrameMessageQueue::PushHandler_Ptr pushHandler = mappingClient->begin_push_frame_message(); - boost::optional elt = pushHandler->get(); + boost::optional elt = pushHandler->get(); if(elt) { - RGBDFrameMessage& msg = **elt; + RGBDFrameMessage &msg = **elt; msg.set_frame_index(static_cast(m_fusedFramesCount)); msg.set_pose(*trackingState->pose_d); msg.set_rgb_image(inputRGBImage); @@ -311,8 +319,10 @@ bool SLAMComponent::process_frame() } else if(trackingState->trackerResult != ITMTrackingState::TRACKING_FAILED) { - // If we're not fusing, but the tracking has not completely failed, update the list of visible blocks so that things are kept up to date. - m_denseVoxelMapper->UpdateVisibleList(view.get(), trackingState.get(), voxelScene.get(), liveVoxelRenderState.get(), resetVisibleList); + // If we're not fusing, but the tracking has not completely failed, update the list of visible blocks so that things + // are kept up to date. + m_denseVoxelMapper->UpdateVisibleList( + view.get(), trackingState.get(), voxelScene.get(), liveVoxelRenderState.get(), resetVisibleList); } else { @@ -323,19 +333,246 @@ bool SLAMComponent::process_frame() // Render from the live camera position to prepare for tracking in the next frame. prepare_for_tracking(m_trackingMode); - // If we're using surfel mapping, render a supersampled index image to use when finding surfel correspondences in the next frame. + // If we're using surfel mapping, render a supersampled index image to use when finding surfel correspondences in the + // next frame. if(m_mappingMode != MAP_VOXELS_ONLY) { - m_context->get_surfel_visualisation_engine()->FindSurfaceSuper(surfelScene.get(), trackingState->pose_d, &view->calib.intrinsics_d, USR_RENDER, liveSurfelRenderState.get()); + m_context->get_surfel_visualisation_engine()->FindSurfaceSuper( + surfelScene.get(), trackingState->pose_d, &view->calib.intrinsics_d, USR_RENDER, liveSurfelRenderState.get()); + } + + // If we're using a composite image source engine, the current sub-engine has run out of images and we're not using + // global poses, disable fusion. + CompositeImageSourceEngine_CPtr compositeImageSourceEngine = + boost::dynamic_pointer_cast(m_imageSourceEngine); + const bool usingGlobalPoses = + m_context->get_settings()->get_first_value("globalPosesSpecifier", "") != ""; + if(compositeImageSourceEngine && !compositeImageSourceEngine->getCurrentSubengine()->hasMoreImages() && + !usingGlobalPoses) + m_fusionEnabled = false; + + // If we're using a fiducial detector and the user wants to detect fiducials and the tracking is good, try to detect + // fiducial markers in the current view of the scene and update the current set of fiducials that we're maintaining + // accordingly. + FiducialDetector_CPtr fiducialDetector = m_context->get_fiducial_detector(m_sceneID); + if(fiducialDetector && m_detectFiducials && trackingState->trackerResult == ITMTrackingState::TRACKING_GOOD) + { + slamState->update_fiducials(fiducialDetector->detect_fiducials(view, *trackingState->pose_d)); + } + +#ifdef WITH_VICON + // If we're using a Vicon fiducial detector to calibrate the Vicon system, and a stable pose for the Vicon origin has + // newly been determined, store the relative transformation from world space to Vicon space. + const ViconInterface_Ptr &vicon = m_context->get_vicon(); + if(vicon && !vicon->get_world_to_vicon_transform(m_sceneID) && + boost::dynamic_pointer_cast(fiducialDetector)) + { + const std::map &fiducials = slamState->get_fiducials(); + if(!fiducials.empty()) + { + Fiducial_CPtr fiducial = fiducials.begin()->second; + if(fiducial->confidence() >= Fiducial::stable_confidence()) + { + vicon->set_world_to_vicon_transform(m_sceneID, fiducial->pose().GetM()); + } + } + } +#endif + + return true; +} + +bool SLAMComponent::process_frame_pose() +{ + const SLAMState_Ptr &slamState = m_context->get_slam_state(m_sceneID); + std::cout << "m_imageSourceEngine->hasImagesNow(): " << m_imageSourceEngine->hasImagesNow() << "\n"; + if(m_imageSourceEngine->hasImagesNow()) + { + slamState->set_input_status(SLAMState::IS_ACTIVE); + } + else + { + // const SLAMState::InputStatus inputStatus = m_imageSourceEngine->hasMoreImages() ? SLAMState::IS_IDLE : + // SLAMState::IS_TERMINATED; + const SLAMState::InputStatus inputStatus = SLAMState::IS_TERMINATED; + // If finish training is enabled and no more images are expected, let the relocaliser know that no more calls will + // be made to its train or update functions. + if(m_finishTrainingEnabled && inputStatus == SLAMState::IS_TERMINATED && + slamState->get_input_status() != SLAMState::IS_TERMINATED) + { + m_context->get_relocaliser(m_sceneID)->finish_training(); + } + const SLAMState_Ptr &slamState = m_context->get_slam_state(m_sceneID); + auto pose = slamState->get_tracking_state()->pose_d->GetM(); + std::cout << "this is " << m_sceneID << " pose: \n" << pose << "\n"; + + slamState->set_input_status(inputStatus); + /* + const TrackingState_Ptr &trackingState = slamState->get_tracking_state(); + const View_Ptr &view = slamState->get_view(); + + std::cout << "1 this is " << m_sceneID << " pose: \n" << trackingState->pose_d->GetM() << "\n"; + m_trackingController->Track(trackingState.get(), view.get()); + std::cout << "2 this is " << m_sceneID << " pose: \n" << trackingState->pose_d->GetM() << "\n"; + m_trackingController->Track(trackingState.get(), view.get()); + std::cout << "3 this is " << m_sceneID << " pose: \n" << trackingState->pose_d->GetM() << "\n"; + m_trackingController->Track(trackingState.get(), view.get()); + std::cout << "4 this is " << m_sceneID << " pose: \n" << trackingState->pose_d->GetM() << "\n"; + */ + + return false; + } + /* + const ORShortImage_Ptr& inputRawDepthImage = slamState->get_input_raw_depth_image(); + const ORUChar4Image_Ptr& inputRGBImage = slamState->get_input_rgb_image(); + */ + const SurfelRenderState_Ptr &liveSurfelRenderState = slamState->get_live_surfel_render_state(); + const VoxelRenderState_Ptr &liveVoxelRenderState = slamState->get_live_voxel_render_state(); + const SpaintSurfelScene_Ptr &surfelScene = slamState->get_surfel_scene(); + + const TrackingState_Ptr &trackingState = slamState->get_tracking_state(); + const View_Ptr &view = slamState->get_view(); + const SpaintVoxelScene_Ptr &voxelScene = slamState->get_voxel_scene(); + + // Get the next frame. + /* + ITMView *newView = view.get(); + m_imageSourceEngine->getImages(inputRGBImage.get(), inputRawDepthImage.get()); + const bool useBilateralFilter = m_trackingMode == TRACK_SURFELS; + m_viewBuilder->UpdateView(&newView, inputRGBImage.get(), inputRawDepthImage.get(), useBilateralFilter); + slamState->set_view(newView); + */ + + // If there's an active input mask of the right size, apply it to the depth image. + /* + ORFloatImage_Ptr maskedDepthImage; + ORUCharImage_CPtr inputMask = m_context->get_slam_state(m_sceneID)->get_input_mask(); + if(inputMask && inputMask->noDims == view->depth->noDims) + { + view->depth->UpdateHostFromDevice(); + maskedDepthImage = SegmentationUtil::apply_mask(inputMask, ORFloatImage_CPtr(view->depth, + boost::serialization::null_deleter()), -1.0f); maskedDepthImage->UpdateDeviceFromHost(); + view->depth->Swap(*maskedDepthImage); + } + */ + + // Make a note of the current pose in case tracking fails. + SE3Pose oldPose(*trackingState->pose_d); + + // If we're mirroring the pose of another scene, copy the pose from that scene's tracking state. + // If not, use our own tracker to estimate the pose. + + if(m_mirrorSceneID != "") + { + *trackingState->pose_d = m_context->get_slam_state(m_mirrorSceneID)->get_pose(); + trackingState->trackerResult = ITMTrackingState::TRACKING_GOOD; + } + else + { + // Note: When using a normal tracker, it's safe to call this even before we've started fusion (it will be a no-op). + // When using a file-based tracker, we *must* call it in order to correctly set the pose for the first frame. + std::cout << "this is m_trackingController\n"; + m_trackingController->Track(trackingState.get(), view.get()); + } + + // If there was an active input mask, restore the original depth image after tracking. + // if(maskedDepthImage) view->depth->Swap(*maskedDepthImage); + + // Determine the tracking quality, taking into account the failure mode being used. + + switch(m_context->get_settings()->behaviourOnFailure) + { + case ITMLibSettings::FAILUREMODE_RELOCALISE: { + // Allow the relocaliser to either improve the pose, store a new keyframe or update its model. + process_relocalisation(); + break; + } + case ITMLibSettings::FAILUREMODE_STOP_INTEGRATION: { + // Since we're not using relocalisation, treat tracking failures like poor tracking, + // on the basis that it's better to try to keep going than to fail completely. + if(trackingState->trackerResult == ITMTrackingState::TRACKING_FAILED) + { + trackingState->trackerResult = ITMTrackingState::TRACKING_POOR; + } + break; + } + case ITMLibSettings::FAILUREMODE_IGNORE: + default: { + // If we're completely ignoring poor or failed tracking, treat the tracking quality as good. + trackingState->trackerResult = ITMTrackingState::TRACKING_GOOD; + break; + } } - // If we're using a composite image source engine, the current sub-engine has run out of images and we're not using global poses, disable fusion. - CompositeImageSourceEngine_CPtr compositeImageSourceEngine = boost::dynamic_pointer_cast(m_imageSourceEngine); - const bool usingGlobalPoses = m_context->get_settings()->get_first_value("globalPosesSpecifier", "") != ""; - if(compositeImageSourceEngine && !compositeImageSourceEngine->getCurrentSubengine()->hasMoreImages() && !usingGlobalPoses) m_fusionEnabled = false; + // Decide whether or not fusion should be run. + bool runFusion = m_fusionEnabled; + std::cout << "this is runFuision: " << runFusion << "\n"; + std::cout << "this is trackingState->trackerResult: " << trackingState->trackerResult << "\n"; + if(trackingState->trackerResult == ITMTrackingState::TRACKING_FAILED || + (trackingState->trackerResult == ITMTrackingState::TRACKING_POOR && m_fusedFramesCount >= m_initialFramesToFuse) || + (m_fallibleTracker && m_fallibleTracker->lost_tracking())) + { + runFusion = false; + } - // If we're using a fiducial detector and the user wants to detect fiducials and the tracking is good, try to detect fiducial markers - // in the current view of the scene and update the current set of fiducials that we're maintaining accordingly. + // Decide whether or not we need to reset the visible list. This is necessary if we won't be rendering + // point clouds during tracking, since otherwise space carving won't work. + const bool resetVisibleList = !m_tracker->requiresPointCloudRendering(); + + if(runFusion) + { + // Run the fusion process. + m_denseVoxelMapper->ProcessFrame( + view.get(), trackingState.get(), voxelScene.get(), liveVoxelRenderState.get(), resetVisibleList); + if(m_mappingMode != MAP_VOXELS_ONLY) + { + m_denseSurfelMapper->ProcessFrame( + view.get(), trackingState.get(), surfelScene.get(), liveSurfelRenderState.get()); + } + + // If a mapping client is active: + const MappingClient_Ptr &mappingClient = m_context->get_mapping_client(m_sceneID); + ++m_fusedFramesCount; + } + else if(trackingState->trackerResult != ITMTrackingState::TRACKING_FAILED) + { + // If we're not fusing, but the tracking has not completely failed, update the list of visible blocks so that things + // are kept up to date. + m_denseVoxelMapper->UpdateVisibleList( + view.get(), trackingState.get(), voxelScene.get(), liveVoxelRenderState.get(), resetVisibleList); + } + else + { + // If the tracking has completely failed, restore the pose from the previous frame. + *trackingState->pose_d = oldPose; + } + auto matrix = trackingState->pose_d->GetM(); + std::cout << "matrix: " << matrix << "\n"; + + // Render from the live camera position to prepare for tracking in the next frame. + prepare_for_tracking(m_trackingMode); + + // If we're using surfel mapping, render a supersampled index image to use when finding surfel correspondences in the + // next frame. + if(m_mappingMode != MAP_VOXELS_ONLY) + { + m_context->get_surfel_visualisation_engine()->FindSurfaceSuper( + surfelScene.get(), trackingState->pose_d, &view->calib.intrinsics_d, USR_RENDER, liveSurfelRenderState.get()); + } + + // If we're using a composite image source engine, the current sub-engine has run out of images and we're not using + // global poses, disable fusion. + CompositeImageSourceEngine_CPtr compositeImageSourceEngine = + boost::dynamic_pointer_cast(m_imageSourceEngine); + const bool usingGlobalPoses = + m_context->get_settings()->get_first_value("globalPosesSpecifier", "") != ""; + if(compositeImageSourceEngine && !compositeImageSourceEngine->getCurrentSubengine()->hasMoreImages() && + !usingGlobalPoses) + m_fusionEnabled = false; + + // If we're using a fiducial detector and the user wants to detect fiducials and the tracking is good, try to detect + // fiducial markers in the current view of the scene and update the current set of fiducials that we're maintaining + // accordingly. FiducialDetector_CPtr fiducialDetector = m_context->get_fiducial_detector(m_sceneID); if(fiducialDetector && m_detectFiducials && trackingState->trackerResult == ITMTrackingState::TRACKING_GOOD) { @@ -343,12 +580,13 @@ bool SLAMComponent::process_frame() } #ifdef WITH_VICON - // If we're using a Vicon fiducial detector to calibrate the Vicon system, and a stable pose for the Vicon origin has newly been determined, - // store the relative transformation from world space to Vicon space. - const ViconInterface_Ptr& vicon = m_context->get_vicon(); - if(vicon && !vicon->get_world_to_vicon_transform(m_sceneID) && boost::dynamic_pointer_cast(fiducialDetector)) + // If we're using a Vicon fiducial detector to calibrate the Vicon system, and a stable pose for the Vicon origin has + // newly been determined, store the relative transformation from world space to Vicon space. + const ViconInterface_Ptr &vicon = m_context->get_vicon(); + if(vicon && !vicon->get_world_to_vicon_transform(m_sceneID) && + boost::dynamic_pointer_cast(fiducialDetector)) { - const std::map& fiducials = slamState->get_fiducials(); + const std::map &fiducials = slamState->get_fiducials(); if(!fiducials.empty()) { Fiducial_CPtr fiducial = fiducials.begin()->second; @@ -366,7 +604,7 @@ bool SLAMComponent::process_frame() void SLAMComponent::reset_scene() { // Reset the scene. - const SLAMState_Ptr& slamState = m_context->get_slam_state(m_sceneID); + const SLAMState_Ptr &slamState = m_context->get_slam_state(m_sceneID); m_denseVoxelMapper->ResetScene(slamState->get_voxel_scene().get()); if(m_mappingMode != MAP_VOXELS_ONLY) { @@ -385,7 +623,7 @@ void SLAMComponent::reset_scene() m_fusionEnabled = true; } -void SLAMComponent::save_models(const std::string& outputDir) const +void SLAMComponent::save_models(const std::string &outputDir) const { // If reconstruction hasn't started yet, early out. SLAMState_CPtr slamState = m_context->get_slam_state(m_sceneID); @@ -400,7 +638,7 @@ void SLAMComponent::save_models(const std::string& outputDir) const // Save relevant settings to a configuration file. Settings_CPtr settings = m_context->get_settings(); - const ITMSceneParams& sceneParams = settings->sceneParams; + const ITMSceneParams &sceneParams = settings->sceneParams; const std::string configFilename = outputDir + "/settings.ini"; { std::ofstream fs(configFilename.c_str()); @@ -423,17 +661,11 @@ void SLAMComponent::save_models(const std::string& outputDir) const m_context->get_relocaliser(m_sceneID)->save_to_disk(outputDir); } -void SLAMComponent::set_detect_fiducials(bool detectFiducials) -{ - m_detectFiducials = detectFiducials; -} +void SLAMComponent::set_detect_fiducials(bool detectFiducials) { m_detectFiducials = detectFiducials; } -void SLAMComponent::set_fusion_enabled(bool fusionEnabled) -{ - m_fusionEnabled = fusionEnabled; -} +void SLAMComponent::set_fusion_enabled(bool fusionEnabled) { m_fusionEnabled = fusionEnabled; } -void SLAMComponent::set_mapping_client(const MappingClient_Ptr& mappingClient) +void SLAMComponent::set_mapping_client(const MappingClient_Ptr &mappingClient) { m_context->get_mapping_client(m_sceneID) = mappingClient; @@ -465,8 +697,9 @@ std::vector SLAMComponent::load_ground_truth_relocalisation_trajectory( { std::vector groundTruthTrajectory; - // Detect any sequences for which we're using force-fail tracking, create a disk-based tracker for each of them, and wrap them into a composite tracker. - const Settings_CPtr& settings = m_context->get_settings(); + // Detect any sequences for which we're using force-fail tracking, create a disk-based tracker for each of them, and + // wrap them into a composite tracker. + const Settings_CPtr &settings = m_context->get_settings(); std::vector diskTrackerConfigs = settings->get_values("diskTrackerConfigs"); std::vector trackerSpecifiers = settings->get_values("trackerSpecifiers"); @@ -483,11 +716,18 @@ std::vector SLAMComponent::load_ground_truth_relocalisation_trajectory( trackerConfig += ""; const bool trackSurfels = false; - const SLAMState_Ptr& slamState = m_context->get_slam_state(m_sceneID); + const SLAMState_Ptr &slamState = m_context->get_slam_state(m_sceneID); FallibleTracker *dummy; - Tracker_Ptr groundTruthTracker = m_context->get_tracker_factory().make_tracker_from_string( - trackerConfig, m_sceneID, trackSurfels, slamState->get_rgb_image_size(), slamState->get_depth_image_size(), m_lowLevelEngine, m_imuCalibrator, settings, dummy - ); + Tracker_Ptr groundTruthTracker = + m_context->get_tracker_factory().make_tracker_from_string(trackerConfig, + m_sceneID, + trackSurfels, + slamState->get_rgb_image_size(), + slamState->get_depth_image_size(), + m_lowLevelEngine, + m_imuCalibrator, + settings, + dummy); // Read in all of the poses from the composite tracker and return the trajectory. ITMTrackingState groundTruthTrackingState(slamState->get_depth_image_size(), MEMORYDEVICE_CUDA); @@ -502,37 +742,43 @@ std::vector SLAMComponent::load_ground_truth_relocalisation_trajectory( void SLAMComponent::prepare_for_tracking(TrackingMode trackingMode) { - const SLAMState_Ptr& slamState = m_context->get_slam_state(m_sceneID); - const TrackingState_Ptr& trackingState = slamState->get_tracking_state(); - const View_Ptr& view = slamState->get_view(); + const SLAMState_Ptr &slamState = m_context->get_slam_state(m_sceneID); + const TrackingState_Ptr &trackingState = slamState->get_tracking_state(); + const View_Ptr &view = slamState->get_view(); switch(trackingMode) { - case TRACK_SURFELS: - { - const SpaintSurfelScene_Ptr& surfelScene = slamState->get_surfel_scene(); - const SurfelRenderState_Ptr& liveSurfelRenderState = slamState->get_live_surfel_render_state(); - m_trackingController->Prepare(trackingState.get(), surfelScene.get(), view.get(), m_context->get_surfel_visualisation_engine().get(), liveSurfelRenderState.get()); - break; - } - case TRACK_VOXELS: - default: - { - const SpaintVoxelScene_Ptr& voxelScene = slamState->get_voxel_scene(); - const VoxelRenderState_Ptr& liveVoxelRenderState = slamState->get_live_voxel_render_state(); - m_trackingController->Prepare(trackingState.get(), voxelScene.get(), view.get(), m_context->get_voxel_visualisation_engine().get(), liveVoxelRenderState.get()); - break; - } + case TRACK_SURFELS: { + const SpaintSurfelScene_Ptr &surfelScene = slamState->get_surfel_scene(); + const SurfelRenderState_Ptr &liveSurfelRenderState = slamState->get_live_surfel_render_state(); + m_trackingController->Prepare(trackingState.get(), + surfelScene.get(), + view.get(), + m_context->get_surfel_visualisation_engine().get(), + liveSurfelRenderState.get()); + break; + } + case TRACK_VOXELS: + default: { + const SpaintVoxelScene_Ptr &voxelScene = slamState->get_voxel_scene(); + const VoxelRenderState_Ptr &liveVoxelRenderState = slamState->get_live_voxel_render_state(); + m_trackingController->Prepare(trackingState.get(), + voxelScene.get(), + view.get(), + m_context->get_voxel_visualisation_engine().get(), + liveVoxelRenderState.get()); + break; + } } } void SLAMComponent::process_relocalisation() { - const Relocaliser_Ptr& relocaliser = m_context->get_relocaliser(m_sceneID); - const SLAMState_Ptr& slamState = m_context->get_slam_state(m_sceneID); - const TrackingState_Ptr& trackingState = slamState->get_tracking_state(); - const View_Ptr& view = slamState->get_view(); - const Vector4f& depthIntrinsics = view->calib.intrinsics_d.projectionParamsSimple.all; + const Relocaliser_Ptr &relocaliser = m_context->get_relocaliser(m_sceneID); + const SLAMState_Ptr &slamState = m_context->get_slam_state(m_sceneID); + const TrackingState_Ptr &trackingState = slamState->get_tracking_state(); + const View_Ptr &view = slamState->get_view(); + const Vector4f &depthIntrinsics = view->calib.intrinsics_d.projectionParamsSimple.all; // Save the current pose in case we need to restore it later. const SE3Pose oldPose(*trackingState->pose_d); @@ -541,11 +787,9 @@ void SLAMComponent::process_relocalisation() // - Relocalising every frame is enabled // - The tracking succeeded and the current frame is not one we should skip const bool performTraining = - m_relocaliseEveryFrame || - ( - trackingState->trackerResult == ITMTrackingState::TRACKING_GOOD && - (m_relocaliserTrainingSkip == 0 || (m_relocaliserTrainingCount++ % m_relocaliserTrainingSkip == 0)) - ); + m_relocaliseEveryFrame || + (trackingState->trackerResult == ITMTrackingState::TRACKING_GOOD && + (m_relocaliserTrainingSkip == 0 || (m_relocaliserTrainingCount++ % m_relocaliserTrainingSkip == 0))); // If we're not training in this frame, allow the relocaliser to perform any necessary internal bookkeeping. // Note that we prevent training and bookkeeping from both running in the same frame for performance reasons. @@ -554,17 +798,22 @@ void SLAMComponent::process_relocalisation() relocaliser->update(); } - // Relocalise if either (a) the tracking has failed, or (b) we're forcibly relocalising every frame for evaluation purposes. - const bool performRelocalisation = m_relocaliseEveryFrame || trackingState->trackerResult == ITMTrackingState::TRACKING_FAILED; + // Relocalise if either (a) the tracking has failed, or (b) we're forcibly relocalising every frame for evaluation + // purposes. + const bool performRelocalisation = + m_relocaliseEveryFrame || trackingState->trackerResult == ITMTrackingState::TRACKING_FAILED; if(performRelocalisation) { - std::vector relocalisationResults = relocaliser->relocalise(view->rgb, view->depth, depthIntrinsics); + std::vector relocalisationResults = + relocaliser->relocalise(view->rgb, view->depth, depthIntrinsics); if(!relocalisationResults.empty()) { - const Relocaliser::Result& bestRelocalisationResult = relocalisationResults[0]; + const Relocaliser::Result &bestRelocalisationResult = relocalisationResults[0]; trackingState->pose_d->SetFrom(&bestRelocalisationResult.pose); - trackingState->trackerResult = bestRelocalisationResult.quality == Relocaliser::RELOCALISATION_GOOD ? ITMTrackingState::TRACKING_GOOD : ITMTrackingState::TRACKING_POOR; + trackingState->trackerResult = bestRelocalisationResult.quality == Relocaliser::RELOCALISATION_GOOD + ? ITMTrackingState::TRACKING_GOOD + : ITMTrackingState::TRACKING_POOR; } } @@ -584,33 +833,45 @@ void SLAMComponent::process_relocalisation() } } -Relocaliser_Ptr SLAMComponent::refine_with_icp(const Relocaliser_Ptr& relocaliser) const +Relocaliser_Ptr SLAMComponent::refine_with_icp(const Relocaliser_Ptr &relocaliser) const { const Vector2i depthImageSize = m_imageSourceEngine->getDepthImageSize(); const Vector2i rgbImageSize = m_imageSourceEngine->getRGBImageSize(); - const Settings_CPtr& settings = m_context->get_settings(); - const SpaintVoxelScene_Ptr& voxelScene = m_context->get_slam_state(m_sceneID)->get_voxel_scene(); + const Settings_CPtr &settings = m_context->get_settings(); + const SpaintVoxelScene_Ptr &voxelScene = m_context->get_slam_state(m_sceneID)->get_voxel_scene(); std::string trackerConfig = ""; - std::string trackerParams = settings->get_first_value(m_settingsNamespace + "refinementTrackerParams", ""); + std::string trackerParams = + settings->get_first_value(m_settingsNamespace + "refinementTrackerParams", ""); if(trackerParams != "") trackerConfig += "" + trackerParams + ""; trackerConfig += ""; const bool trackSurfels = false; FallibleTracker *dummy; - Tracker_Ptr tracker = m_context->get_tracker_factory().make_tracker_from_string( - trackerConfig, m_sceneID, trackSurfels, rgbImageSize, depthImageSize, m_lowLevelEngine, m_imuCalibrator, settings, dummy - ); - - return Relocaliser_Ptr(new ICPRefiningRelocaliser( - relocaliser, tracker, rgbImageSize, depthImageSize, m_imageSourceEngine->getCalib(), voxelScene, m_denseVoxelMapper, settings - )); + Tracker_Ptr tracker = m_context->get_tracker_factory().make_tracker_from_string(trackerConfig, + m_sceneID, + trackSurfels, + rgbImageSize, + depthImageSize, + m_lowLevelEngine, + m_imuCalibrator, + settings, + dummy); + + return Relocaliser_Ptr(new ICPRefiningRelocaliser(relocaliser, + tracker, + rgbImageSize, + depthImageSize, + m_imageSourceEngine->getCalib(), + voxelScene, + m_denseVoxelMapper, + settings)); } void SLAMComponent::setup_fiducial_detector() { const SpaintVoxelScene_CPtr scene = m_context->get_slam_state(m_sceneID)->get_voxel_scene(); - const Settings_CPtr& settings = m_context->get_settings(); + const Settings_CPtr &settings = m_context->get_settings(); const std::string type = m_context->get_settings()->get_first_value("fiducialDetectorType"); FiducialDetector_CPtr fiducialDetector; @@ -626,7 +887,8 @@ void SLAMComponent::setup_fiducial_detector() ViconInterface_CPtr vicon = m_context->get_vicon(); if(vicon) { - FiducialDetector_CPtr calibratingDetector(new ArUcoFiducialDetector(scene, settings, ArUcoFiducialDetector::PEM_RAYCAST)); + FiducialDetector_CPtr calibratingDetector( + new ArUcoFiducialDetector(scene, settings, ArUcoFiducialDetector::PEM_RAYCAST)); fiducialDetector.reset(new ViconFiducialDetector(vicon, calibratingDetector)); } #endif @@ -639,7 +901,7 @@ void SLAMComponent::setup_relocaliser() { // Look up the non-relocaliser-specific settings, such as the type of relocaliser to construct. // Note that the relocaliser type is a primary setting, so is not in the SLAMComponent namespace. - const Settings_CPtr& settings = m_context->get_settings(); + const Settings_CPtr &settings = m_context->get_settings(); m_relocaliserType = settings->get_first_value("relocaliserType"); m_finishTrainingEnabled = settings->get_first_value(m_settingsNamespace + "finishTrainingEnabled", false); @@ -647,27 +909,33 @@ void SLAMComponent::setup_relocaliser() m_relocaliserTrainingSkip = settings->get_first_value(m_settingsNamespace + "relocaliserTrainingSkip", 0); m_context->get_relocaliser(m_sceneID) = RelocaliserFactory::make_relocaliser( - m_relocaliserType, - m_imageSourceEngine->getDepthImageSize(), - m_relocaliseEveryFrame, - boost::bind(&SLAMComponent::refine_with_icp, this, _1), - boost::bind(&SLAMComponent::load_ground_truth_relocalisation_trajectory, this), - settings - ); + m_relocaliserType, + m_imageSourceEngine->getDepthImageSize(), + m_relocaliseEveryFrame, + boost::bind(&SLAMComponent::refine_with_icp, this, _1), + boost::bind(&SLAMComponent::load_ground_truth_relocalisation_trajectory, this), + settings); } void SLAMComponent::setup_tracker() { - const MappingServer_Ptr& mappingServer = m_context->get_mapping_server(); - const Settings_CPtr& settings = m_context->get_settings(); - const SLAMState_Ptr& slamState = m_context->get_slam_state(m_sceneID); - const Vector2i& depthImageSize = slamState->get_depth_image_size(); - const Vector2i& rgbImageSize = slamState->get_rgb_image_size(); + const MappingServer_Ptr &mappingServer = m_context->get_mapping_server(); + const Settings_CPtr &settings = m_context->get_settings(); + const SLAMState_Ptr &slamState = m_context->get_slam_state(m_sceneID); + const Vector2i &depthImageSize = slamState->get_depth_image_size(); + const Vector2i &rgbImageSize = slamState->get_rgb_image_size(); m_imuCalibrator.reset(new ITMIMUCalibrator_iPad); - m_tracker = m_context->get_tracker_factory().make_tracker_from_string( - m_trackerConfig, m_sceneID, m_trackingMode == TRACK_SURFELS, rgbImageSize, depthImageSize, m_lowLevelEngine, m_imuCalibrator, settings, m_fallibleTracker, mappingServer - ); + m_tracker = m_context->get_tracker_factory().make_tracker_from_string(m_trackerConfig, + m_sceneID, + m_trackingMode == TRACK_SURFELS, + rgbImageSize, + depthImageSize, + m_lowLevelEngine, + m_imuCalibrator, + settings, + m_fallibleTracker, + mappingServer); } -} +} // namespace spaint diff --git a/modules/spaint/src/slamstate/SLAMState.cpp b/modules/spaint/src/slamstate/SLAMState.cpp index 4628885..b18cdad 100644 --- a/modules/spaint/src/slamstate/SLAMState.cpp +++ b/modules/spaint/src/slamstate/SLAMState.cpp @@ -168,6 +168,11 @@ void SLAMState::set_surfel_scene(const SpaintSurfelScene_Ptr& surfelScene) m_surfelScene = surfelScene; } +void SLAMState::set_tracking_controller(const TrackingController_Ptr& trackingController) +{ + m_trackingController = trackingController; +} + void SLAMState::set_tracking_state(const TrackingState_Ptr& trackingState) { m_trackingState = trackingState; 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;