From 5e1763965b680f1b8dad3fcf79e98cdb49113f03 Mon Sep 17 00:00:00 2001 From: tsm Date: Mon, 31 Oct 2022 15:38:14 +0800 Subject: [PATCH 01/23] add multi thread for relocalisation without compiling --- .vscode/settings.json | 69 ++++++++++++++++ .../CollaborativeComponent.h | 7 ++ .../CollaborativeComponent.cpp | 80 ++++++++++++------- 3 files changed, 129 insertions(+), 27 deletions(-) create mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..24412e1 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,69 @@ +{ + "files.associations": { + "xstring": "cpp", + "memory": "cpp", + "*.tpp": "cpp", + "*.tcu": "cpp", + "algorithm": "cpp", + "atomic": "cpp", + "bit": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "compare": "cpp", + "concepts": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "deque": "cpp", + "exception": "cpp", + "fstream": "cpp", + "functional": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "ios": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "iterator": "cpp", + "limits": "cpp", + "list": "cpp", + "map": "cpp", + "new": "cpp", + "numeric": "cpp", + "ostream": "cpp", + "queue": "cpp", + "set": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "string": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "typeinfo": "cpp", + "unordered_map": "cpp", + "utility": "cpp", + "vector": "cpp", + "xfacet": "cpp", + "xhash": "cpp", + "xiosbase": "cpp", + "xlocale": "cpp", + "xlocinfo": "cpp", + "xlocmon": "cpp", + "xlocnum": "cpp", + "xloctime": "cpp", + "xmemory": "cpp", + "xstddef": "cpp", + "xtr1common": "cpp", + "xtree": "cpp", + "xutility": "cpp", + "ratio": "cpp", + "stop_token": "cpp", + "thread": "cpp" + } +} \ No newline at end of file diff --git a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h index 407029a..497d1a6 100644 --- a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h +++ b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h @@ -21,6 +21,9 @@ namespace spaint { +const int relocalisationThreadsCount = 5; +const int bestCandidateMaxCount = 5; + /** * \brief An instance of this pipeline component can be used to determine the relative poses between agents participating in collaborative SLAM. */ @@ -31,6 +34,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::queue> m_bestCandidates = std::queue>(); + /** The timer used to compute the time spent collaborating. */ boost::optional m_collaborationTimer; @@ -61,6 +66,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; diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index 7676af3..7785e18 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -21,6 +21,8 @@ using boost::bind; #include using namespace orx; +#include + #define DEBUGGING 0 namespace spaint { @@ -42,7 +44,10 @@ CollaborativeComponent::CollaborativeComponent(const CollaborativeContext_Ptr& c m_stopAtFirstConsistentReconstruction = settings->get_first_value(settingsNamespace + "stopAtFirstConsistentReconstruction", false); m_timeCollaboration = settings->get_first_value(settingsNamespace + "timeCollaboration", false); - m_relocalisationThread = boost::thread(boost::bind(&CollaborativeComponent::run_relocalisation, this)); + for (int i=0; iget_first_value("globalPosesSpecifier", ""); m_context->get_collaborative_pose_optimiser()->start(globalPosesSpecifier); @@ -53,8 +58,12 @@ CollaborativeComponent::CollaborativeComponent(const CollaborativeContext_Ptr& c CollaborativeComponent::~CollaborativeComponent() { m_stopRelocalisationThread = true; - m_readyToRelocalise.notify_one(); - m_relocalisationThread.join(); + // m_readyToRelocalise.notify_one(); + m_readyToRelocalise.notify_all(); + for (int i=0; i lock(m_mutex); - while(!m_bestCandidate) + // while(!m_bestCandidate) + while (m_bestCandidates.empty()) { m_readyToRelocalise.wait(lock); @@ -370,27 +383,32 @@ void CollaborativeComponent::run_relocalisation() if(m_stopRelocalisationThread) return; } } - - std::cout << "Attempting to relocalise frame " << m_bestCandidate->m_frameIndexJ << " of " << m_bestCandidate->m_sceneJ << " against " << m_bestCandidate->m_sceneI << "..."; + m_mutex.lock(); + auto now_bestCandidate = m_bestCandidates.front(); + m_bestCandidates.pop(); + m_mutex.unlock(); + + std::cout << "Attempting to relocalise frame " << now_bestCandidate->m_frameIndexJ << " of " << now_bestCandidate->m_sceneJ << " against " << now_bestCandidate->m_sceneI << "..."; + // std::cout << "Attempting to relocalise frame " << m_bestCandidate->m_frameIndexJ << " of " << m_bestCandidate->m_sceneJ << " against " << m_bestCandidate->m_sceneI << "..."; // Render synthetic images of the source scene from the relevant pose and copy them across to the GPU for use by the relocaliser. // The synthetic images have the size of the images in the target scene and are generated using the target scene's intrinsics. - const SLAMState_CPtr slamStateI = m_context->get_slam_state(m_bestCandidate->m_sceneI); - const SLAMState_CPtr slamStateJ = m_context->get_slam_state(m_bestCandidate->m_sceneJ); + const SLAMState_CPtr slamStateI = m_context->get_slam_state(now_bestCandidate->m_sceneI); + const SLAMState_CPtr slamStateJ = m_context->get_slam_state(now_bestCandidate->m_sceneJ); const View_CPtr viewI = slamStateI->get_view(); ORFloatImage_Ptr depth(new ORFloatImage(slamStateI->get_depth_image_size(), true, true)); ORUChar4Image_Ptr rgb(new ORUChar4Image(slamStateI->get_rgb_image_size(), true, true)); - VoxelRenderState_Ptr& renderStateD = m_depthRenderStates[m_bestCandidate->m_sceneI]; + VoxelRenderState_Ptr& renderStateD = m_depthRenderStates[now_bestCandidate->m_sceneI]; m_visualisationGenerator->generate_depth_from_voxels( - depth, slamStateJ->get_voxel_scene(), m_bestCandidate->m_localPoseJ, viewI->calib.intrinsics_d, + depth, slamStateJ->get_voxel_scene(), now_bestCandidate->m_localPoseJ, viewI->calib.intrinsics_d, renderStateD, DepthVisualiser::DT_ORTHOGRAPHIC ); - VoxelRenderState_Ptr& renderStateRGB = m_rgbRenderStates[m_bestCandidate->m_sceneI]; + VoxelRenderState_Ptr& renderStateRGB = m_rgbRenderStates[now_bestCandidate->m_sceneI]; m_visualisationGenerator->generate_voxel_visualisation( - rgb, slamStateJ->get_voxel_scene(), m_bestCandidate->m_localPoseJ, viewI->calib.intrinsics_rgb, + rgb, slamStateJ->get_voxel_scene(), now_bestCandidate->m_localPoseJ, viewI->calib.intrinsics_rgb, renderStateRGB, VisualisationGenerator::VT_SCENE_COLOUR, boost::none ); @@ -410,12 +428,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. @@ -465,19 +483,20 @@ void CollaborativeComponent::run_relocalisation() #endif // Determine the average depth difference for valid pixels in the source and target depth images. - m_bestCandidate->m_meanDepthDiff = cv::mean(cvMaskedDepthDiff); + now_bestCandidate->m_meanDepthDiff = cv::mean(cvMaskedDepthDiff); #if DEBUGGING - std::cout << "\nMean Depth Difference: " << m_bestCandidate->m_meanDepthDiff << std::endl; + std::cout << "\nMean Depth Difference: " << now_bestCandidate->m_meanDepthDiff << std::endl; #endif // Compute the fraction of the target depth image that is valid. - m_bestCandidate->m_targetValidFraction = static_cast(cv::countNonZero(cvTargetMask == 255)) / (cvTargetMask.size().width * cvTargetMask.size().height); + now_bestCandidate->m_targetValidFraction = static_cast(cv::countNonZero(cvTargetMask == 255)) / (cvTargetMask.size().width * cvTargetMask.size().height); #if DEBUGGING std::cout << "Valid Target Pixels: " << cv::countNonZero(cvTargetMask == 255) << std::endl; #endif // Decide whether or not to verify the relocalisation, based on the average depth difference and the fraction of the target depth image that is valid. - verified = is_verified(*m_bestCandidate); + // verified = is_verified(*m_bestCandidate); + verified = is_verified(*now_bestCandidate); #else // If we didn't build with OpenCV, we can't do any verification, so just mark the relocalisation as verified and hope for the best. verified = true; @@ -489,8 +508,8 @@ void CollaborativeComponent::run_relocalisation() if(verified) { // cjTwi^-1 * cjTwj = wiTcj * cjTwj = wiTwj - m_bestCandidate->m_relativePose = ORUtils::SE3Pose(result->pose.GetInvM() * m_bestCandidate->m_localPoseJ.GetM()); - m_context->get_collaborative_pose_optimiser()->add_relative_transform_sample(m_bestCandidate->m_sceneI, m_bestCandidate->m_sceneJ, *m_bestCandidate->m_relativePose, m_mode); + now_bestCandidate->m_relativePose = ORUtils::SE3Pose(result->pose.GetInvM() * now_bestCandidate->m_localPoseJ.GetM()); + m_context->get_collaborative_pose_optimiser()->add_relative_transform_sample(now_bestCandidate->m_sceneI, now_bestCandidate->m_sceneJ, *now_bestCandidate->m_relativePose, m_mode); std::cout << "succeeded!" << std::endl; #if defined(WITH_OPENCV) && DEBUGGING @@ -512,7 +531,7 @@ void CollaborativeComponent::run_relocalisation() #if DEBUGGING m_results.push_back(*m_bestCandidate); #endif - m_bestCandidate.reset(); + // m_bestCandidate.reset(); } // In live mode, allow a bit of extra time for training before running the next relocalisation. @@ -569,8 +588,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 +621,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()) + 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); } } From 1330f8d783795cd9bcc9d361c55ffaff73b0b994 Mon Sep 17 00:00:00 2001 From: tsm Date: Mon, 31 Oct 2022 18:45:35 +0800 Subject: [PATCH 02/23] fixed bugs in collaborativeComponent --- .../include/spaint/pipelinecomponents/CollaborativeComponent.h | 2 ++ .../spaint/src/pipelinecomponents/CollaborativeComponent.cpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h index 497d1a6..4f1244f 100644 --- a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h +++ b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h @@ -6,6 +6,8 @@ #ifndef H_SPAINT_COLLABORATIVECOMPONENT #define H_SPAINT_COLLABORATIVECOMPONENT +#include + #include #include #include diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index 7785e18..1673387 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -61,7 +61,7 @@ CollaborativeComponent::~CollaborativeComponent() // m_readyToRelocalise.notify_one(); m_readyToRelocalise.notify_all(); for (int i=0; i Date: Mon, 31 Oct 2022 18:49:21 +0800 Subject: [PATCH 03/23] fixed bug --- .../spaint/src/pipelinecomponents/CollaborativeComponent.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index 1673387..93b1fcf 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -622,7 +622,7 @@ void CollaborativeComponent::try_schedule_relocalisation() // Schedule the best candidate for relocalisation. // m_bestCandidate.reset(new CollaborativeRelocalisation(candidates.back())); - auto now_bestCandidate = new CollaborativeRelocalisation(candidates.back()) + auto 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. From c13b695165385ec56ec55b64bbd9030009434879 Mon Sep 17 00:00:00 2001 From: tsm Date: Mon, 31 Oct 2022 18:50:11 +0800 Subject: [PATCH 04/23] fixed bug --- .../spaint/src/pipelinecomponents/CollaborativeComponent.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index 93b1fcf..8294cd4 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -623,7 +623,7 @@ void CollaborativeComponent::try_schedule_relocalisation() // Schedule the best candidate for relocalisation. // m_bestCandidate.reset(new CollaborativeRelocalisation(candidates.back())); auto now_bestCandidate = new CollaborativeRelocalisation(candidates.back()); - m_bestCandidates.push_back(now_bestCandidate); + m_bestCandidates.push(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) From 0a8e71167dbe65907192badcfd8082ea6a5ab3cb Mon Sep 17 00:00:00 2001 From: tsm <18188116781@163.com> Date: Mon, 31 Oct 2022 06:38:05 -0400 Subject: [PATCH 05/23] build success for multi thread --- .../pipelinecomponents/CollaborativeComponent.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index 8294cd4..c8d3725 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -8,6 +8,9 @@ using namespace ITMLib; using namespace ORUtils; using namespace itmx; +#include +#include + #include #include @@ -366,8 +369,8 @@ void CollaborativeComponent::output_results() const void CollaborativeComponent::run_relocalisation() { - auto tid = std::this_thread::get_id(); - std::cout << "this is " << tid << " thread\n"; + int tid = syscall(SYS_gettid); + std::cout << tid << "\n"; while(!m_stopRelocalisationThread) { @@ -388,7 +391,7 @@ void CollaborativeComponent::run_relocalisation() m_bestCandidates.pop(); m_mutex.unlock(); - std::cout << "Attempting to relocalise frame " << now_bestCandidate->m_frameIndexJ << " of " << now_bestCandidate->m_sceneJ << " against " << now_bestCandidate->m_sceneI << "..."; + std::cout << tid <<" : Attempting to relocalise frame " << now_bestCandidate->m_frameIndexJ << " of " << now_bestCandidate->m_sceneJ << " against " << now_bestCandidate->m_sceneI << "..."; // std::cout << "Attempting to relocalise frame " << m_bestCandidate->m_frameIndexJ << " of " << m_bestCandidate->m_sceneJ << " against " << m_bestCandidate->m_sceneI << "..."; // Render synthetic images of the source scene from the relevant pose and copy them across to the GPU for use by the relocaliser. @@ -622,7 +625,8 @@ void CollaborativeComponent::try_schedule_relocalisation() // Schedule the best candidate for relocalisation. // m_bestCandidate.reset(new CollaborativeRelocalisation(candidates.back())); - auto now_bestCandidate = new CollaborativeRelocalisation(candidates.back()); + // auto now_bestCandidate = new CollaborativeRelocalisation(candidates.back()); + boost::shared_ptr now_bestCandidate(new CollaborativeRelocalisation(candidates.back())); m_bestCandidates.push(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. From aa8a165b9709d362afb69bdb702dc22f1d62bf61 Mon Sep 17 00:00:00 2001 From: tsm Date: Tue, 1 Nov 2022 14:44:50 +0800 Subject: [PATCH 06/23] print candidate usecount --- .../spaint/src/pipelinecomponents/CollaborativeComponent.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index c8d3725..35fdcbc 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -534,6 +534,7 @@ void CollaborativeComponent::run_relocalisation() #if DEBUGGING m_results.push_back(*m_bestCandidate); #endif + std::cout << "this is use_count: " << now_bestCandidate.use_count() << "\n"; // m_bestCandidate.reset(); } @@ -626,7 +627,7 @@ void CollaborativeComponent::try_schedule_relocalisation() // Schedule the best candidate for relocalisation. // m_bestCandidate.reset(new CollaborativeRelocalisation(candidates.back())); // auto now_bestCandidate = new CollaborativeRelocalisation(candidates.back()); - boost::shared_ptr now_bestCandidate(new CollaborativeRelocalisation(candidates.back())); + boost::shared_ptr now_bestCandidate(new CollaborativeRelocalisation(candidates.back())); m_bestCandidates.push(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. From bec3d7e48e7074521b2886d5d220342385f999d4 Mon Sep 17 00:00:00 2001 From: tsm <18188116781@163.com> Date: Tue, 1 Nov 2022 02:35:18 -0400 Subject: [PATCH 07/23] add reset for candidate pointer --- .../spaint/src/pipelinecomponents/CollaborativeComponent.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index 35fdcbc..43def1e 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -534,7 +534,7 @@ void CollaborativeComponent::run_relocalisation() #if DEBUGGING m_results.push_back(*m_bestCandidate); #endif - std::cout << "this is use_count: " << now_bestCandidate.use_count() << "\n"; + now_bestCandidate.reset(); // m_bestCandidate.reset(); } From b7d82ed6aeb2be42bd919678c53bc98dc2144281 Mon Sep 17 00:00:00 2001 From: tsm Date: Tue, 1 Nov 2022 15:14:03 +0800 Subject: [PATCH 08/23] change queue to list --- .../spaint/pipelinecomponents/CollaborativeComponent.h | 4 +--- .../spaint/src/pipelinecomponents/CollaborativeComponent.cpp | 4 ++-- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h index 4f1244f..206b06f 100644 --- a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h +++ b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h @@ -6,8 +6,6 @@ #ifndef H_SPAINT_COLLABORATIVECOMPONENT #define H_SPAINT_COLLABORATIVECOMPONENT -#include - #include #include #include @@ -36,7 +34,7 @@ class CollaborativeComponent /** The best relocalisation candidate, as chosen by the scheduler. This will be the next relocalisation attempted. */ boost::shared_ptr m_bestCandidate; - std::queue> m_bestCandidates = std::queue>(); + std::list> m_bestCandidates = std::list>(); /** The timer used to compute the time spent collaborating. */ boost::optional m_collaborationTimer; diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index 43def1e..7083a8c 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -388,7 +388,7 @@ void CollaborativeComponent::run_relocalisation() } m_mutex.lock(); auto now_bestCandidate = m_bestCandidates.front(); - m_bestCandidates.pop(); + m_bestCandidates.pop_front(); m_mutex.unlock(); std::cout << tid <<" : Attempting to relocalise frame " << now_bestCandidate->m_frameIndexJ << " of " << now_bestCandidate->m_sceneJ << " against " << now_bestCandidate->m_sceneI << "..."; @@ -628,7 +628,7 @@ void CollaborativeComponent::try_schedule_relocalisation() // 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(now_bestCandidate); + 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) From 4d167cf1db154089d9afb4d195a3450350b837ab Mon Sep 17 00:00:00 2001 From: tsm <18188116781@163.com> Date: Tue, 1 Nov 2022 04:13:15 -0400 Subject: [PATCH 09/23] inspect some memory part --- .../CollaborativeComponent.h | 4 +- .../CollaborativeComponent.cpp | 40 +++++++++++++++++-- 2 files changed, 38 insertions(+), 6 deletions(-) diff --git a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h index 206b06f..2645616 100644 --- a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h +++ b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h @@ -21,8 +21,8 @@ namespace spaint { -const int relocalisationThreadsCount = 5; -const int bestCandidateMaxCount = 5; +const int relocalisationThreadsCount = 7; +const int bestCandidateMaxCount = 7; /** * \brief An instance of this pipeline component can be used to determine the relative poses between agents participating in collaborative SLAM. diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index 7083a8c..a457bd8 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -24,10 +24,45 @@ using boost::bind; #include using namespace orx; -#include +#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 #################### @@ -390,7 +425,6 @@ void CollaborativeComponent::run_relocalisation() auto now_bestCandidate = m_bestCandidates.front(); m_bestCandidates.pop_front(); m_mutex.unlock(); - std::cout << tid <<" : Attempting to relocalise frame " << now_bestCandidate->m_frameIndexJ << " of " << now_bestCandidate->m_sceneJ << " against " << now_bestCandidate->m_sceneI << "..."; // std::cout << "Attempting to relocalise frame " << m_bestCandidate->m_frameIndexJ << " of " << m_bestCandidate->m_sceneJ << " against " << m_bestCandidate->m_sceneI << "..."; @@ -537,7 +571,6 @@ void CollaborativeComponent::run_relocalisation() 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)); @@ -629,7 +662,6 @@ void CollaborativeComponent::try_schedule_relocalisation() // 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) { From d704a825c4ccdf99ed7600db3a0d5f9e11baa358 Mon Sep 17 00:00:00 2001 From: tsm <18188116781@163.com> Date: Mon, 14 Nov 2022 03:51:32 -0500 Subject: [PATCH 10/23] debug for multi thread --- .../src/ransac/interface/PreemptiveRansac.cpp | 28 +++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/modules/grove/src/ransac/interface/PreemptiveRansac.cpp b/modules/grove/src/ransac/interface/PreemptiveRansac.cpp index d6ba86f..c1375af 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; #ifdef WITH_OPENMP #pragma omp parallel for schedule(dynamic) #endif for(int i = 0; i < nbPoseCandidates; ++i) { + int tid = syscall(SYS_gettid); + std::cout << "break 1\n"; + m.lock(); + threadCnt.insert(tid); + m.unlock(); + // std::cout << tid << ": in update_candidate_poses openmp\n"; update_candidate_pose(i); } + std::cout << "this is cnt size in update_candidate_poses: " < PreemptiveRansac::estimate_pose(const Keypoint3DColourImage_CPtr& keypointsImage, const ScorePredictionsImage_CPtr& predictionsImage) @@ -322,7 +339,8 @@ void PreemptiveRansac::compute_candidate_poses_kabsch() #if 0 std::cout << "Generated " << nbPoseCandidates << " candidates." << std::endl; #endif - + boost::mutex m; + std::unordered_set threadCnt{}; // For each candidate: #ifdef WITH_OPENMP #pragma omp parallel for @@ -336,13 +354,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 << "this is cnt size in compute_candidate_poses_kabsch: " << threadCnt.size() <<"\n"; + sleep(35); } void PreemptiveRansac::reset_inliers(bool resetMask) From 73b16875d1e21e50126307c8142109d632dc86c8 Mon Sep 17 00:00:00 2001 From: tsm <18188116781@163.com> Date: Tue, 15 Nov 2022 00:34:18 -0500 Subject: [PATCH 11/23] parallelCandidate debug --- modules/grove/src/ransac/interface/PreemptiveRansac.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/modules/grove/src/ransac/interface/PreemptiveRansac.cpp b/modules/grove/src/ransac/interface/PreemptiveRansac.cpp index c1375af..6e2fc8a 100644 --- a/modules/grove/src/ransac/interface/PreemptiveRansac.cpp +++ b/modules/grove/src/ransac/interface/PreemptiveRansac.cpp @@ -134,20 +134,20 @@ void PreemptiveRansac::update_candidate_poses() std::unordered_set threadCnt{}; boost::mutex m; + int ttid = syscall(SYS_gettid); #ifdef WITH_OPENMP #pragma omp parallel for schedule(dynamic) #endif for(int i = 0; i < nbPoseCandidates; ++i) { int tid = syscall(SYS_gettid); - std::cout << "break 1\n"; m.lock(); threadCnt.insert(tid); m.unlock(); // std::cout << tid << ": in update_candidate_poses openmp\n"; update_candidate_pose(i); } - std::cout << "this is cnt size in update_candidate_poses: " < threadCnt{}; + int ttid = syscall(SYS_gettid); // For each candidate: #ifdef WITH_OPENMP #pragma omp parallel for @@ -365,7 +366,7 @@ void PreemptiveRansac::compute_candidate_poses_kabsch() // 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 << "this is cnt size in compute_candidate_poses_kabsch: " << threadCnt.size() <<"\n"; + std::cout << ttid << ": this is cnt size in compute_candidate_poses_kabsch: " << threadCnt.size() <<"\n"; sleep(35); } From 39eacfca03b1e7848354d1e4b0f14685f4716763 Mon Sep 17 00:00:00 2001 From: tsm <18188116781@163.com> Date: Mon, 21 Nov 2022 01:15:33 -0500 Subject: [PATCH 12/23] change relocaliser number --- .../src/ransac/interface/PreemptiveRansac.cpp | 18 +++++++++--------- .../interface/ScoreRelocaliser.cpp | 7 +++++-- .../CollaborativeComponent.h | 4 ++-- .../CollaborativeComponent.cpp | 1 + 4 files changed, 17 insertions(+), 13 deletions(-) diff --git a/modules/grove/src/ransac/interface/PreemptiveRansac.cpp b/modules/grove/src/ransac/interface/PreemptiveRansac.cpp index 6e2fc8a..d88ef85 100644 --- a/modules/grove/src/ransac/interface/PreemptiveRansac.cpp +++ b/modules/grove/src/ransac/interface/PreemptiveRansac.cpp @@ -141,14 +141,14 @@ void PreemptiveRansac::update_candidate_poses() for(int i = 0; i < nbPoseCandidates; ++i) { int tid = syscall(SYS_gettid); - m.lock(); - threadCnt.insert(tid); - m.unlock(); + // 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: " <(candidate.pointsCamera[i].v); worldPoints.col(i) = Eigen::Map(candidate.pointsWorld[i].v); @@ -367,7 +367,7 @@ void PreemptiveRansac::compute_candidate_poses_kabsch() 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); + //sleep(35); } void PreemptiveRansac::reset_inliers(bool resetMask) diff --git a/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp b/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp index 9c0204e..b5f2296 100644 --- a/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp +++ b/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp @@ -8,6 +8,8 @@ using namespace ORUtils; using namespace tvgutil; #include +#include +#include namespace bf = boost::filesystem; #include @@ -129,6 +131,7 @@ std::vector ScoreRelocaliser::relocalise(const ORUChar4Imag std::vector results; + int tid = syscall(SYS_gettid); // Iff we have enough valid depth values, try to estimate the camera pose: if(m_preemptiveRansac->count_valid_depths(depthImage) > m_preemptiveRansac->get_min_nb_required_points()) { @@ -138,10 +141,10 @@ std::vector ScoreRelocaliser::relocalise(const ORUChar4Imag // Step 2: Create a single SCoRe prediction (a single set of clusters) for each keypoint. make_predictions(colourImage); - + std::cout << tid << ": b9 after make_predictions\n"; // Step 3: Perform P-RANSAC to try to estimate the camera pose. boost::optional poseCandidate = m_preemptiveRansac->estimate_pose(m_keypointsImage, m_predictionsImage); - + std::cout << tid << ": b10 after estimate_pose\n"; // Step 4: If we succeeded in estimating a camera pose: if(poseCandidate) { diff --git a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h index 2645616..d91087a 100644 --- a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h +++ b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h @@ -21,8 +21,8 @@ namespace spaint { -const int relocalisationThreadsCount = 7; -const int bestCandidateMaxCount = 7; +const int relocalisationThreadsCount = 3; +const int bestCandidateMaxCount = 3; /** * \brief An instance of this pipeline component can be used to determine the relative poses between agents participating in collaborative SLAM. diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index a457bd8..e642949 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -466,6 +466,7 @@ void CollaborativeComponent::run_relocalisation() // Attempt to relocalise the synthetic images using the relocaliser for the target scene. Relocaliser_CPtr relocaliserI = m_context->get_relocaliser(now_bestCandidate->m_sceneI); + std::cout << "b5\n"; std::vector results = relocaliserI->relocalise(rgb.get(), depth.get(), now_bestCandidate->m_depthIntrinsicsI); boost::optional result = results.empty() ? boost::none : boost::optional(results[0]); From aacf2adc7219937bc0fef858dfa7741ef390c3d5 Mon Sep 17 00:00:00 2001 From: tsm <18188116781@163.com> Date: Mon, 21 Nov 2022 04:54:12 -0500 Subject: [PATCH 13/23] evaluate time cost in relocaliser --- .../src/ransac/interface/PreemptiveRansac.cpp | 4 +- .../interface/ScoreRelocaliser.cpp | 54 ++++++++++++++++--- .../CollaborativeComponent.h | 4 +- tmp | 47 ++++++++++++++++ 4 files changed, 97 insertions(+), 12 deletions(-) create mode 100644 tmp diff --git a/modules/grove/src/ransac/interface/PreemptiveRansac.cpp b/modules/grove/src/ransac/interface/PreemptiveRansac.cpp index d88ef85..b00bc59 100644 --- a/modules/grove/src/ransac/interface/PreemptiveRansac.cpp +++ b/modules/grove/src/ransac/interface/PreemptiveRansac.cpp @@ -147,7 +147,7 @@ void PreemptiveRansac::update_candidate_poses() // std::cout << tid << ": in update_candidate_poses openmp\n"; update_candidate_pose(i); } - std::cout << ttid << ": this is cnt size in update_candidate_poses: " < 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"; + // std::cout << ttid << ": this is cnt size in compute_candidate_poses_kabsch: " << threadCnt.size() <<"\n"; //sleep(35); } diff --git a/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp b/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp index b5f2296..cf2523e 100644 --- a/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp +++ b/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp @@ -10,6 +10,7 @@ using namespace tvgutil; #include #include #include +#include namespace bf = boost::filesystem; #include @@ -70,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 #################### @@ -125,26 +143,41 @@ 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); std::vector results; - int tid = syscall(SYS_gettid); // Iff we have enough valid depth values, try to estimate the camera pose: if(m_preemptiveRansac->count_valid_depths(depthImage) > m_preemptiveRansac->get_min_nb_required_points()) { // 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); - std::cout << tid << ": b9 after make_predictions\n"; + 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); - std::cout << tid << ": b10 after estimate_pose\n"; + 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) { @@ -160,8 +193,13 @@ std::vector ScoreRelocaliser::relocalise(const ORUChar4Imag { // Get all of the candidates that survived the initial culling process during P-RANSAC. std::vector candidates; - m_preemptiveRansac->get_best_poses(candidates); - + totalGetBestPose++; + gettimeofday(&t0,NULL); + m_preemptiveRansac->get_best_poses(candidates); + gettimeofday(&t1,NULL); + t_cost = (t1.tv_sec - t0.tv_sec)*1000.0 + (double)(t1.tv_usec - t0.tv_usec)/1000.0; + std::cout << "4. get_best_poses time cost: " << t_cost << "ms\n"; + totalGetBestPoset += t_cost; // Add the best candidates to the results (skipping the first one, since it's the same one returned by estimate_pose above). const size_t maxElements = std::min(candidates.size(), m_maxRelocalisationsToOutput); for(size_t i = 1; i < maxElements; ++i) diff --git a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h index d91087a..47f07c7 100644 --- a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h +++ b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h @@ -21,8 +21,8 @@ namespace spaint { -const int relocalisationThreadsCount = 3; -const int bestCandidateMaxCount = 3; +const int relocalisationThreadsCount = 2; +const int bestCandidateMaxCount = 2; /** * \brief An instance of this pipeline component can be used to determine the relative poses between agents participating in collaborative SLAM. 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; From 028d43d99e0a6b89c47aeaa10a4d8762e8ee7b9b Mon Sep 17 00:00:00 2001 From: tsm <18188116781@163.com> Date: Tue, 22 Nov 2022 21:42:32 -0500 Subject: [PATCH 14/23] change args --- modules/grove/src/ransac/interface/PreemptiveRansac.cpp | 4 ++-- .../grove/src/relocalisation/interface/ScoreRelocaliser.cpp | 6 +++--- .../spaint/pipelinecomponents/CollaborativeComponent.h | 4 ++-- .../src/pipelinecomponents/CollaborativeComponent.cpp | 3 ++- 4 files changed, 9 insertions(+), 8 deletions(-) diff --git a/modules/grove/src/ransac/interface/PreemptiveRansac.cpp b/modules/grove/src/ransac/interface/PreemptiveRansac.cpp index b00bc59..423c6b4 100644 --- a/modules/grove/src/ransac/interface/PreemptiveRansac.cpp +++ b/modules/grove/src/ransac/interface/PreemptiveRansac.cpp @@ -136,7 +136,7 @@ void PreemptiveRansac::update_candidate_poses() 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) { @@ -344,7 +344,7 @@ void PreemptiveRansac::compute_candidate_poses_kabsch() 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) { diff --git a/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp b/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp index cf2523e..a3f5f09 100644 --- a/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp +++ b/modules/grove/src/relocalisation/interface/ScoreRelocaliser.cpp @@ -162,21 +162,21 @@ std::vector ScoreRelocaliser::relocalise(const ORUChar4Imag 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"; + // 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"; + // 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"; + //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) diff --git a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h index 47f07c7..40fff0e 100644 --- a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h +++ b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h @@ -21,8 +21,8 @@ namespace spaint { -const int relocalisationThreadsCount = 2; -const int bestCandidateMaxCount = 2; +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. diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index e642949..93dc3c1 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -80,7 +80,8 @@ 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_timeCollaboration = settings->get_first_value(settingsNamespace + "timeCollaboration", false); + m_timeCollaboration = true; for (int i=0; i Date: Wed, 23 Nov 2022 12:43:49 +0800 Subject: [PATCH 15/23] bind cpu core for relocalisation thread --- .../CollaborativeComponent.h | 6 +++++- .../CollaborativeComponent.cpp | 19 ++++++++++++++++--- 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h index d91087a..e7b41d0 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 @@ -155,7 +158,8 @@ class CollaborativeComponent /** * \brief Runs the relocalisation thread, repeatedly attempting scheduled relocalisations until the collaborative component is destroyed. */ - void run_relocalisation(); + // void run_relocalisation(); + void run_relocalisation(cpu_set_t *mask); /** * \brief Scores all of the specified candidate relocalisations to allow one of them to be chosen for a relocalisation attempt. diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index e642949..a7e8eb6 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -82,8 +82,15 @@ CollaborativeComponent::CollaborativeComponent(const CollaborativeContext_Ptr& c m_stopAtFirstConsistentReconstruction = settings->get_first_value(settingsNamespace + "stopAtFirstConsistentReconstruction", false); m_timeCollaboration = settings->get_first_value(settingsNamespace + "timeCollaboration", false); + int cpuCnt = sysconf(_SC_NPROCESSORS_CONF); + int average = cpuCnt/relocalisationThreadsCount; for (int i=0; i Date: Wed, 23 Nov 2022 00:50:50 -0500 Subject: [PATCH 16/23] add output info --- .../spaint/pipelinecomponents/CollaborativeComponent.h | 6 +++--- .../src/pipelinecomponents/CollaborativeComponent.cpp | 7 ++++--- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h index e7b41d0..899fea7 100644 --- a/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h +++ b/modules/spaint/include/spaint/pipelinecomponents/CollaborativeComponent.h @@ -24,8 +24,8 @@ namespace spaint { -const int relocalisationThreadsCount = 3; -const int bestCandidateMaxCount = 3; +const int relocalisationThreadsCount = 4; +const int bestCandidateMaxCount = 4; /** * \brief An instance of this pipeline component can be used to determine the relative poses between agents participating in collaborative SLAM. @@ -159,7 +159,7 @@ class CollaborativeComponent * \brief Runs the relocalisation thread, repeatedly attempting scheduled relocalisations until the collaborative component is destroyed. */ // void run_relocalisation(); - void run_relocalisation(cpu_set_t *mask); + void run_relocalisation(cpu_set_t mask); /** * \brief Scores all of the specified candidate relocalisations to allow one of them to be chosen for a relocalisation attempt. diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index a7e8eb6..ff1f2f9 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -87,10 +87,11 @@ CollaborativeComponent::CollaborativeComponent(const CollaborativeContext_Ptr& c for (int i=0; i Date: Sun, 8 Jan 2023 02:44:16 -0500 Subject: [PATCH 17/23] fix cout mask bug --- .../spaint/src/pipelinecomponents/CollaborativeComponent.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index 374f090..366725a 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -420,7 +420,7 @@ void CollaborativeComponent::run_relocalisation(cpu_set_t mask) if (sched_setaffinity(tid, sizeof(mask), &mask) < 0) { std::cout << "set thread affinity failed\n"; }else { - std::cout << "set " << tid << "to cpu set: " << mask << "\n"; + std::cout << "set " << tid << "to cpu set \n"; } while(!m_stopRelocalisationThread) { From 017b532f94072b61d7cd4e517b3055645c996900 Mon Sep 17 00:00:00 2001 From: tsm Date: Thu, 2 Feb 2023 14:38:04 +0800 Subject: [PATCH 18/23] renew processVoxel --- .gitignore | 1 + apps/spaintgui/Application.cpp | 15 +- apps/spaintgui/Application.h | 3 + apps/spaintgui/core/CollaborativePipeline.cpp | 20 - apps/spaintgui/main.cpp | 527 +++++++++--------- .../CollaborativeComponent.cpp | 18 +- 6 files changed, 272 insertions(+), 312 deletions(-) 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/apps/spaintgui/Application.cpp b/apps/spaintgui/Application.cpp index 9caa1b9..3ac07b5 100644 --- a/apps/spaintgui/Application.cpp +++ b/apps/spaintgui/Application.cpp @@ -51,8 +51,9 @@ using namespace tvgutil; //#################### CONSTRUCTORS #################### -Application::Application(const MultiScenePipeline_Ptr& pipeline, bool renderFiducials) -: m_activeSubwindowIndex(0), +Application::Application(const MultiScenePipeline_Ptr& pipeline, bool renderFiducials, std::string name) +: m_name(name), + m_activeSubwindowIndex(0), m_batchModeEnabled(false), m_commandManager(10), m_pauseBetweenFrames(true), @@ -141,7 +142,9 @@ bool Application::run() if(m_saveMeshOnExit) save_mesh(); // If desired, save a model of each scene before the application terminates. - if(m_saveModelsOnExit) save_models(); + // if(m_saveModelsOnExit) save_models(); + // set save model as default to save voxel scene for relocalisation later + save_models(); return true; } @@ -1033,12 +1036,14 @@ void Application::save_mesh() const void Application::save_models() const { // Find the models directory and make sure it exists. - boost::filesystem::path modelsSubdir = find_subdir_from_executable("models"); + // boost::filesystem::path modelsSubdir = find_subdir_from_executable("models"); + boost::filesystem::path modelsSubdir = "./" + m_name; boost::filesystem::create_directories(modelsSubdir); // Determine the directory to use for saving the models, based on either the experiment tag (if specified) or the current timestamp (otherwise). const Settings_CPtr& settings = m_pipeline->get_model()->get_settings(); - std::string modelName = settings->get_first_value("experimentTag", TimeUtil::get_iso_timestamp()); + // std::string modelName = settings->get_first_value("experimentTag", TimeUtil::get_iso_timestamp()); + std::string modelName = m_name; boost::filesystem::path outputDir = modelsSubdir / modelName; // Save the models to disk. diff --git a/apps/spaintgui/Application.h b/apps/spaintgui/Application.h index 6599b78..c199709 100644 --- a/apps/spaintgui/Application.h +++ b/apps/spaintgui/Application.h @@ -45,6 +45,9 @@ class Application //#################### PRIVATE VARIABLES #################### private: + /** Save model and relocalisation to this file. */ + std::string m_name; + /** The index of the sub-window with which the user is interacting. */ size_t m_activeSubwindowIndex; diff --git a/apps/spaintgui/core/CollaborativePipeline.cpp b/apps/spaintgui/core/CollaborativePipeline.cpp index 7e651f0..95c32ae 100644 --- a/apps/spaintgui/core/CollaborativePipeline.cpp +++ b/apps/spaintgui/core/CollaborativePipeline.cpp @@ -51,26 +51,6 @@ CollaborativePipeline::CollaborativePipeline(const Settings_Ptr& settings, const } } - // Finally, we add a collaborative component to handle relocalisation between the different scenes. - /* std::cout << "after SLAMComponent allocate\n"; - for(int i = 0; i < 3; ++i) - { - // Set the GPU as active. - ORcudaSafeCall(cudaSetDevice(i)); - - // Look up its memory usage. - size_t freeMemory, totalMemory; - ORcudaSafeCall(cudaMemGetInfo(&freeMemory, &totalMemory)); - - // Convert the memory usage to MB. - const size_t bytesPerMb = 1024 * 1024; - const size_t freeMb = freeMemory / bytesPerMb; - const size_t usedMb = (totalMemory - freeMemory) / bytesPerMb; - const size_t totalMb = totalMemory / bytesPerMb; - - // Save the memory usage to the output stream. - std::cout << i << " freeMb: " << freeMb << ";" << " usedMb: " << usedMb << ";" << " totalMb: "<< totalMb << "\n"; - }*/ m_collaborativeComponent.reset(new CollaborativeComponent(m_model, collaborationMode)); } diff --git a/apps/spaintgui/main.cpp b/apps/spaintgui/main.cpp index 3ffd613..6cc0ee2 100644 --- a/apps/spaintgui/main.cpp +++ b/apps/spaintgui/main.cpp @@ -10,21 +10,22 @@ #include #include -// Note: This must appear before anything that could include SDL.h, since it includes boost/asio.hpp, a header that has a WinSock conflict with SDL.h. +// Note: This must appear before anything that could include SDL.h, since it includes boost/asio.hpp, a header that has +// a WinSock conflict with SDL.h. #include "Application.h" #if defined(WITH_ARRAYFIRE) && defined(WITH_CUDA) - #ifdef _MSC_VER - // Suppress a VC++ warning that is produced when including ArrayFire headers. - #pragma warning(disable:4275) - #endif +#ifdef _MSC_VER +// Suppress a VC++ warning that is produced when including ArrayFire headers. +#pragma warning(disable : 4275) +#endif - #include +#include - #ifdef _MSC_VER - // Reenable the suppressed warning for the rest of the translation unit. - #pragma warning(default:4275) - #endif +#ifdef _MSC_VER +// Reenable the suppressed warning for the rest of the translation unit. +#pragma warning(default : 4275) +#endif #endif #include @@ -57,8 +58,8 @@ #include "core/CollaborativePipeline.h" #include "core/ObjectivePipeline.h" -#include "core/SemanticPipeline.h" #include "core/SLAMPipeline.h" +#include "core/SemanticPipeline.h" #include "sequences/SpaintSequence.h" using namespace InputSource; @@ -81,6 +82,7 @@ struct CommandLineArguments //~~~~~~~~~~~~~~~~~~~~ PUBLIC VARIABLES ~~~~~~~~~~~~~~~~~~~~ // User-specifiable arguments + std::string name; bool batch; std::string calibrationFilename; bool cameraAfterDisk; @@ -133,7 +135,7 @@ struct CommandLineArguments * * \param settings The settings object. */ - void add_to_settings(const Settings_Ptr& settings) + void add_to_settings(const Settings_Ptr &settings) { std::vector diskTrackerConfigs; for(size_t i = 0, size = sequences.size(); i < size; ++i) @@ -141,50 +143,54 @@ struct CommandLineArguments diskTrackerConfigs.push_back(sequences[i]->make_disk_tracker_config()); } - #define ADD_SETTING(arg) settings->add_value(#arg, boost::lexical_cast(arg)) - #define ADD_SETTINGS(arg) for(size_t i = 0; i < arg.size(); ++i) { settings->add_value(#arg, boost::lexical_cast(arg[i])); } - ADD_SETTING(batch); - ADD_SETTING(calibrationFilename); - ADD_SETTING(collaborationMode); - ADD_SETTINGS(depthImageMasks); - ADD_SETTINGS(depthNoiseSigmas); - ADD_SETTING(detectFiducials); - ADD_SETTINGS(diskTrackerConfigs); - ADD_SETTING(experimentTag); - ADD_SETTING(fiducialDetectorType); - ADD_SETTING(globalPosesSpecifier); - ADD_SETTING(headless); - ADD_SETTING(host); - ADD_SETTINGS(initialFrameNumbers); - ADD_SETTING(leapFiducialID); - ADD_SETTING(mapSurfels); - ADD_SETTINGS(missingDepthFractions); - ADD_SETTING(modelSpecifier); - ADD_SETTING(noRelocaliser); - ADD_SETTING(openNIDeviceURI); - ADD_SETTING(pipelineType); - ADD_SETTING(port); - ADD_SETTINGS(poseFileMasks); - ADD_SETTING(prefetchBufferCapacity); - ADD_SETTING(profileMemory); - ADD_SETTING(relocaliserType); - ADD_SETTING(renderFiducials); - ADD_SETTINGS(rgbImageMasks); - ADD_SETTING(runServer); - ADD_SETTING(saveMeshOnExit); - ADD_SETTING(saveModelsOnExit); - ADD_SETTINGS(semanticImageMasks); - ADD_SETTINGS(sequenceSpecifiers); - ADD_SETTINGS(sequenceTypes); - ADD_SETTING(subwindowConfigurationIndex); - ADD_SETTINGS(trackerSpecifiers); - ADD_SETTING(trackObject); - ADD_SETTING(trackSurfels); - ADD_SETTING(useVicon); - ADD_SETTING(verbose); - ADD_SETTING(viconHost); - #undef ADD_SETTINGS - #undef ADD_SETTING +#define ADD_SETTING(arg) settings->add_value(#arg, boost::lexical_cast(arg)) +#define ADD_SETTINGS(arg) \ + for(size_t i = 0; i < arg.size(); ++i) \ + { \ + settings->add_value(#arg, boost::lexical_cast(arg[i])); \ + } + ADD_SETTING(batch); + ADD_SETTING(calibrationFilename); + ADD_SETTING(collaborationMode); + ADD_SETTINGS(depthImageMasks); + ADD_SETTINGS(depthNoiseSigmas); + ADD_SETTING(detectFiducials); + ADD_SETTINGS(diskTrackerConfigs); + ADD_SETTING(experimentTag); + ADD_SETTING(fiducialDetectorType); + ADD_SETTING(globalPosesSpecifier); + ADD_SETTING(headless); + ADD_SETTING(host); + ADD_SETTINGS(initialFrameNumbers); + ADD_SETTING(leapFiducialID); + ADD_SETTING(mapSurfels); + ADD_SETTINGS(missingDepthFractions); + ADD_SETTING(modelSpecifier); + ADD_SETTING(noRelocaliser); + ADD_SETTING(openNIDeviceURI); + ADD_SETTING(pipelineType); + ADD_SETTING(port); + ADD_SETTINGS(poseFileMasks); + ADD_SETTING(prefetchBufferCapacity); + ADD_SETTING(profileMemory); + ADD_SETTING(relocaliserType); + ADD_SETTING(renderFiducials); + ADD_SETTINGS(rgbImageMasks); + ADD_SETTING(runServer); + ADD_SETTING(saveMeshOnExit); + ADD_SETTING(saveModelsOnExit); + ADD_SETTINGS(semanticImageMasks); + ADD_SETTINGS(sequenceSpecifiers); + ADD_SETTINGS(sequenceTypes); + ADD_SETTING(subwindowConfigurationIndex); + ADD_SETTINGS(trackerSpecifiers); + ADD_SETTING(trackObject); + ADD_SETTING(trackSurfels); + ADD_SETTING(useVicon); + ADD_SETTING(verbose); + ADD_SETTING(viconHost); +#undef ADD_SETTINGS +#undef ADD_SETTING } }; @@ -196,11 +202,11 @@ struct CommandLineArguments * \param parsedOptions The set of parsed options. * \param settings The settings object. */ -void add_unregistered_options_to_settings(const po::parsed_options& parsedOptions, const Settings_Ptr& settings) +void add_unregistered_options_to_settings(const po::parsed_options &parsedOptions, const Settings_Ptr &settings) { for(size_t i = 0, optionCount = parsedOptions.options.size(); i < optionCount; ++i) { - const po::basic_option& option = parsedOptions.options[i]; + const po::basic_option &option = parsedOptions.options[i]; if(option.unregistered) { // Add all the specified values for the option in the correct order. @@ -227,17 +233,20 @@ ImageSourceEngine *check_camera_subengine(ImageSourceEngine *cameraSubengine) delete cameraSubengine; return NULL; } - else return cameraSubengine; + else + return cameraSubengine; } /** - * \brief Copies any (voxel) scene parameters that have been specified in the configuration file across to the actual scene parameters object. + * \brief Copies any (voxel) scene parameters that have been specified in the configuration file across to the actual + * scene parameters object. * * \param settings The settings for the application. */ -void copy_scene_params(const Settings_Ptr& settings) +void copy_scene_params(const Settings_Ptr &settings) { -#define COPY_PARAM(type, name, defaultValue) settings->sceneParams.name = settings->get_first_value("SceneParams."#name, defaultValue) +#define COPY_PARAM(type, name, defaultValue) \ + settings->sceneParams.name = settings->get_first_value("SceneParams." #name, defaultValue) // Note: The default values are taken from InfiniTAM. COPY_PARAM(int, maxW, 100); @@ -251,13 +260,15 @@ void copy_scene_params(const Settings_Ptr& settings) } /** - * \brief Copies any surfel scene parameters that have been specified in the configuration file across to the actual surfel scene parameters object. + * \brief Copies any surfel scene parameters that have been specified in the configuration file across to the actual + * surfel scene parameters object. * * \param settings The settings for the application. */ -void copy_surfel_scene_params(const Settings_Ptr& settings) +void copy_surfel_scene_params(const Settings_Ptr &settings) { -#define COPY_PARAM(type, name, defaultValue) settings->surfelSceneParams.name = settings->get_first_value("SurfelSceneParams."#name, defaultValue) +#define COPY_PARAM(type, name, defaultValue) \ + settings->surfelSceneParams.name = settings->get_first_value("SurfelSceneParams." #name, defaultValue) // Note: The default values are taken from InfiniTAM. COPY_PARAM(float, deltaRadius, 0.5f); @@ -279,12 +290,13 @@ void copy_surfel_scene_params(const Settings_Ptr& settings) } /** - * \brief Determines the sequences (if any) that the user wants to load from disk, based on the program's command-line arguments. + * \brief Determines the sequences (if any) that the user wants to load from disk, based on the program's command-line + * arguments. * * \param args The program's command-line arguments. * \return The sequences (if any) that the user wants to load from disk. */ -std::vector determine_sequences(const CommandLineArguments& args) +std::vector determine_sequences(const CommandLineArguments &args) { std::vector sequences; @@ -301,7 +313,7 @@ std::vector determine_sequences(const CommandLineArguments& args) double missingDepthFraction = i < args.missingDepthFractions.size() ? args.missingDepthFractions[i] : 0.0; float depthNoiseSigma = i < args.depthNoiseSigmas.size() ? args.depthNoiseSigmas[i] : 0.0f; - const std::string& sequenceSpecifier = args.sequenceSpecifiers[i]; + const std::string &sequenceSpecifier = args.sequenceSpecifiers[i]; if(!bf::is_regular_file(sequenceSpecifier)) { // Determine the sequence type. @@ -309,13 +321,16 @@ std::vector determine_sequences(const CommandLineArguments& args) // Determine the directory containing the sequence. bf::path dir = bf::is_directory(sequenceSpecifier) - ? sequenceSpecifier - : find_subdir_from_executable(sequenceType + "s") / sequenceSpecifier; + ? sequenceSpecifier + : find_subdir_from_executable(sequenceType + "s") / sequenceSpecifier; // Add the sequence to the list. - sequences.push_back(Sequence_CPtr(new SpaintSequence(dir, initialFrameNumber, missingDepthFraction, depthNoiseSigma))); + sequences.push_back( + Sequence_CPtr(new SpaintSequence(dir, initialFrameNumber, missingDepthFraction, depthNoiseSigma))); } - else throw std::runtime_error("Error: The sequence specifier '" + sequenceSpecifier + "' denotes a file rather than a directory"); + else + throw std::runtime_error("Error: The sequence specifier '" + sequenceSpecifier + + "' denotes a file rather than a directory"); } // Add any sequence that the user specifies implicitly via depth/RGB/pose masks. @@ -331,7 +346,13 @@ std::vector determine_sequences(const CommandLineArguments& args) double missingDepthFraction = j < args.missingDepthFractions.size() ? args.missingDepthFractions[j] : 0.0; float depthNoiseSigma = j < args.depthNoiseSigmas.size() ? args.depthNoiseSigmas[j] : 0.0f; - sequences.push_back(Sequence_CPtr(new SpaintSequence(depthImageMask, rgbImageMask, poseFileMask, semanticImageMask, initialFrameNumber, missingDepthFraction, depthNoiseSigma))); + sequences.push_back(Sequence_CPtr(new SpaintSequence(depthImageMask, + rgbImageMask, + poseFileMask, + semanticImageMask, + initialFrameNumber, + missingDepthFraction, + depthNoiseSigma))); } return sequences; @@ -343,13 +364,15 @@ std::vector determine_sequences(const CommandLineArguments& args) * \param globalPosesSpecifier The global poses specifier. * \return The global poses from the file, if possible, or an empty map otherwise. */ -std::map load_global_poses(const std::string& globalPosesSpecifier) +std::map load_global_poses(const std::string &globalPosesSpecifier) { - std::map globalPoses; + std::map globalPoses; // Determine the file from which to load the global poses. const std::string dirName = "global_poses"; - const bf::path p = bf::is_regular(globalPosesSpecifier) ? globalPosesSpecifier : find_subdir_from_executable(dirName) / (globalPosesSpecifier + ".txt"); + const bf::path p = bf::is_regular(globalPosesSpecifier) + ? globalPosesSpecifier + : find_subdir_from_executable(dirName) / (globalPosesSpecifier + ".txt"); // Try to read the poses from the file. If we can't, throw. std::ifstream fs(p.string().c_str()); @@ -371,7 +394,7 @@ std::map load_global_poses(const std::string& globalPoses * \param args The program's command-line arguments. * \return The camera subengine, if a suitable camera is attached, or NULL otherwise. */ -ImageSourceEngine *make_camera_subengine(const CommandLineArguments& args) +ImageSourceEngine *make_camera_subengine(const CommandLineArguments &args) { ImageSourceEngine *cameraSubengine = NULL; @@ -380,14 +403,19 @@ ImageSourceEngine *make_camera_subengine(const CommandLineArguments& args) if(cameraSubengine == NULL) { std::cout << "[spaint] Probing OpenNI camera: " << args.openNIDeviceURI << '\n'; - boost::optional uri = args.openNIDeviceURI == "Default" ? boost::none : boost::optional(args.openNIDeviceURI); + boost::optional uri = + args.openNIDeviceURI == "Default" ? boost::none : boost::optional(args.openNIDeviceURI); bool useInternalCalibration = !uri; // if reading from a file, assume that the provided calibration is to be used - cameraSubengine = check_camera_subengine(new OpenNIEngine(args.calibrationFilename.c_str(), uri ? uri->c_str() : NULL, useInternalCalibration + cameraSubengine = check_camera_subengine(new OpenNIEngine( + args.calibrationFilename.c_str(), + uri ? uri->c_str() : NULL, + useInternalCalibration #if USE_LOW_USB_BANDWIDTH_MODE - // If there is insufficient USB bandwidth available to support 640x480 RGB input, use 320x240 instead. - , Vector2i(320, 240) + // If there is insufficient USB bandwidth available to support 640x480 RGB input, use 320x240 instead. + , + Vector2i(320, 240) #endif - )); + )); } #endif @@ -421,11 +449,12 @@ ImageSourceEngine *make_camera_subengine(const CommandLineArguments& args) return cameraSubengine; } -boost::shared_ptr make_image_source_engine(const CommandLineArguments& args) +boost::shared_ptr make_image_source_engine(const CommandLineArguments &args) { boost::shared_ptr imageSourceEngine(new CompositeImageSourceEngine); - // If a model was specified without either a disk sequence or the camera following it, add an idle subengine to allow the model to still be viewed. + // If a model was specified without either a disk sequence or the camera following it, add an idle subengine to allow + // the model to still be viewed. if(args.modelDir && args.sequences.empty() && !args.cameraAfterDisk) { const std::string calibrationFilename = (*args.modelDir / "calib.txt").string(); @@ -439,13 +468,17 @@ boost::shared_ptr make_image_source_engine(const Com // FIXME: It would be better to use the correct calibration file for each disk sequence, but our pipeline doesn't // yet support changing the camera calibration parameters during reconstruction. const bf::path calibrationPath = args.sequences[0]->default_calib_path(); - const std::string calibrationFilename = (args.calibrationFilename != "" || !bf::exists(calibrationPath)) ? args.calibrationFilename : calibrationPath.string(); + const std::string calibrationFilename = (args.calibrationFilename != "" || !bf::exists(calibrationPath)) + ? args.calibrationFilename + : calibrationPath.string(); std::cout << "[spaint] Reading images from disk: " << *args.sequences[i] << '\n'; - imageSourceEngine->addSubengine(new AsyncImageSourceEngine(args.sequences[i]->make_image_source_engine(calibrationFilename), args.prefetchBufferCapacity)); + imageSourceEngine->addSubengine(new AsyncImageSourceEngine( + args.sequences[i]->make_image_source_engine(calibrationFilename), args.prefetchBufferCapacity)); } - // If no model and no disk sequences were specified, or we want to switch to the camera once all the disk sequences finish, add a camera subengine. + // If no model and no disk sequences were specified, or we want to switch to the camera once all the disk sequences + // finish, add a camera subengine. if((!args.modelDir && args.sequences.empty()) || args.cameraAfterDisk) { ImageSourceEngine *cameraSubengine = make_camera_subengine(args); @@ -456,17 +489,18 @@ boost::shared_ptr make_image_source_engine(const Com } /** - * \brief Makes the overall tracker configuration based on any tracker specifiers that were passed in on the command line. + * \brief Makes the overall tracker configuration based on any tracker specifiers that were passed in on the command + * line. * * \param args The program's command-line arguments. * \return The overall tracker configuration. */ -std::string make_tracker_config(const CommandLineArguments& args) +std::string make_tracker_config(const CommandLineArguments &args) { std::string result; // If the user wants to use global poses for the scenes, load them from disk. - std::map globalPoses; + std::map globalPoses; if(args.globalPosesSpecifier != "") globalPoses = load_global_poses(args.globalPosesSpecifier); // Determine the number of different trackers that will be needed. size_t trackerCount = args.sequences.size(); @@ -478,7 +512,8 @@ std::string make_tracker_config(const CommandLineArguments& args) // For each tracker that is needed: for(size_t i = 0; i < trackerCount; ++i) { - // Look to see if the user specified an explicit tracker specifier for it on the command line; if not, use a default tracker specifier. + // Look to see if the user specified an explicit tracker specifier for it on the command line; if not, use a default + // tracker specifier. const std::string trackerSpecifier = i < args.trackerSpecifiers.size() ? args.trackerSpecifiers[i] : "InfiniTAM"; // Separate the tracker specifier into chunks. typedef boost::char_separator sep; @@ -506,19 +541,23 @@ std::string make_tracker_config(const CommandLineArguments& args) // Try to find the global pose for this scene based on the sequence ID. const std::string sequenceID = args.sequences[i]->id(); - std::map::const_iterator it = globalPoses.find(sequenceID); + std::map::const_iterator it = globalPoses.find(sequenceID); // If that doesn't work, try to find the global pose based on the scene ID. if(it == globalPoses.end()) { - // FIXME: We shouldn't hard-code "Local" here - it's based on knowing how CollaborativePipeline assigns scene names. - const std::string sceneID = i == 0 ? Model::get_world_scene_id() : "Local" + boost::lexical_cast(i); + // FIXME: We shouldn't hard-code "Local" here - it's based on knowing how CollaborativePipeline assigns + // scene names. + const std::string sceneID = + i == 0 ? Model::get_world_scene_id() : "Local" + boost::lexical_cast(i); it = globalPoses.find(sceneID); } // If we now have a global pose, specify the creation of a global tracker that uses it. If not, throw. - if(it != globalPoses.end()) result += "" + boost::lexical_cast(it->second) + ""; - else throw std::runtime_error("Error: Global pose for sequence '" + sequenceID + "' not found"); + if(it != globalPoses.end()) + result += "" + boost::lexical_cast(it->second) + ""; + else + throw std::runtime_error("Error: Global pose for sequence '" + sequenceID + "' not found"); } // Specify the creation of a disk-based tracker that reads poses from disk. @@ -551,7 +590,10 @@ std::string make_tracker_config(const CommandLineArguments& args) * \param vm The variables map for the application. * \param settings The settings for the application. */ -void parse_configuration_file(const std::string& filename, const po::options_description& options, po::variables_map& vm, const Settings_Ptr& settings) +void parse_configuration_file(const std::string &filename, + const po::options_description &options, + po::variables_map &vm, + const Settings_Ptr &settings) { // Parse the options in the configuration file. po::parsed_options parsedConfigFileOptions = po::parse_config_file(filename.c_str(), options, true); @@ -571,15 +613,22 @@ void parse_configuration_file(const std::string& filename, const po::options_des * \param vm The variables map for the application. * \param settings The settings for the application. */ -void postprocess_arguments(CommandLineArguments& args, const po::options_description& options, po::variables_map& vm, const Settings_Ptr& settings) +void postprocess_arguments(CommandLineArguments &args, + const po::options_description &options, + po::variables_map &vm, + const Settings_Ptr &settings) { - // Determine the sequences (if any) that the user wants to load from disk, based on the program's command-line arguments. + // Determine the sequences (if any) that the user wants to load from disk, based on the program's command-line + // arguments. args.sequences = determine_sequences(args); - // If the user specified a model to load, determine the model directory and parse the model's configuration file (if present). + // If the user specified a model to load, determine the model directory and parse the model's configuration file (if + // present). if(args.modelSpecifier != "") { - args.modelDir = bf::is_directory(args.modelSpecifier) ? args.modelSpecifier : find_subdir_from_executable("models") / args.modelSpecifier / Model::get_world_scene_id(); + args.modelDir = bf::is_directory(args.modelSpecifier) + ? args.modelSpecifier + : find_subdir_from_executable("models") / args.modelSpecifier / Model::get_world_scene_id(); const bf::path configPath = *args.modelDir / "settings.ini"; if(bf::is_regular_file(configPath)) @@ -590,7 +639,8 @@ void postprocess_arguments(CommandLineArguments& args, const po::options_descrip } } - // If the user wants to use global poses for the scenes, make sure that each disk sequence has a tracker specifier set to Disk. + // If the user wants to use global poses for the scenes, make sure that each disk sequence has a tracker specifier set + // to Disk. if(args.globalPosesSpecifier != "") { args.trackerSpecifiers.resize(args.sequenceSpecifiers.size()); @@ -614,7 +664,8 @@ void postprocess_arguments(CommandLineArguments& args, const po::options_descrip // (there is no way to control the application without the UI anyway). if(args.headless) args.batch = true; - // If the user wants to use a Vicon fiducial detector or a Vicon-based tracker, make sure that the Vicon system it needs is enabled. + // If the user wants to use a Vicon fiducial detector or a Vicon-based tracker, make sure that the Vicon system it + // needs is enabled. if(args.fiducialDetectorType == "vicon") { args.useVicon = true; @@ -660,65 +711,80 @@ void postprocess_arguments(CommandLineArguments& args, const po::options_descrip * \param settings The application settings. * \return true, if the program should continue after parsing the command-line arguments, or false otherwise. */ -bool parse_command_line(int argc, char *argv[], CommandLineArguments& args, const Settings_Ptr& settings) +bool parse_command_line(int argc, char *argv[], CommandLineArguments &args, const Settings_Ptr &settings) { // Specify the possible options. po::options_description genericOptions("Generic options"); - genericOptions.add_options() - ("help", "produce help message") - ("batch", po::bool_switch(&args.batch), "enable batch mode") - ("calib,c", po::value(&args.calibrationFilename)->default_value(""), "calibration filename") - ("cameraAfterDisk", po::bool_switch(&args.cameraAfterDisk), "switch to the camera after a disk sequence") - ("collaborationMode", po::value(&args.collaborationMode)->default_value("batch"), "collaboration mode (batch|live)") - ("configFile,f", po::value(), "additional parameters filename") - ("depthNoiseSigma", po::value >(&args.depthNoiseSigmas)->multitoken(), "depth noise sigma") - ("detectFiducials", po::bool_switch(&args.detectFiducials), "enable fiducial detection") - ("experimentTag", po::value(&args.experimentTag)->default_value(Settings::NOT_SET), "experiment tag") - ("fiducialDetectorType", po::value(&args.fiducialDetectorType)->default_value("aruco"), "fiducial detector type (aruco|vicon)") - ("globalPosesSpecifier,g", po::value(&args.globalPosesSpecifier)->default_value(""), "global poses specifier") - ("headless", po::bool_switch(&args.headless), "run in headless mode") - ("host,h", po::value(&args.host)->default_value(""), "remote mapping host") - ("leapFiducialID", po::value(&args.leapFiducialID)->default_value(""), "the ID of the fiducial to use for the Leap Motion") - ("mapSurfels", po::bool_switch(&args.mapSurfels), "enable surfel mapping") - ("missingDepthFraction", po::value >(&args.missingDepthFractions)->multitoken(), "missing depth fraction [0,1]") - ("modelSpecifier,m", po::value(&args.modelSpecifier)->default_value(""), "model specifier") - ("noRelocaliser", po::bool_switch(&args.noRelocaliser), "don't use the relocaliser") - ("pipelineType", po::value(&args.pipelineType)->default_value("semantic"), "pipeline type") - ("port", po::value(&args.port)->default_value("7851"), "remote mapping port") - ("profileMemory", po::bool_switch(&args.profileMemory)->default_value(false), "whether or not to profile the memory usage") - ("relocaliserType", po::value(&args.relocaliserType)->default_value("forest"), "relocaliser type") - ("renderFiducials", po::bool_switch(&args.renderFiducials), "enable fiducial rendering") - ("runServer", po::bool_switch(&args.runServer), "run a remote mapping server") - ("saveMeshOnExit", po::bool_switch(&args.saveMeshOnExit), "save a mesh of the scene on exiting the application") - ("saveModelsOnExit", po::bool_switch(&args.saveModelsOnExit), "save a model of each voxel scene on exiting the application") - ("subwindowConfigurationIndex", po::value(&args.subwindowConfigurationIndex)->default_value("1"), "subwindow configuration index") - ("trackerSpecifier,t", po::value >(&args.trackerSpecifiers)->multitoken(), "tracker specifier") - ("trackSurfels", po::bool_switch(&args.trackSurfels), "enable surfel mapping and tracking") - ("useVicon", po::bool_switch(&args.useVicon)->default_value(false), "whether or not to use the Vicon system") - ("verbose,v", po::bool_switch(&args.verbose), "enable verbose output") - ("viconHost", po::value(&args.viconHost)->default_value("192.168.0.101"), "Vicon host") - ; + genericOptions.add_options()("help", + "produce help message")("batch", po::bool_switch(&args.batch), "enable batch mode")( + "calib,c", po::value(&args.calibrationFilename)->default_value(""), "calibration filename")( + "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 +829,7 @@ bool parse_command_line(int argc, char *argv[], CommandLineArguments& args, cons * \param message The error message. * \param code The exit code. */ -void quit(const std::string& message, int code = EXIT_FAILURE) +void quit(const std::string &message, int code = EXIT_FAILURE) { std::cerr << message << '\n'; SDL_Quit(); @@ -779,13 +845,13 @@ try Settings_Ptr settings(new Settings); settings->trackerConfig = NULL; - // Parse the command-line arguments. + // Parse the command-line arguments. (set headless mode as default) CommandLineArguments args; if(!parse_command_line(argc, argv, args, settings)) { return 0; } - + // If we're not running in headless mode, initialise the GUI-only subsystems. if(!args.headless) { @@ -795,15 +861,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 +893,8 @@ try afcu::setNativeId(0); #endif - // Copy any scene parameters that have been set in the configuration file across to the actual scene parameters objects. + // Copy any scene parameters that have been set in the configuration file across to the actual scene parameters + // objects. copy_scene_params(settings); copy_surfel_scene_params(settings); @@ -841,123 +908,41 @@ try MappingServer_Ptr mappingServer; if(args.runServer) { - const MappingServer::Mode mode = args.pipelineType == "collaborative" ? MappingServer::SM_MULTI_CLIENT : MappingServer::SM_SINGLE_CLIENT; + const MappingServer::Mode mode = + args.pipelineType == "collaborative" ? MappingServer::SM_MULTI_CLIENT : MappingServer::SM_SINGLE_CLIENT; mappingServer.reset(new MappingServer(mode)); mappingServer->start(); } // Construct the pipeline. MultiScenePipeline_Ptr pipeline; - if(args.pipelineType != "collaborative") - { - const size_t maxLabelCount = 10; - SLAMComponent::MappingMode mappingMode = args.mapSurfels ? SLAMComponent::MAP_BOTH : SLAMComponent::MAP_VOXELS_ONLY; - SLAMComponent::TrackingMode trackingMode = args.trackSurfels ? SLAMComponent::TRACK_SURFELS : SLAMComponent::TRACK_VOXELS; - - if(args.pipelineType == "slam") - { - pipeline.reset(new SLAMPipeline( - settings, - Application::resources_dir().string(), - make_image_source_engine(args), - make_tracker_config(args), - mappingMode, - trackingMode, - args.modelDir, - args.detectFiducials - )); - } - else if(args.pipelineType == "semantic") - { - const unsigned int seed = 12345; - pipeline.reset(new SemanticPipeline( - settings, - Application::resources_dir().string(), - maxLabelCount, - make_image_source_engine(args), - seed, - make_tracker_config(args), - mappingMode, - trackingMode, - args.modelDir, - args.detectFiducials - )); - } - else if(args.pipelineType == "objective") - { - pipeline.reset(new ObjectivePipeline( - settings, - Application::resources_dir().string(), - maxLabelCount, - make_image_source_engine(args), - make_tracker_config(args), - mappingMode, - trackingMode, - args.detectFiducials, - !args.trackObject - )); - } - else throw std::runtime_error("Unknown pipeline type: " + args.pipelineType); - } - else - { - // Set a reasonable default for the voxel size (this can be overridden using a configuration file). - if(!settings->has_values("SceneParams.voxelSize")) - { - settings->sceneParams.voxelSize = 0.015f; - settings->sceneParams.mu = settings->sceneParams.voxelSize * 4; - } - - // Set up the image source engines, mapping modes, tracking modes and tracker configurations. - std::vector imageSourceEngines; - std::vector mappingModes; - std::vector trackingModes; - std::vector trackerConfigs; - - // Add an image source engine for each disk sequence specified. - for(size_t i = 0; i < args.sequences.size(); ++i) - { - const bf::path calibrationPath = args.sequences[i]->default_calib_path(); - const std::string calibrationFilename = bf::exists(calibrationPath) ? calibrationPath.string() : args.calibrationFilename; - - std::cout << "[spaint] Adding local agent for disk sequence: " << *args.sequences[i] << '\n'; - CompositeImageSourceEngine_Ptr imageSourceEngine(new CompositeImageSourceEngine); - imageSourceEngine->addSubengine(new AsyncImageSourceEngine(args.sequences[i]->make_image_source_engine(calibrationFilename), args.prefetchBufferCapacity)); - - imageSourceEngines.push_back(imageSourceEngine); - } - - // Set up the mapping modes, tracking modes and tracker configurations. - for(size_t i = 0, size = imageSourceEngines.size(); i < size; ++i) - { - mappingModes.push_back(args.mapSurfels ? SLAMComponent::MAP_BOTH : SLAMComponent::MAP_VOXELS_ONLY); - trackingModes.push_back(args.trackSurfels ? SLAMComponent::TRACK_SURFELS : SLAMComponent::TRACK_VOXELS); - - // FIXME: We don't always want to read the poses from disk - make it possible to run the normal tracker instead. - trackerConfigs.push_back(args.sequences[i]->make_disk_tracker_config()); - } - - // Construct the pipeline itself. - const CollaborationMode collaborationMode = args.collaborationMode == "batch" ? CM_BATCH : CM_LIVE; - pipeline.reset(new CollaborativePipeline( - settings, - Application::resources_dir().string(), - imageSourceEngines, - trackerConfigs, - mappingModes, - trackingModes, - args.detectFiducials, - mappingServer, - collaborationMode - )); - } + // Set a reasonable default for the voxel size (this can be overridden using a configuration file). + if(!settings->has_values("SceneParams.voxelSize")) + { + settings->sceneParams.voxelSize = 0.015f; + settings->sceneParams.mu = settings->sceneParams.voxelSize * 4; + } + const size_t maxLabelCount = 10; + SLAMComponent::MappingMode mappingMode = args.mapSurfels ? SLAMComponent::MAP_BOTH : SLAMComponent::MAP_VOXELS_ONLY; + SLAMComponent::TrackingMode trackingMode = args.trackSurfels ? SLAMComponent::TRACK_SURFELS : SLAMComponent::TRACK_VOXELS; + pipeline.reset(new SLAMPipeline(settings, + Application::resources_dir().string(), + make_image_source_engine(args), + make_tracker_config(args), + mappingMode, + trackingMode, + args.modelDir, + args.detectFiducials)); // If a remote host was specified, set up a mapping client for the world scene. if(args.host != "") { std::cout << "Setting mapping client for host '" << args.host << "' and port '" << args.port << "'\n"; - const pooled_queue::PoolEmptyStrategy poolEmptyStrategy = settings->get_first_value("MappingClient.poolEmptyStrategy", pooled_queue::PES_DISCARD); - pipeline->set_mapping_client(Model::get_world_scene_id(), MappingClient_Ptr(new MappingClient(args.host, args.port, poolEmptyStrategy))); + const pooled_queue::PoolEmptyStrategy poolEmptyStrategy = + settings->get_first_value("MappingClient.poolEmptyStrategy", + pooled_queue::PES_DISCARD); + pipeline->set_mapping_client(Model::get_world_scene_id(), + MappingClient_Ptr(new MappingClient(args.host, args.port, poolEmptyStrategy))); } #ifdef WITH_LEAP @@ -966,7 +951,7 @@ try #endif // Configure and run the application. - Application app(pipeline, args.renderFiducials); + Application app(pipeline, args.renderFiducials, args.name); if(args.batch) app.set_batch_mode_enabled(true); if(args.runServer) app.set_server_mode_enabled(true); app.set_save_memory_usage(args.profileMemory); @@ -980,10 +965,10 @@ try // If we're not running in headless mode, shut down the GUI-only subsystems. if(!args.headless) { - #ifdef WITH_OVR +#ifdef WITH_OVR // If we built with Rift support, shut down the Rift SDK. ovr_Shutdown(); - #endif +#endif // Shut down SDL. SDL_Quit(); @@ -991,7 +976,7 @@ try return runSucceeded ? EXIT_SUCCESS : EXIT_FAILURE; } -catch(std::exception& e) +catch(std::exception &e) { std::cerr << e.what() << '\n'; return EXIT_FAILURE; diff --git a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp index 366725a..99aab89 100644 --- a/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp +++ b/modules/spaint/src/pipelinecomponents/CollaborativeComponent.cpp @@ -80,24 +80,10 @@ 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_timeCollaboration = true; - - int cpuCnt = sysconf(_SC_NPROCESSORS_CONF); - int average = cpuCnt/relocalisationThreadsCount; - for (int i=0; iget_first_value(settingsNamespace + "timeCollaboration", false); const std::string globalPosesSpecifier = settings->get_first_value("globalPosesSpecifier", ""); - m_context->get_collaborative_pose_optimiser()->start(globalPosesSpecifier); + } //#################### DESTRUCTOR #################### From 7fe8081ea78d7a50cf75ae7760303515405e1afe Mon Sep 17 00:00:00 2001 From: tsm <693555980@qq.com> Date: Sun, 5 Feb 2023 15:18:04 +0800 Subject: [PATCH 19/23] reload process voxel --- apps/spaintgui/Application.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/spaintgui/Application.h b/apps/spaintgui/Application.h index c199709..23f33dc 100644 --- a/apps/spaintgui/Application.h +++ b/apps/spaintgui/Application.h @@ -116,7 +116,7 @@ class Application * \param pipeline The multi-scene pipeline that the application should use. * \param renderFiducials Whether or not to render the fiducials (if any) that have been detected in the 3D scene. */ - Application(const MultiScenePipeline_Ptr& pipeline, bool renderFiducials = false); + Application(const MultiScenePipeline_Ptr& pipeline, bool renderFiducials = false, std::string name = ""); //#################### PUBLIC MEMBER FUNCTIONS #################### public: From e7d68e518c7337c441d34de5d781d9a93df7018d Mon Sep 17 00:00:00 2001 From: tsm <693555980@qq.com> Date: Sun, 5 Feb 2023 15:48:27 +0800 Subject: [PATCH 20/23] fixed a bug --- apps/spaintgui/main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/apps/spaintgui/main.cpp b/apps/spaintgui/main.cpp index 6cc0ee2..8f84f50 100644 --- a/apps/spaintgui/main.cpp +++ b/apps/spaintgui/main.cpp @@ -718,6 +718,7 @@ bool parse_command_line(int argc, char *argv[], CommandLineArguments &args, cons genericOptions.add_options()("help", "produce help message")("batch", po::bool_switch(&args.batch), "enable batch mode")( "calib,c", po::value(&args.calibrationFilename)->default_value(""), "calibration filename")( + "name", po::value(&args.name), "sequence name and save directory name")( "cameraAfterDisk", po::bool_switch(&args.cameraAfterDisk), "switch to the camera after a disk sequence")( "collaborationMode", po::value(&args.collaborationMode)->default_value("batch"), @@ -851,7 +852,7 @@ try { return 0; } - + // If we're not running in headless mode, initialise the GUI-only subsystems. if(!args.headless) { @@ -924,7 +925,8 @@ try } 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; pipeline.reset(new SLAMPipeline(settings, Application::resources_dir().string(), make_image_source_engine(args), From 5c2325c2d0aa86792e1877c5d913ef8f3b1ae993 Mon Sep 17 00:00:00 2001 From: tsm <693555980@qq.com> Date: Sun, 5 Feb 2023 16:31:54 +0800 Subject: [PATCH 21/23] fix save dir name --- apps/spaintgui/Application.cpp | 10 +--------- apps/spaintgui/core/MultiScenePipeline.cpp | 2 +- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/apps/spaintgui/Application.cpp b/apps/spaintgui/Application.cpp index 3ac07b5..33d8f20 100644 --- a/apps/spaintgui/Application.cpp +++ b/apps/spaintgui/Application.cpp @@ -1035,16 +1035,8 @@ void Application::save_mesh() const void Application::save_models() const { - // Find the models directory and make sure it exists. - // boost::filesystem::path modelsSubdir = find_subdir_from_executable("models"); - boost::filesystem::path modelsSubdir = "./" + m_name; - boost::filesystem::create_directories(modelsSubdir); - - // Determine the directory to use for saving the models, based on either the experiment tag (if specified) or the current timestamp (otherwise). - const Settings_CPtr& settings = m_pipeline->get_model()->get_settings(); - // std::string modelName = settings->get_first_value("experimentTag", TimeUtil::get_iso_timestamp()); std::string modelName = m_name; - boost::filesystem::path outputDir = modelsSubdir / modelName; + boost::filesystem::path outputDir = modelName; // Save the models to disk. m_pipeline->save_models(outputDir); diff --git a/apps/spaintgui/core/MultiScenePipeline.cpp b/apps/spaintgui/core/MultiScenePipeline.cpp index 46a2611..d61db28 100644 --- a/apps/spaintgui/core/MultiScenePipeline.cpp +++ b/apps/spaintgui/core/MultiScenePipeline.cpp @@ -132,7 +132,7 @@ void MultiScenePipeline::save_models(const bf::path& outputDir) const // Save the models for each scene into a separate subdirectory. for(std::map::const_iterator it = m_slamComponents.begin(), iend = m_slamComponents.end(); it != iend; ++it) { - const bf::path scenePath = outputDir / it->first; + const bf::path scenePath = outputDir / "model"; std::cout << "Saving models for " << it->first << " in: " << scenePath << '\n'; it->second->save_models(scenePath.string()); } From aa846821cb41ad255f056c6ffb153f6cdb288ece Mon Sep 17 00:00:00 2001 From: tsm <693555980@qq.com> Date: Wed, 8 Feb 2023 14:55:29 +0800 Subject: [PATCH 22/23] change randomforest directory --- .../src/relocalisation/interface/ScoreForestRelocaliser.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/modules/grove/src/relocalisation/interface/ScoreForestRelocaliser.cpp b/modules/grove/src/relocalisation/interface/ScoreForestRelocaliser.cpp index 0436a24..293ae57 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").string()); std::cout << "Loading relocalisation forest from: " << modelFilename << '\n'; m_scoreForest = DecisionForestFactory::make_forest(modelFilename, deviceType); } From eecbb211f7e2b4a593101fb1fe567f8d842835c8 Mon Sep 17 00:00:00 2001 From: tsm <693555980@qq.com> Date: Wed, 8 Feb 2023 16:03:52 +0800 Subject: [PATCH 23/23] modify modelFilename --- .../src/relocalisation/interface/ScoreForestRelocaliser.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/grove/src/relocalisation/interface/ScoreForestRelocaliser.cpp b/modules/grove/src/relocalisation/interface/ScoreForestRelocaliser.cpp index 293ae57..00408a1 100644 --- a/modules/grove/src/relocalisation/interface/ScoreForestRelocaliser.cpp +++ b/modules/grove/src/relocalisation/interface/ScoreForestRelocaliser.cpp @@ -38,7 +38,7 @@ 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 = ("randomForest" / "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); }