From 19a8292ad0d0678107a9c9e000b006cb0e9ecb09 Mon Sep 17 00:00:00 2001 From: zeitlerv Date: Sun, 16 Feb 2025 00:39:05 +0100 Subject: [PATCH 01/19] RAM: add entilement com.apple.developer.kernel.increased-memory-limit to increase the available ram for the app. ( Effect on iphone 12 = 3GB ->4GB) (#1446) --- app/ios/RTABMapApp.xcodeproj/project.pbxproj | 4 ++++ app/ios/RTABMapApp/RTABMapApp.entitlements | 8 ++++++++ 2 files changed, 12 insertions(+) create mode 100644 app/ios/RTABMapApp/RTABMapApp.entitlements diff --git a/app/ios/RTABMapApp.xcodeproj/project.pbxproj b/app/ios/RTABMapApp.xcodeproj/project.pbxproj index e2406206..38790c4f 100644 --- a/app/ios/RTABMapApp.xcodeproj/project.pbxproj +++ b/app/ios/RTABMapApp.xcodeproj/project.pbxproj @@ -155,6 +155,7 @@ /* End PBXBuildFile section */ /* Begin PBXFileReference section */ + 44D4681D2D538A4100B094BA /* RTABMapApp.entitlements */ = {isa = PBXFileReference; lastKnownFileType = text.plist.entitlements; name = RTABMapApp.entitlements; path = RTABMapApp/RTABMapApp.entitlements; sourceTree = ""; }; 4E0D83822621F52C00C879AC /* Settings.bundle */ = {isa = PBXFileReference; lastKnownFileType = "wrapper.plug-in"; path = Settings.bundle; sourceTree = ""; }; 4E20B24D266AB94300316EE6 /* Images.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; name = Images.xcassets; path = RTABMapApp/Images.xcassets; sourceTree = SOURCE_ROOT; }; 4E20B24F266AB95600316EE6 /* Images.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; name = Images.xcassets; path = RTABMapApp/Images.xcassets; sourceTree = ""; }; @@ -475,6 +476,7 @@ 4EE01634259BDCC7008CCE65 /* RTABMapApp */ = { isa = PBXGroup; children = ( + 44D4681D2D538A4100B094BA /* RTABMapApp.entitlements */, 4E20B24F266AB95600316EE6 /* Images.xcassets */, 4EFD0BB0259D503200575D88 /* NativeWrapper */, 4EFD0B62259D501E00575D88 /* tango-gl */, @@ -986,6 +988,7 @@ ASSETCATALOG_COMPILER_GLOBAL_ACCENT_COLOR_NAME = AccentColor; CLANG_CXX_LIBRARY = "libc++"; CLANG_USE_OPTIMIZATION_PROFILE = NO; + CODE_SIGN_ENTITLEMENTS = RTABMapApp/RTABMapApp.entitlements; CODE_SIGN_IDENTITY = "Apple Development"; CODE_SIGN_STYLE = Automatic; CURRENT_PROJECT_VERSION = 1; @@ -1043,6 +1046,7 @@ ASSETCATALOG_COMPILER_GLOBAL_ACCENT_COLOR_NAME = AccentColor; CLANG_CXX_LIBRARY = "libc++"; CLANG_USE_OPTIMIZATION_PROFILE = NO; + CODE_SIGN_ENTITLEMENTS = RTABMapApp/RTABMapApp.entitlements; CODE_SIGN_IDENTITY = "Apple Development"; CODE_SIGN_STYLE = Automatic; CURRENT_PROJECT_VERSION = 1; diff --git a/app/ios/RTABMapApp/RTABMapApp.entitlements b/app/ios/RTABMapApp/RTABMapApp.entitlements new file mode 100644 index 00000000..99f47167 --- /dev/null +++ b/app/ios/RTABMapApp/RTABMapApp.entitlements @@ -0,0 +1,8 @@ + + + + + com.apple.developer.kernel.increased-memory-limit + + + From 1cf051d63b807649a3f89c80f36937e5bdd169c6 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 19 Feb 2025 18:39:36 -0800 Subject: [PATCH 02/19] Preferences: fixed Grid/RayTracing not reachable when 3D is unchecked. --- guilib/src/ui/preferencesDialog.ui | 690 ++++++++++++++--------------- 1 file changed, 345 insertions(+), 345 deletions(-) diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 91a45e44..381fed6c 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - 0 + -520 713 4653 @@ -95,7 +95,7 @@ QFrame::Raised - 9 + 15 @@ -13437,90 +13437,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 1 - - - 1000 - - - 5 - - - - - + + - Footprint filtering length (0=disabled). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - m - - - 2 - - - 10.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 - - - - - - - m - - - 1 - - - 100.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 - - - - - - - Noise filtering radius (0=disabled). Done after segmentation. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Maximum obstacles height (0=disabled). - - - true + Resolution (cell size). Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse @@ -13546,105 +13466,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - m - - - 2 - - - 10.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 - - - - - - - m - - - 2 - - - -99999.000000000000000 - - - 0.000000000000000 - - - 0.010000000000000 - - - -0.050000000000000 - - - - - - - Laser scan decimation. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - m - - - 2 - - - 9999.000000000000000 - - - - - - - m - - - 2 - - - 10.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 - - - - - - - Create local occupancy grid maps. This should be enabled to generate a global occupancy grid map. A cloud will be created from the depth image (see option below) or the 3D laser scan. This cloud will be segmented into ground and obstacle cells. For 2D laser scan, the occupancy grid is created using 2D ray casting (only Resolution and 2D Laser Scan related parameters are used). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -13658,45 +13479,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Input cloud is downsampled by voxel filter (voxel size is cell size) before doing segmentation of obstacles and ground. + - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Footprint filtering height (0=disabled). Footprint length and width should be set. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - m - - - 2 - - - 99999.000000000000000 - - - 0.010000000000000 + + false @@ -13723,84 +13512,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - false - - - - - - - Minimum range from the sensor. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Sensor from which the local grid is created. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Footprint filtering width (0=disabled). Footprint length should be set. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - - - - - LiDAR - - - - - Camera(s) - - - - - LiDAR and Camera(s) - - - - @@ -13840,6 +13551,247 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + m + + + 2 + + + 10.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Sensor from which the local grid is created. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 2 + + + 9999.000000000000000 + + + + + + + Create local occupancy grid maps. This should be enabled to generate a global occupancy grid map. A cloud will be created from the depth image (see option below) or the 3D laser scan. This cloud will be segmented into ground and obstacle cells. For 2D laser scan, the occupancy grid is created using 2D ray casting (only Resolution and 2D Laser Scan related parameters are used). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 2 + + + 99999.000000000000000 + + + 0.010000000000000 + + + + + + + Maximum obstacles height (0=disabled). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 2 + + + 10.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + + + + false + + + + + + + m + + + 2 + + + -99999.000000000000000 + + + 0.000000000000000 + + + 0.010000000000000 + + + -0.050000000000000 + + + + + + + m + + + 2 + + + 10.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Footprint filtering width (0=disabled). Footprint length should be set. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Laser scan decimation. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + LiDAR + + + + + Camera(s) + + + + + LiDAR and Camera(s) + + + + + + + + Noise filtering radius (0=disabled). Done after segmentation. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 1000 + + + 5 + + + + + + + Minimum range from the sensor. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -13859,29 +13811,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 1 - - - 100 - - - 1 - - - - - - - Resolution (cell size). - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -13898,6 +13827,100 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Footprint filtering length (0=disabled). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 1 + + + 100.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Footprint filtering height (0=disabled). Footprint length and width should be set. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Input cloud is downsampled by voxel filter (voxel size is cell size) before doing segmentation of obstacles and ground. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 100 + + + 1 + + + + + + + Ray tracing is done for each obstacle, filling unknown space between the sensor and obstacles. If RTAB-Map is not built with OctoMap, 3D ray tracing cannot be done. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + @@ -14176,29 +14199,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Ray tracing is done for each obstacle, filling unknown space between the sensor and obstacles. If RTAB-Map is not built with OctoMap, 3D ray tracing cannot be done. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - From d77a3e3e7cc99d3d2c34f24aed5597f2fa41cc3d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 20 Feb 2025 22:19:42 -0800 Subject: [PATCH 03/19] Update package.xml disabled grid_map_core by default --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index a9835453..8e5f32ae 100644 --- a/package.xml +++ b/package.xml @@ -22,7 +22,7 @@ libsqlite3-dev liboctomap-dev - grid_map_core + qt_gui_cpp zlib From c0e83ce9e9d8418dd2948fe5cf5555a98330a2dd Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 21 Feb 2025 16:33:29 -0800 Subject: [PATCH 04/19] Adding options to bound OdometryF2M keyframes (#1448) * Added OdomF2M/BundleAdjustmentMinMotion and OdomF2M/BundleAdjustmentMaxKeyFramesPerFeature parameters * Added new odom stats localBundleMaxKeyFramesForInlier * Adjusted how localBundleMaxKeyFramesForInlier is computed when OdomF2M/BundleAdjustmentMaxFrames is used. * optimization: Dont compute average pixel distance if parameter is not used --- corelib/include/rtabmap/core/OdometryInfo.h | 6 + corelib/include/rtabmap/core/Parameters.h | 2 + .../rtabmap/core/odometry/OdometryF2M.h | 2 + corelib/src/odometry/OdometryF2M.cpp | 82 ++++- guilib/src/MainWindow.cpp | 4 + guilib/src/PreferencesDialog.cpp | 2 + guilib/src/ui/preferencesDialog.ui | 336 ++++++++++-------- 7 files changed, 285 insertions(+), 149 deletions(-) diff --git a/corelib/include/rtabmap/core/OdometryInfo.h b/corelib/include/rtabmap/core/OdometryInfo.h index 19b3e86c..0b4b5b23 100644 --- a/corelib/include/rtabmap/core/OdometryInfo.h +++ b/corelib/include/rtabmap/core/OdometryInfo.h @@ -49,6 +49,8 @@ public: localBundleOutliers(0), localBundleConstraints(0), localBundleTime(0), + localBundleAvgInlierDistance(0.0f), + localBundleMaxKeyFramesForInlier(0), keyFrameAdded(false), timeDeskewing(0.0f), timeEstimation(0.0f), @@ -76,6 +78,8 @@ public: output.localBundleTime = localBundleTime; output.localBundlePoses = localBundlePoses; output.localBundleModels = localBundleModels; + output.localBundleAvgInlierDistance = localBundleAvgInlierDistance; + output.localBundleMaxKeyFramesForInlier = localBundleMaxKeyFramesForInlier; output.keyFrameAdded = keyFrameAdded; output.timeDeskewing = timeDeskewing; output.timeEstimation = timeEstimation; @@ -106,6 +110,8 @@ public: float localBundleTime; std::map localBundlePoses; std::map > localBundleModels; + float localBundleAvgInlierDistance; + int localBundleMaxKeyFramesForInlier; bool keyFrameAdded; float timeDeskewing; float timeEstimation; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index ffd3abd3..8b4e5336 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -488,6 +488,8 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres."); #endif RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map)."); + RTABMAP_PARAM(OdomF2M, BundleAdjustmentMinMotion, float, 0.0, "To create a new keyframe with bundle adjustment, a minimum motion (in pixels) can be required. The motion is computed by the average distance between inliers of the previous keyframe and new frame."); + RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxKeyFramesPerFeature, int, 0, "Maximum keyframes per feature for bundle adjustment. 0 means not limit."); // Odometry Mono RTABMAP_PARAM(OdomMono, InitMinFlow, float, 100, "Minimum optical flow required for the initialization step."); diff --git a/corelib/include/rtabmap/core/odometry/OdometryF2M.h b/corelib/include/rtabmap/core/odometry/OdometryF2M.h index ce5c1173..feeb4cc1 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryF2M.h +++ b/corelib/include/rtabmap/core/odometry/OdometryF2M.h @@ -69,6 +69,8 @@ private: float scanMapMaxRange_; int bundleAdjustment_; int bundleMaxFrames_; + float bundleMinMotion_; + int bundleMaxKeyFramesPerFeature_; float validDepthRatio_; int pointToPlaneK_; float pointToPlaneRadius_; diff --git a/corelib/src/odometry/OdometryF2M.cpp b/corelib/src/odometry/OdometryF2M.cpp index 6239a8d0..73e4961f 100644 --- a/corelib/src/odometry/OdometryF2M.cpp +++ b/corelib/src/odometry/OdometryF2M.cpp @@ -67,6 +67,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) : scanMapMaxRange_(Parameters::defaultOdomF2MScanRange()), bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()), bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()), + bundleMinMotion_(Parameters::defaultOdomF2MBundleAdjustmentMinMotion()), + bundleMaxKeyFramesPerFeature_(Parameters::defaultOdomF2MBundleAdjustmentMaxKeyFramesPerFeature()), validDepthRatio_(Parameters::defaultOdomF2MValidDepthRatio()), pointToPlaneK_(Parameters::defaultIcpPointToPlaneK()), pointToPlaneRadius_(Parameters::defaultIcpPointToPlaneRadius()), @@ -91,6 +93,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomF2MScanRange(), scanMapMaxRange_); Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_); Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_); + Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMinMotion(), bundleMinMotion_); + Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxKeyFramesPerFeature(), bundleMaxKeyFramesPerFeature_); Parameters::parse(parameters, Parameters::kOdomF2MValidDepthRatio(), validDepthRatio_); Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), pointToPlaneK_); @@ -280,6 +284,7 @@ Transform OdometryF2M::computeTransform( std::map bundlePoses; std::multimap bundleLinks; std::map > bundleModels; + float bundleAvgInlierDistance = 0.0f; for(int guessIteration=0; guessIteration<(!guess.isNull()&®Pipeline_->isImageRequired()?2:1) && transform.isNull(); @@ -386,6 +391,7 @@ Transform OdometryF2M::computeTransform( UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size()); std::map > wordReferences; + size_t maxKeyFramesForInlier = 0; for(unsigned int i=0; i >::iterator refIter = bundleWordReferences_.find(wordId); UASSERT_MSG(refIter != bundleWordReferences_.end(), uFormat("wordId=%d", wordId).c_str()); + if(info && refIter->second.size() > maxKeyFramesForInlier) + { + maxKeyFramesForInlier = refIter->second.size(); + } std::map references; int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1; @@ -472,6 +482,7 @@ Transform OdometryF2M::computeTransform( { info->localBundlePoses = bundlePoses; info->localBundleModels = bundleModels; + info->localBundleMaxKeyFramesForInlier = maxKeyFramesForInlier; } UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str()); @@ -534,13 +545,52 @@ Transform OdometryF2M::computeTransform( regInfo.covariance.at(4,4) *= 0.1; if(regInfo.covariance.at(5,5)>thrAng) regInfo.covariance.at(5,5) *= 0.1; + + // Estimate how much the new frame moved from previous frame in term of pixels + if(bundleMinMotion_ > 0.0f) + { + UASSERT(!bundlePoses_.empty()); + int count = 0; + for(unsigned int i=0; i >::iterator wter = wordReferences.find(regInfo.inliersIDs[i]); + if(wter != wordReferences.end()) + { + std::map::iterator fter = wter->second.find(bundlePoses_.rbegin()->first); + if(fter != wter->second.end()) + { + const FeatureBA & f1 = fter->second; // previous key-frame + const FeatureBA & f2 = wter->second.find(lastFrame_->id())->second; // current key-frame + float dx = f1.kpt.pt.x - f2.kpt.pt.x; + float dy = f1.kpt.pt.y - f2.kpt.pt.y; + bundleAvgInlierDistance += sqrt(dx*dx + dy*dy); + ++count; + } + } + } + if(count) + { + bundleAvgInlierDistance /= count; + } + UDEBUG("Average pixel distance between %d inliers: %f", count, bundleAvgInlierDistance); + if(info) + { + info->localBundleAvgInlierDistance = bundleAvgInlierDistance; + } + } } + UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str()); + } + else + { + regInfo.rejectedMsg = "Last bundle pose is null?!"; + transform.setNull(); } - UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str()); } else { - UWARN("Local bundle adjustment failed! transform is not refined."); + regInfo.rejectedMsg = "Local bundle adjustment failed!"; + transform.setNull(); } } } @@ -598,15 +648,24 @@ Transform OdometryF2M::computeTransform( (keyFrameThr_ == 0.0f || visKeyFrameThr_ == 0 || float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) || - regInfo.inliers <= visKeyFrameThr_); + regInfo.inliers <= visKeyFrameThr_) && + (bundleAdjustment_==0 || bundleAvgInlierDistance >= bundleMinMotion_); bool addGeometricKeyFrame = regPipeline_->isScanRequired() && (scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_); - addKeyFrame = false;//bundleLinks.rbegin()->second.transform().getNorm() > 5.0f*0.075f; - addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame; + addKeyFrame = addVisualKeyFrame || addGeometricKeyFrame; + + UDEBUG("keyframeThr=%f visKeyFrameThr_=%d matches=%d inliers=%d (avg dist=%f, min=%f) features=%d mp=%d", + keyFrameThr_, + visKeyFrameThr_, + regInfo.matches, + regInfo.inliers, + bundleAvgInlierDistance, + bundleMinMotion_, + (int)lastFrame_->sensorData().keypoints().size(), + (int)mapPoints.size()); - UDEBUG("keyframeThr=%f visKeyFrameThr_=%d matches=%d inliers=%d features=%d mp=%d", keyFrameThr_, visKeyFrameThr_, regInfo.matches, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size(), (int)mapPoints.size()); if(addKeyFrame) { //Visual @@ -733,7 +792,16 @@ Transform OdometryF2M::computeTransform( } else { - bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex))); + std::map & keyframes = bundleWordReferences_.find(iter->first)->second; + if(bundleMaxKeyFramesPerFeature_ != 0 && (int)keyframes.size() > bundleMaxKeyFramesPerFeature_) + { + // To keep number of keyframes looking at same feature bounded + int frameId = keyframes.rbegin()->first; + UASSERT(bundlePoseReferences_.find(frameId) != bundlePoseReferences_.end()); + bundlePoseReferences_.at(frameId) -= 1; + keyframes.erase(frameId); + } + keyframes.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex))); } } } diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 2a1e6284..caa4303f 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -659,6 +659,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh _ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", false); _ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", false); _ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", false); + _ui->statsToolBox->updateStat("Odometry/localBundleAvgInlierDistance/pix", false); + _ui->statsToolBox->updateStat("Odometry/localBundleMaxKeyFramesForInlier/", false); _ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", false); _ui->statsToolBox->updateStat("Odometry/Interval/ms", false); _ui->statsToolBox->updateStat("Odometry/Speed/kph", false); @@ -1829,6 +1831,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI _ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/localBundleAvgInlierDistance/pix", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleAvgInlierDistance, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/localBundleMaxKeyFramesForInlier/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleMaxKeyFramesForInlier, _preferencesDialog->isCacheSavedInFigures()); } _ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)data->id(), _preferencesDialog->isCacheSavedInFigures()); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 8e69a498..5a5add5e 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1410,6 +1410,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->odom_f2m_validDepthRatio->setObjectName(Parameters::kOdomF2MValidDepthRatio().c_str()); _ui->odom_f2m_bundleStrategy->setObjectName(Parameters::kOdomF2MBundleAdjustment().c_str()); _ui->odom_f2m_bundleMaxFrames->setObjectName(Parameters::kOdomF2MBundleAdjustmentMaxFrames().c_str()); + _ui->odom_f2m_bundleMinMotion->setObjectName(Parameters::kOdomF2MBundleAdjustmentMinMotion().c_str()); + _ui->odom_f2m_bundleMaxKeyFramesPerFeature->setObjectName(Parameters::kOdomF2MBundleAdjustmentMaxKeyFramesPerFeature().c_str()); //Odometry Frame To Frame _ui->comboBox_odomf2f_corType->setObjectName(Parameters::kVisCorType().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 381fed6c..d2f4366f 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -15799,7 +15799,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 1 + 0 @@ -15824,32 +15824,32 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 0 + + + + [Visual] If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map. - - 999999 + + true - - 1 - - - 0 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + m - 3 + 0 + + + 9999.000000000000000 - 0.025000000000000 + 0.000000000000000 @@ -15866,6 +15866,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + 0 + + + 999999999 + + + 1 + + + 0 + + + @@ -15892,33 +15908,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 0 - - - 999999999 - - - 1 - - - 0 - - - - - - - 999999999 - - - - - + + - [Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit. + [Visual] To create a new keyframe with bundle adjustment, a minimum motion (in pixels) can be required. The motion is computed by the average distance between inliers of the previous keyframe and new frame. true @@ -15928,6 +15921,55 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + [Visual] Maximum frames used for bundle adjustment (0=inf or all current frames in the local map). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + [Visual] Gravity sigma used for bundle adjustment (<0, use same value than Optimizer/GravitySigma parameter) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + deg + + + 0 + + + 180.000000000000000 + + + 0.000000000000000 + + + + + + + 999999 + + + @@ -15941,23 +15983,77 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + 0 + 999999 + + 1 + + + 0 + - - - - [Visual] Maximum frames used for bundle adjustment (0=inf or all current frames in the local map). + + + + pixels - - true + + 1 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 99.000000000000000 + + + 0.500000000000000 + + + 0.000000000000000 + + + + + + + 999999999 + + + + + + + + + + 2 + + + 1.000000000000000 + + + 0.050000000000000 + + + 0.750000000000000 + + + + + + + m + + + 3 + + + 0.025000000000000 @@ -15988,6 +16084,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + [Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -16001,96 +16110,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - deg - - - 0 - - - 180.000000000000000 - - - 0.000000000000000 - - - - - - - [Visual] If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - 2 - - - 1.000000000000000 - - - 0.050000000000000 - - - 0.750000000000000 - - - - - - - [Geometry] Distance Range used to filter points of local map (when > 0). 0 means local map is updated using time and not range. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - m - - - 0 - - - 9999.000000000000000 - - - 0.000000000000000 - - - - - - - [Visual] Gravity sigma used for bundle adjustment (<0, use same value than Optimizer/GravitySigma parameter) - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -16113,6 +16132,39 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + [Geometry] Distance Range used to filter points of local map (when > 0). 0 means local map is updated using time and not range. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + [Visual] Maximum keyframes per feature for bundle adjustment. 0 means not limit. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 999999 + + + From 921381b3bd36efabddc34c092bc6e3c2b27ac4eb Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 21 Feb 2025 23:01:24 -0800 Subject: [PATCH 05/19] Updated CI action versions --- .github/workflows/docker.yml | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 29623ac2..dace7282 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -172,21 +172,21 @@ jobs: uses: actions/checkout@v2 - name: Set up QEMU - uses: docker/setup-qemu-action@v1 + uses: docker/setup-qemu-action@v3 with: platforms: all - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v1 + uses: docker/setup-buildx-action@v3 - name: Login to DockerHub - uses: docker/login-action@v1 + uses: docker/login-action@v3 with: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - name: Build and push - uses: docker/build-push-action@v2 + uses: docker/build-push-action@v6 with: context: . push: true @@ -194,6 +194,7 @@ jobs: file: ./docker/${{ matrix.docker_path }}/Dockerfile build-args: | ${{ matrix.docker_args }} + ulimit: stack=33554432:33554432 tags: ${{ matrix.docker_tags }} cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }} cache-to: type=inline From 079600bf3ab135b71ef8a556cd7516691bd9ac3c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 22 Feb 2025 19:48:33 -0800 Subject: [PATCH 06/19] Fixing CI #1454 --- .github/workflows/docker.yml | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index dace7282..1b9fa16c 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -49,21 +49,22 @@ jobs: uses: actions/checkout@v2 - name: Set up QEMU - uses: docker/setup-qemu-action@v1 + uses: docker/setup-qemu-action@v3 with: + image: tonistiigi/binfmt:qemu-v7.0.0-28 platforms: all - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v1 + uses: docker/setup-buildx-action@v3 - name: Login to DockerHub - uses: docker/login-action@v1 + uses: docker/login-action@v3 with: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - name: Build and push - uses: docker/build-push-action@v2 + uses: docker/build-push-action@v6 with: context: . push: true @@ -174,6 +175,7 @@ jobs: name: Set up QEMU uses: docker/setup-qemu-action@v3 with: + image: tonistiigi/binfmt:qemu-v7.0.0-28 platforms: all - name: Set up Docker Buildx @@ -194,7 +196,6 @@ jobs: file: ./docker/${{ matrix.docker_path }}/Dockerfile build-args: | ${{ matrix.docker_args }} - ulimit: stack=33554432:33554432 tags: ${{ matrix.docker_tags }} cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }} cache-to: type=inline From 7adaa249ff5925854b714300b9699c678f8b0da7 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 23 Feb 2025 10:07:05 -0800 Subject: [PATCH 07/19] Disabling ****-deps docker from CI (#1454) --- .github/workflows/docker.yml | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 1b9fa16c..df07410d 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -7,6 +7,16 @@ on: jobs: docker_deps: + + # Disabling ###-deps step from CI because it is too flaky (seg faults, arm64 build timeout...) + # Only way I was able to build all images is to do it from a ubuntu 20.04 computer with: + # $ sudo add-apt-repository ppa:canonical-server/server-backports + # $ sudo apt-get update + # $ sudo apt-get upgrade qemu-user-static + # $ docker run --rm --privileged multiarch/qemu-user-static --reset -p yes -c yes + # More info: https://github.com/introlab/rtabmap/issues/1454 + if: false + runs-on: ubuntu-latest strategy: @@ -75,7 +85,7 @@ jobs: cache-to: type=inline docker: - needs: docker_deps + #needs: docker_deps runs-on: ubuntu-latest strategy: From e65c1c2f853146e1a4d6e8177756501e29d5135e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 23 Feb 2025 10:49:25 -0800 Subject: [PATCH 08/19] jammy-deps: explicitly uninstall grid-map-core till new humble version is released --- docker/jammy/deps/Dockerfile | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/docker/jammy/deps/Dockerfile b/docker/jammy/deps/Dockerfile index f7617f82..8f29497d 100644 --- a/docker/jammy/deps/Dockerfile +++ b/docker/jammy/deps/Dockerfile @@ -24,18 +24,11 @@ RUN apt-get update && \ apt-get install -y git libtbb-dev ros-humble-ros-base ros-dev-tools && \ apt-get clean && rm -rf /var/lib/apt/lists/ -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then apt-get update && \ +RUN apt-get update && \ apt upgrade -y && \ apt-get install -y libfreenect-dev ros-humble-rtabmap-ros && \ - apt-get remove -y ros-humble-rtabmap* && \ - apt-get clean && rm -rf /var/lib/apt/lists/; fi - -# current issue that ros-humble-rtabmap-ros is not available on arm64 -RUN if [ "$TARGETPLATFORM" = "linux/arm64" ]; then apt-get update && \ - apt upgrade -y && \ - apt-get install -y libfreenect-dev ros-humble-rtabmap-launch && \ - apt-get remove -y ros-humble-rtabmap* && \ - apt-get clean && rm -rf /var/lib/apt/lists/; fi + apt-get remove -y ros-humble-rtabmap* ros-humble-grid-map-core && \ + apt-get clean && rm -rf /var/lib/apt/lists/ WORKDIR /root/ From 7a4f8d9783cfc3ddb9b5444c3ef543c537ab5760 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 23 Feb 2025 13:34:36 -0800 Subject: [PATCH 09/19] reverting qemu version #1454 --- .github/workflows/docker.yml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index df07410d..75b7631c 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -61,7 +61,6 @@ jobs: name: Set up QEMU uses: docker/setup-qemu-action@v3 with: - image: tonistiigi/binfmt:qemu-v7.0.0-28 platforms: all - name: Set up Docker Buildx @@ -185,7 +184,6 @@ jobs: name: Set up QEMU uses: docker/setup-qemu-action@v3 with: - image: tonistiigi/binfmt:qemu-v7.0.0-28 platforms: all - name: Set up Docker Buildx From ec4e076a088b42267557db6ab26de26e429f31de Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 24 Feb 2025 10:29:45 -0800 Subject: [PATCH 10/19] CI: moved opengv back in deps dockerfile --- docker/jammy/Dockerfile | 17 ----------------- docker/jammy/deps/Dockerfile | 16 ++++++++++++++++ 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/docker/jammy/Dockerfile b/docker/jammy/Dockerfile index c7dbc07c..21660df6 100644 --- a/docker/jammy/Dockerfile +++ b/docker/jammy/Dockerfile @@ -2,23 +2,6 @@ FROM introlab3it/rtabmap:jammy-deps -# June 19 2023: moved opengv here so that jammy-deps can be built on my computer. Not sure why but on my machine opengv arm64 fails, but not on CI. -#commit Aug 6 2020 -RUN apt-get update && apt install -y wget && \ - apt-get clean && rm -rf /var/lib/apt/lists/ -RUN git clone https://github.com/laurentkneip/opengv.git && \ - cd opengv && \ - git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \ - wget https://gist.githubusercontent.com/matlabbe/a412cf7c4627253874f81a00745a7fbb/raw/accc3acf465d1ffd0304a46b17741f62d4d354ef/opengv_disable_march_native.patch && \ - git apply opengv_disable_march_native.patch && \ - mkdir build && \ - cd build && \ - cmake -DCMAKE_BUILD_TYPE=Release .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r opengv - # Will be used to read/store databases on host RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map diff --git a/docker/jammy/deps/Dockerfile b/docker/jammy/deps/Dockerfile index 8f29497d..0ec47c53 100644 --- a/docker/jammy/deps/Dockerfile +++ b/docker/jammy/deps/Dockerfile @@ -79,6 +79,22 @@ RUN git clone --branch 4.5.4 https://github.com/opencv/opencv.git && \ cd ../.. && \ rm -rf opencv opencv_contrib +#commit Aug 6 2020 +RUN apt-get update && apt install -y wget && \ + apt-get clean && rm -rf /var/lib/apt/lists/ +RUN git clone https://github.com/laurentkneip/opengv.git && \ + cd opengv && \ + git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \ + wget https://gist.githubusercontent.com/matlabbe/a412cf7c4627253874f81a00745a7fbb/raw/accc3acf465d1ffd0304a46b17741f62d4d354ef/opengv_disable_march_native.patch && \ + git apply opengv_disable_march_native.patch && \ + mkdir build && \ + cd build && \ + cmake -DCMAKE_BUILD_TYPE=Release .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r opengv + RUN rm /bin/sh && ln -s /bin/bash /bin/sh COPY ./docker/jammy/deps/ros_entrypoint.sh /ros_entrypoint.sh From 35c3ca20ad55977cdb1c3229b26d349e1d8aabfe Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 25 Feb 2025 16:09:32 -0800 Subject: [PATCH 11/19] 2d ray tracing: Fixed assert when viewpoint is farther than cellSize*10 from the pose. --- corelib/src/util3d_mapping.cpp | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/corelib/src/util3d_mapping.cpp b/corelib/src/util3d_mapping.cpp index ce876486..9599f206 100644 --- a/corelib/src/util3d_mapping.cpp +++ b/corelib/src/util3d_mapping.cpp @@ -703,10 +703,7 @@ cv::Mat create2DMap(const std::map & poses, cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize); if(end!=start) { - if(localScans.size() > 1 || map.at(end.y, end.x) != 0) - { - rayTrace(start, end, map, true); // trace free space - } + rayTrace(start, end, map, true); // trace free space } } // ray tracing for no hits @@ -730,14 +727,7 @@ cv::Mat create2DMap(const std::map & poses, cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize); if(end!=start) { - if(localScans.size() > 1 || map.at(end.y, end.x) != 0) - { - rayTrace(start, end, map, true); // trace free space - if(map.at(end.y, end.x) == -1) - { - map.at(end.y, end.x) = 0; // empty - } - } + rayTrace(start, end, map, true); // trace free space } } } @@ -823,8 +813,6 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid { UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str()); UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str()); - UASSERT_MSG(end.x >= 0 && end.x < grid.cols, uFormat("end.x=%d grid.cols=%d", end.x, grid.cols).c_str()); - UASSERT_MSG(end.y >= 0 && end.y < grid.rows, uFormat("end.x=%d grid.cols=%d", end.y, grid.rows).c_str()); cv::Point2i ptA, ptB; ptA = start; @@ -832,6 +820,10 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x); + // clip end point + ptB.x = std::min(std::max(ptB.x, 0), grid.cols-1); + ptB.y = std::min(std::max(ptB.y, 0), grid.rows-1); + bool swapped = false; if(slope<-1.0f || slope>1.0f) { From 258095e0f3dbc1c0fb0ad355ca53900ff58a47e4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 26 Feb 2025 16:06:13 -0800 Subject: [PATCH 12/19] DBReader: added fine-grained camera selection (#1457) --- corelib/include/rtabmap/core/DBReader.h | 6 +- corelib/src/DBReader.cpp | 120 +++++++++++++++++++----- guilib/src/PreferencesDialog.cpp | 49 ++++++++-- guilib/src/ui/preferencesDialog.ui | 23 ++--- tools/Reprocess/main.cpp | 24 ++++- 5 files changed, 171 insertions(+), 51 deletions(-) diff --git a/corelib/include/rtabmap/core/DBReader.h b/corelib/include/rtabmap/core/DBReader.h index 4dac18a2..63e2713b 100644 --- a/corelib/include/rtabmap/core/DBReader.h +++ b/corelib/include/rtabmap/core/DBReader.h @@ -51,7 +51,7 @@ public: bool ignoreGoalDelay = false, bool goalsIgnored = false, int startId = 0, - int cameraIndex = -1, + const std::vector & cameraIndices = std::vector(), int stopId = 0, bool intermediateNodesIgnored = false, bool landmarksIgnored = false, @@ -65,7 +65,7 @@ public: bool ignoreGoalDelay = false, bool goalsIgnored = false, int startId = 0, - int cameraIndex = -1, + const std::vector & cameraIndices = std::vector(), int stopId = 0, bool intermediateNodesIgnored = false, bool landmarksIgnored = false, @@ -99,7 +99,7 @@ private: bool _goalsIgnored; int _startId; int _stopId; - int _cameraIndex; + std::vector _cameraIndices; bool _intermediateNodesIgnored; bool _landmarksIgnored; bool _featuresIgnored; diff --git a/corelib/src/DBReader.cpp b/corelib/src/DBReader.cpp index 5d91f548..acec939a 100644 --- a/corelib/src/DBReader.cpp +++ b/corelib/src/DBReader.cpp @@ -48,7 +48,7 @@ DBReader::DBReader(const std::string & databasePath, bool ignoreGoalDelay, bool goalsIgnored, int startId, - int cameraIndex, + const std::vector & cameraIndices, int stopId, bool intermediateNodesIgnored, bool landmarksIgnored, @@ -63,7 +63,7 @@ DBReader::DBReader(const std::string & databasePath, _goalsIgnored(goalsIgnored), _startId(startId), _stopId(stopId), - _cameraIndex(cameraIndex), + _cameraIndices(cameraIndices), _intermediateNodesIgnored(intermediateNodesIgnored), _landmarksIgnored(landmarksIgnored), _featuresIgnored(featuresIgnored), @@ -94,7 +94,7 @@ DBReader::DBReader(const std::list & databasePaths, bool ignoreGoalDelay, bool goalsIgnored, int startId, - int cameraIndex, + const std::vector & cameraIndices, int stopId, bool intermediateNodesIgnored, bool landmarksIgnored, @@ -109,7 +109,7 @@ DBReader::DBReader(const std::list & databasePaths, _goalsIgnored(goalsIgnored), _startId(startId), _stopId(stopId), - _cameraIndex(cameraIndex), + _cameraIndices(cameraIndices), _intermediateNodesIgnored(intermediateNodesIgnored), _landmarksIgnored(landmarksIgnored), _featuresIgnored(featuresIgnored), @@ -557,34 +557,77 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info) } data.uncompressData(); - if(data.cameraModels().size() > 1 && - _cameraIndex >= 0) + std::map cameraOldNewIndices; + std::vector dbModels = data.cameraModels(); + if(dbModels.empty() && !data.stereoCameraModels().empty()) { - if(_cameraIndex < (int)data.cameraModels().size()) + for(size_t i=0; i 1 && + !_cameraIndices.empty()) + { + cv::Mat combinedImages; + cv::Mat combinedDepthImages; + std::vector combinedModels; + std::vector combinedStereoModels; + for(size_t i=0; i<_cameraIndices.size(); ++i) + { + UASSERT_MSG(_cameraIndices[i] < dbModels.size(), uFormat("DBReader: camera index %ld is not valid (should be between 0 and %ld)", + _cameraIndices[i], dbModels.size()-1).c_str()); + + int addedCameras = std::max(combinedModels.size(), combinedStereoModels.size()); + + int subImageWidth = data.imageRaw().cols/dbModels.size(); UASSERT(!data.imageRaw().empty() && - data.imageRaw().cols % data.cameraModels().size() == 0 && - _cameraIndex*subImageWidth < data.imageRaw().cols); - image= cv::Mat(data.imageRaw(), - cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.imageRaw().rows)).clone(); + data.imageRaw().cols % dbModels.size() == 0 && + (int)_cameraIndices[i]*subImageWidth < data.imageRaw().cols); + if(combinedImages.empty()) + { + // initialize with first camera + combinedImages = cv::Mat(data.imageRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.imageRaw().type()); + } + + cv::Mat fromROI = cv::Mat(data.imageRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.imageRaw().rows)); + cv::Mat toROI = cv::Mat(combinedImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedImages.rows)); + fromROI.copyTo(toROI); cv::Mat depth; if(!data.depthOrRightRaw().empty()) { - UASSERT(data.depthOrRightRaw().cols % data.cameraModels().size() == 0 && - subImageWidth == data.depthOrRightRaw().cols/(int)data.cameraModels().size() && - _cameraIndex*subImageWidth < data.depthOrRightRaw().cols); - depth = cv::Mat(data.depthOrRightRaw(), - cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows)).clone(); + subImageWidth = data.depthOrRightRaw().cols/dbModels.size(); + UASSERT(data.depthOrRightRaw().cols % dbModels.size() == 0 && + subImageWidth == data.depthOrRightRaw().cols/(int)dbModels.size() && + (int)_cameraIndices[i]*subImageWidth < data.depthOrRightRaw().cols); + if(combinedDepthImages.empty()) + { + // initialize with first camera + combinedDepthImages = cv::Mat(data.depthOrRightRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.depthOrRightRaw().type()); + } + fromROI = cv::Mat(data.depthOrRightRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows)); + toROI = cv::Mat(combinedDepthImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedDepthImages.rows)); + fromROI.copyTo(toROI); } - data.setRGBDImage(image, depth, data.cameraModels().at(_cameraIndex)); + + if(!data.cameraModels().empty()) + { + combinedModels.push_back(data.cameraModels()[_cameraIndices[i]]); + } + else + { + combinedStereoModels.push_back(data.stereoCameraModels()[_cameraIndices[i]]); + } + cameraOldNewIndices.insert(std::make_pair(_cameraIndices[i], i)); + } + if(!combinedModels.empty()) + { + data.setRGBDImage(combinedImages, combinedDepthImages, combinedModels); } else { - UWARN("DBReader: Camera index %d doesn't exist! Camera models = %d.", _cameraIndex, (int)data.cameraModels().size()); + data.setStereoImage(combinedImages, combinedDepthImages, combinedStereoModels); } } data.setId(seq); @@ -623,7 +666,40 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info) (keypoints3D.empty() || keypoints.size() == keypoints3D.size()) && (descriptors.empty() || (int)keypoints.size() == descriptors.rows)) { - data.setFeatures(keypoints, keypoints3D, descriptors); + if(!cameraOldNewIndices.empty()) + { + cv::Mat newDescriptors; + std::vector newKeypoints; + std::vector newKeypoints3D; + UASSERT(!dbModels.empty() && dbModels[0].imageWidth()>0); + int subImageWidth = dbModels[0].imageWidth(); + for(size_t i = 0; i= 0 && cameraIndex < (int)dbModels.size(), + uFormat("cameraIndex=%d, db models=%d, kpt.x=%f, image width=%d", + cameraIndex, (int)dbModels.size(), keypoints[i].pt.x, subImageWidth).c_str()); + if(cameraOldNewIndices.find(cameraIndex) != cameraOldNewIndices.end()) + { + int newCameraIndex = cameraOldNewIndices.at(cameraIndex); + newKeypoints.push_back(keypoints[i]); + newKeypoints.back().pt.x += (newCameraIndex-cameraIndex)*subImageWidth; + if(!keypoints3D.empty()) + { + newKeypoints3D.push_back(keypoints3D.at(i)); + } + if(!descriptors.empty()) + { + newDescriptors.push_back(descriptors.row(i)); + } + } + } + data.setFeatures(newKeypoints, newKeypoints3D, newDescriptors); + } + else + { + data.setFeatures(keypoints, keypoints3D, descriptors); + } } else if(!_featuresIgnored && !keypoints.empty() && (!keypoints3D.empty() || !descriptors.empty())) { diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 5a5add5e..9dd7c265 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -742,7 +742,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->source_spinBox_databaseStartId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->source_spinBox_databaseStopId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->source_spinBox_database_cameraIndex, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->source_lineEdit_databaseCameraIndex, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->source_checkBox_stereoToDepthDB, SIGNAL(toggled(bool)), _ui->checkbox_stereo_depthGenerated, SLOT(setChecked(bool))); connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(toggled(bool)), _ui->source_checkBox_stereoToDepthDB, SLOT(setChecked(bool))); @@ -2136,7 +2136,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->source_checkBox_ignorePriors->setChecked(false); _ui->source_spinBox_databaseStartId->setValue(0); _ui->source_spinBox_databaseStopId->setValue(0); - _ui->source_spinBox_database_cameraIndex->setValue(-1); + _ui->source_lineEdit_databaseCameraIndex->setText(""); _ui->source_checkBox_useDbStamps->setChecked(true); #ifdef _WIN32 @@ -2867,7 +2867,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->source_checkBox_ignorePriors->setChecked(settings.value("ignorePriors", _ui->source_checkBox_ignorePriors->isChecked()).toBool()); _ui->source_spinBox_databaseStartId->setValue(settings.value("startId", _ui->source_spinBox_databaseStartId->value()).toInt()); _ui->source_spinBox_databaseStopId->setValue(settings.value("stopId", _ui->source_spinBox_databaseStopId->value()).toInt()); - _ui->source_spinBox_database_cameraIndex->setValue(settings.value("cameraIndex", _ui->source_spinBox_database_cameraIndex->value()).toInt()); + _ui->source_lineEdit_databaseCameraIndex->setText(settings.value("cameraIndices", _ui->source_lineEdit_databaseCameraIndex->text()).toString()); _ui->source_checkBox_useDbStamps->setChecked(settings.value("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked()).toBool()); settings.endGroup(); // Database @@ -3467,7 +3467,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("ignorePriors", _ui->source_checkBox_ignorePriors->isChecked()); settings.setValue("startId", _ui->source_spinBox_databaseStartId->value()); settings.setValue("stopId", _ui->source_spinBox_databaseStopId->value()); - settings.setValue("cameraIndex", _ui->source_spinBox_database_cameraIndex->value()); + settings.setValue("cameraIndices", _ui->source_lineEdit_databaseCameraIndex->text()); settings.setValue("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked()); settings.endGroup(); // Database @@ -4389,7 +4389,7 @@ void PreferencesDialog::selectSourceDatabase() _ui->source_database_lineEdit_path->setText(paths.size()==1?paths.front():paths.join(";")); _ui->source_spinBox_databaseStartId->setValue(0); _ui->source_spinBox_databaseStopId->setValue(0); - _ui->source_spinBox_database_cameraIndex->setValue(-1); + _ui->source_lineEdit_databaseCameraIndex->setText(""); } } @@ -6953,13 +6953,50 @@ Camera * PreferencesDialog::createCamera( } else if(driver == kSrcDatabase) { + std::vector cameraIndices; + if(!_ui->source_lineEdit_databaseCameraIndex->text().isEmpty()) + { + // read the first node to know how many cameras we have in the database + std::shared_ptr reader( + new DBReader( + _ui->source_database_lineEdit_path->text().toStdString(), + 0, + false, + false, + false, + _ui->source_spinBox_databaseStartId->value())); + if(reader->init()) + { + SensorData data = reader->takeImage(); + unsigned int numCamerasInDb = data.cameraModels().size()>0?data.cameraModels().size():data.stereoCameraModels().size(); + if(numCamerasInDb>1) + { + QStringList indicesStr = _ui->source_lineEdit_databaseCameraIndex->text().split(' '); + for(QStringList::iterator iter=indicesStr.begin(); iter!=indicesStr.end(); ++iter) + { + cameraIndices.push_back(iter->toInt()); + if(cameraIndices.back() > numCamerasInDb) + { + QMessageBox::warning(this, + tr("Creating Database Reader"), + tr("Camera index %1 is not valid, it should be between 0 and %2. Remove or update camera indices under Source->Database panel (currently set to \"%3\").") + .arg(cameraIndices.back()).arg(numCamerasInDb).arg(_ui->source_lineEdit_databaseCameraIndex->text()), + QMessageBox::Ok); + cameraIndices.clear(); + return 0; + } + } + } + } + } + camera = new DBReader(_ui->source_database_lineEdit_path->text().toStdString(), _ui->source_checkBox_useDbStamps->isChecked()?-1:this->getGeneralInputRate(), _ui->source_checkBox_ignoreOdometry->isChecked(), _ui->source_checkBox_ignoreGoalDelay->isChecked(), _ui->source_checkBox_ignoreGoals->isChecked(), _ui->source_spinBox_databaseStartId->value(), - _ui->source_spinBox_database_cameraIndex->value(), + cameraIndices, _ui->source_spinBox_databaseStopId->value(), !_ui->general_checkBox_createIntermediateNodes->isChecked(), _ui->source_checkBox_ignoreLandmarks->isChecked(), diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index d2f4366f..afd95297 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -520 + -594 713 4653 @@ -95,7 +95,7 @@ QFrame::Raised - 15 + 5 @@ -3424,7 +3424,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 2 + 3 @@ -6777,7 +6777,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki false - + @@ -6930,7 +6930,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - Camera index. If the database contains multi-camera data, you can choose which camera to use. -1 means that all cameras are streamed. + Camera index. If the database contains multi-camera data, you can choose which camera to use. Leave empty to use all cameras. Can also be multiple indices split by spaces in a string like "0 2" to stream cameras 0 and 2 only. true @@ -6981,16 +6981,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - -1 - - - 9999 - - - @@ -7041,6 +7031,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + diff --git a/tools/Reprocess/main.cpp b/tools/Reprocess/main.cpp index 67ad4e5f..45881ca3 100644 --- a/tools/Reprocess/main.cpp +++ b/tools/Reprocess/main.cpp @@ -81,7 +81,8 @@ void showUsage() " -stop_s # Last map session to process.\n" " -a Append mode: if Mem/IncrementalMemory is true, RTAB-Map is initialized with the first input database,\n" " then next databases are reprocessed on top of the first one.\n" - " -cam # Camera index to stream. Ignored if a database doesn't contain multi-camera data.\n" + " -cam # Camera index to stream. Ignored if a database doesn't contain multi-camera data. Can also be multiple \n" + " indices split by spaces in a string like \"0 2\" to stream cameras 0 and 2 only.\n" " -nolandmark Don't republish landmarks contained in input database.\n" " -nopriors Don't republish priors contained in input database.\n" " -pub_loops Republish loop closures contained in input database.\n" @@ -225,6 +226,15 @@ int main(int argc, char * argv[]) ULogger::setType(ULogger::kTypeConsole); ULogger::setLevel(ULogger::kError); + // override help from Parameters to show the whole thing + for(int i=1; i cameraIndices; int framesToSkip = 0; bool ignoreLandmarks = false; bool ignorePriors = false; @@ -361,8 +371,12 @@ int main(int argc, char * argv[]) ++i; if(i < argc - 2) { - cameraIndex = atoi(argv[i]); - printf("Camera index = %d.\n", cameraIndex); + std::list indicesStr = uSplit(argv[i], ' '); + for(std::list::iterator iter=indicesStr.begin(); iter!=indicesStr.end(); ++iter) + { + cameraIndices.push_back(uStr2Int(*iter)); + printf("Camera index = %d.\n", cameraIndices.back()); + } } else { @@ -768,7 +782,7 @@ int main(int argc, char * argv[]) false, false, startId, - cameraIndex, + cameraIndices, stopId, !intermediateNodes, ignoreLandmarks, From a85e732f0103999801ff33eca7b7378a71ad673f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Mar 2025 15:53:02 -0800 Subject: [PATCH 13/19] bump to 0.21.11 for api change --- CMakeLists.txt | 2 +- package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ac2c2e24..2148ee96 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 21) -SET(RTABMAP_PATCH_VERSION 10) +SET(RTABMAP_PATCH_VERSION 11) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) diff --git a/package.xml b/package.xml index 8e5f32ae..a4e3e4ba 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap - 0.21.10 + 0.21.11 RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe From 2c7aa5a78fb1d26af96c4c55d0b73fddd4cdd4b5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Mar 2025 19:46:03 -0800 Subject: [PATCH 14/19] Setting WITH_GRIDMAP=OFF by default till official grid_map_core can be correctly integrated in libraries downstream. --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2148ee96..fe77f6a8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -208,7 +208,7 @@ option(WITH_MYNTEYE "Include mynteye-s support" ON) option(WITH_DEPTHAI "Include depthai-core support" OFF) option(WITH_XVSDK "Include XVisio SDK support" OFF) option(WITH_OCTOMAP "Include OctoMap support" ON) -option(WITH_GRIDMAP "Include GridMap support" ON) +option(WITH_GRIDMAP "Include GridMap support" OFF) option(WITH_CPUTSDF "Include CPUTSDF support" OFF) option(WITH_OPENCHISEL "Include open_chisel support" OFF) option(WITH_ALICE_VISION "Include AliceVision support" OFF) From 4eea83d1afee433e63bdd858a0c5f63e9c72e060 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 6 Mar 2025 11:00:33 -0800 Subject: [PATCH 15/19] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 7e17ddfc..d5d91447 100644 --- a/README.md +++ b/README.md @@ -64,8 +64,8 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated Build Status - Iron - Build Status + Jazzy + Build Status Rolling From d88353dc1b665669f07b2f81831aa6a2b242c314 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Sun, 9 Mar 2025 03:30:11 +0800 Subject: [PATCH 16/19] Further improvements to DepthAI integration (#1458) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Reorganize DepthAI params * Advanced Tuning of DepthAI’s SGBM Pipeline * Update IMU Local Transform for OAK-D LR, OAK-D SR, and PoE Devices * Reorganiz the usage of DepthAI. Get params before creating the pipeline, and automatically determine the resolution of different models. * Identify the IMU type to specifically handle BMI270 and BNO085/086 --- .../rtabmap/core/camera/CameraDepthAI.h | 23 +- corelib/src/camera/CameraDepthAI.cpp | 640 ++++++++---------- guilib/src/PreferencesDialog.cpp | 59 +- guilib/src/ui/preferencesDialog.ui | 497 +++++++------- 4 files changed, 608 insertions(+), 611 deletions(-) diff --git a/corelib/include/rtabmap/core/camera/CameraDepthAI.h b/corelib/include/rtabmap/core/camera/CameraDepthAI.h index 163572b7..b512eba0 100644 --- a/corelib/include/rtabmap/core/camera/CameraDepthAI.h +++ b/corelib/include/rtabmap/core/camera/CameraDepthAI.h @@ -50,21 +50,20 @@ public: public: CameraDepthAI( const std::string & mxidOrName = "", - int resolution = 1, // 0=720p, 1=800p, 2=400p - float imageRate=0.0f, + int imageWidth = 1280, // 640 or 1280 + float imageRate = 0.0f, const Transform & localTransform = Transform::getIdentity()); virtual ~CameraDepthAI(); void setOutputMode(int outputMode = 0); void setDepthProfile(int confThreshold = 200, int lrcThreshold = 5); - void setExtendedDisparity(bool extendedDisparity); - void setSubpixelMode(bool enabled, int fractionalBits = 3); - void setCompanding(bool enabled, int width=96); - void setRectification(bool useSpecTranslation, float alphaScaling = 0.0f, bool enabled=true); + void setExtendedDisparity(bool extendedDisparity = false, bool enableCompanding = false); + void setSubpixelMode(bool enabled = false, int fractionalBits = 3); + void setDisparityWidthAndFilter(int disparityWidth = 96, int medianFilter = 5); + void setRectification(bool useSpecTranslation = false, float alphaScaling = 0.0f, bool enabled = true); void setIMU(bool imuPublished, bool publishInterIMU); void setIrIntensity(float dotIntensity = 0.0f, float floodIntensity = 0.0f); - void setDetectFeatures(int detectFeatures = 0); - void setBlobPath(const std::string & blobPath); + void setDetectFeatures(int detectFeatures = 0, const std::string & blobPath = ""); void setGFTTDetector(bool useHarrisDetector = false, float minDistance = 7.0f, int numTargetFeatures = 1000); void setSuperPointDetector(float threshold = 0.01f, bool nms = true, int nmsRadius = 4); @@ -84,10 +83,12 @@ private: int outputMode_; int confThreshold_; int lrcThreshold_; - int resolution_; + int imageWidth_; bool extendedDisparity_; + bool enableCompanding_; int subpixelFractionalBits_; - int compandingWidth_; + int disparityWidth_; + int medianFilter_; bool useSpecTranslation_; float alphaScaling_; bool imagesRectified_; @@ -103,7 +104,7 @@ private: bool nms_; int nmsRadius_; std::string blobPath_; - std::shared_ptr device_; + std::unique_ptr device_; std::shared_ptr cameraQueue_; std::map accBuffer_; std::map gyroBuffer_; diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index 36b94089..c5bdf4ac 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -47,7 +47,7 @@ bool CameraDepthAI::available() CameraDepthAI::CameraDepthAI( const std::string & mxidOrName, - int resolution, + int imageWidth, float imageRate, const Transform & localTransform) : Camera(imageRate, localTransform) @@ -57,12 +57,15 @@ CameraDepthAI::CameraDepthAI( outputMode_(0), confThreshold_(200), lrcThreshold_(5), - resolution_(resolution), + imageWidth_(imageWidth), extendedDisparity_(false), - subpixelFractionalBits_(0), - compandingWidth_(0), + enableCompanding_(false), + subpixelFractionalBits_(3), + disparityWidth_(1), + medianFilter_(5), useSpecTranslation_(false), alphaScaling_(0.0), + imagesRectified_(true), imuPublished_(true), publishInterIMU_(false), dotIntensity_(0.0), @@ -70,14 +73,16 @@ CameraDepthAI::CameraDepthAI( detectFeatures_(0), useHarrisDetector_(false), minDistance_(7.0), - numTargetFeatures_(1000), + numTargetFeatures_(320), threshold_(0.01), nms_(true), nmsRadius_(4) #endif { #ifdef RTABMAP_DEPTHAI - UASSERT(resolution_>=(int)dai::MonoCameraProperties::SensorResolution::THE_720_P && resolution_<=(int)dai::MonoCameraProperties::SensorResolution::THE_1200_P); + UASSERT(imageWidth_ == 640 || imageWidth_ == 1280); + if(this->getImageRate() <= 0) + this->setImageRate(30); #endif } @@ -85,9 +90,7 @@ CameraDepthAI::~CameraDepthAI() { #ifdef RTABMAP_DEPTHAI if(device_.get()) - { device_->close(); - } #endif } @@ -110,22 +113,15 @@ void CameraDepthAI::setDepthProfile(int confThreshold, int lrcThreshold) #endif } -void CameraDepthAI::setExtendedDisparity(bool extendedDisparity) +void CameraDepthAI::setExtendedDisparity(bool extendedDisparity, bool enableCompanding) { #ifdef RTABMAP_DEPTHAI extendedDisparity_ = extendedDisparity; - if(extendedDisparity_) + enableCompanding_ = enableCompanding; + if(extendedDisparity_ && enableCompanding_) { - if(subpixelFractionalBits_>0) - { - UWARN("Extended disparity has been enabled while subpixel being also enabled, disabling subpixel..."); - subpixelFractionalBits_ = 0; - } - if(compandingWidth_>0) - { - UWARN("Extended disparity has been enabled while companding being also enabled, disabling companding..."); - compandingWidth_ = 0; - } + UWARN("Extended disparity has been enabled while companding being also enabled, disabling companding..."); + enableCompanding_ = false; } #else UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); @@ -137,25 +133,22 @@ void CameraDepthAI::setSubpixelMode(bool enabled, int fractionalBits) #ifdef RTABMAP_DEPTHAI UASSERT(fractionalBits>=3 && fractionalBits<=5); subpixelFractionalBits_ = enabled?fractionalBits:0; - if(subpixelFractionalBits_ != 0 && extendedDisparity_) - { - UWARN("Subpixel has been enabled while extended disparity being also enabled, disabling extended disparity..."); - extendedDisparity_ = false; - } #else UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); #endif } -void CameraDepthAI::setCompanding(bool enabled, int width) +void CameraDepthAI::setDisparityWidthAndFilter(int disparityWidth, int medianFilter) { #ifdef RTABMAP_DEPTHAI - UASSERT(width == 64 || width == 96); - compandingWidth_ = enabled?width:0; - if(compandingWidth_ != 0 && extendedDisparity_) + UASSERT(disparityWidth == 64 || disparityWidth == 96); + disparityWidth_ = disparityWidth; + medianFilter_ = medianFilter; + int maxDisp = (extendedDisparity_?2:1) * std::pow(2,subpixelFractionalBits_) * (disparityWidth_-1); + if(medianFilter_ && maxDisp > 1024) { - UWARN("Companding has been enabled while extended disparity being also enabled, disabling extended disparity..."); - extendedDisparity_ = false; + UWARN("Maximum disparity value '%d' exceeds the maximum supported '1024' by median filter, disabling median filter...", maxDisp); + medianFilter_ = 0; } #else UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); @@ -193,19 +186,26 @@ void CameraDepthAI::setIrIntensity(float dotIntensity, float floodIntensity) #endif } -void CameraDepthAI::setDetectFeatures(int detectFeatures) +void CameraDepthAI::setDetectFeatures(int detectFeatures, const std::string & blobPath) { #ifdef RTABMAP_DEPTHAI detectFeatures_ = detectFeatures; -#else - UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); -#endif -} - -void CameraDepthAI::setBlobPath(const std::string & blobPath) -{ -#ifdef RTABMAP_DEPTHAI blobPath_ = blobPath; + if(detectFeatures_ && outputMode_==2) + { + UWARN("On-device feature detectors cannot be enabled on color camera input, disabling on-device feature detector..."); + detectFeatures_ = 0; + } + if(detectFeatures_>=2 && blobPath_.empty()) + { + UWARN("Missing MyriadX blob file, disabling on-device feature detector"); + detectFeatures_ = 0; + } + if(detectFeatures_>=2 && this->getImageRate()>15) + { + UWARN("On-device SuperPoint or HF-Net enabled, image rate is limited to 15 FPS!"); + this->setImageRate(15); + } #else UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); #endif @@ -245,9 +245,6 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin return false; } - if(device_.get()) - device_->close(); - accBuffer_.clear(); gyroBuffer_.clear(); @@ -268,259 +265,22 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin return false; } - // look for calibration files - stereoModel_ = StereoCameraModel(); - targetSize_ = cv::Size(resolution_<2?1280:resolution_==4?1920:640, resolution_==0?720:resolution_==1?800:resolution_==2?400:resolution_==3?480:1200); - - dai::Pipeline p; - auto monoLeft = p.create(); - auto monoRight = p.create(); - std::shared_ptr stereo; - if(imagesRectified_) - stereo = p.create(); - std::shared_ptr colorCam; - if(outputMode_==2) - { - colorCam = p.create(); - if(!imagesRectified_) - colorCam->setMeshSource(dai::CameraProperties::WarpMeshSource::NONE); - if(detectFeatures_) - { - UWARN("On-device feature detectors cannot be enabled on color camera input!"); - detectFeatures_ = 0; - } - } - std::shared_ptr imu; - if(imuPublished_) - imu = p.create(); - std::shared_ptr gfttDetector; - std::shared_ptr manip; - std::shared_ptr neuralNetwork; - if(detectFeatures_ == 1) - { - gfttDetector = p.create(); - } - else if(detectFeatures_ >= 2) - { - if(!blobPath_.empty()) - { - manip = p.create(); - neuralNetwork = p.create(); - } - else - { - UWARN("Missing MyriadX blob file!"); - detectFeatures_ = 0; - } - } - - auto sync = p.create(); - auto xoutCamera = p.create(); - std::shared_ptr xoutIMU; - if(imuPublished_) - xoutIMU = p.create(); - - // XLinkOut - xoutCamera->setStreamName("camera"); - if(imuPublished_) - xoutIMU->setStreamName("imu"); - - monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_); - monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_); - monoLeft->setCamera("left"); - monoRight->setCamera("right"); - if(detectFeatures_ >= 2) - { - if(this->getImageRate() <= 0 || this->getImageRate() > 15) - { - UWARN("On-device SuperPoint or HF-Net enabled, image rate is limited to 15 FPS!"); - monoLeft->setFps(15); - monoRight->setFps(15); - } - } - else if(this->getImageRate() > 0) - { - monoLeft->setFps(this->getImageRate()); - monoRight->setFps(this->getImageRate()); - } - - // StereoDepth - if(stereo.get()) - { - if(outputMode_ == 2) - stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A); - else - stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT); - stereo->setExtendedDisparity(extendedDisparity_); - stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout - stereo->enableDistortionCorrection(true); - stereo->setDisparityToDepthUseSpecTranslation(useSpecTranslation_); - stereo->setDepthAlignmentUseSpecTranslation(useSpecTranslation_); - if(alphaScaling_ > -1.0f) - stereo->setAlphaScaling(alphaScaling_); - stereo->initialConfig.setConfidenceThreshold(confThreshold_); - stereo->initialConfig.setLeftRightCheck(lrcThreshold_>=0); - if(lrcThreshold_>=0) - stereo->initialConfig.setLeftRightCheckThreshold(lrcThreshold_); - stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7); - auto config = stereo->initialConfig.get(); - config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9; - config.censusTransform.kernelMask = 0X2AA00AA805540155; - config.postProcessing.brightnessFilter.maxBrightness = 255; - stereo->initialConfig.set(config); - - // Link plugins CAM -> STEREO -> XLINK - monoLeft->out.link(stereo->left); - monoRight->out.link(stereo->right); - } - - if(outputMode_ == 2) - { - colorCam->setBoardSocket(dai::CameraBoardSocket::CAM_A); - colorCam->setSize(targetSize_.width, targetSize_.height); - if(this->getImageRate() > 0) - colorCam->setFps(this->getImageRate()); - if(alphaScaling_ > -1.0f) - colorCam->setCalibrationAlpha(alphaScaling_); - } - this->setImageRate(0); - - // Using VideoEncoder on PoE devices, Subpixel is not supported - if(deviceToUse.protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos) - { - auto leftOrColorEnc = p.create(); - auto depthOrRightEnc = p.create(); - leftOrColorEnc->setDefaultProfilePreset(monoLeft->getFps(), dai::VideoEncoderProperties::Profile::MJPEG); - depthOrRightEnc->setDefaultProfilePreset(monoRight->getFps(), dai::VideoEncoderProperties::Profile::MJPEG); - if(outputMode_ < 2) - { - if(imagesRectified_) { - stereo->rectifiedLeft.link(leftOrColorEnc->input); - } - else { - monoLeft->out.link(leftOrColorEnc->input); - } - leftOrColorEnc->bitstream.link(sync->inputs["left"]); - } - else - { - colorCam->video.link(leftOrColorEnc->input); - leftOrColorEnc->bitstream.link(sync->inputs["color"]); - } - if(imagesRectified_ && outputMode_) - { - depthOrRightEnc->setQuality(100); - stereo->disparity.link(depthOrRightEnc->input); - depthOrRightEnc->bitstream.link(sync->inputs["depth"]); - } - else - { - if(imagesRectified_) { - stereo->rectifiedRight.link(depthOrRightEnc->input); - } - else { - monoRight->out.link(depthOrRightEnc->input); - } - depthOrRightEnc->bitstream.link(sync->inputs["right"]); - } - } - else - { - if(stereo.get()) { - stereo->setSubpixel(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5); - if(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5) - stereo->setSubpixelFractionalBits(subpixelFractionalBits_); - auto config = stereo->initialConfig.get(); - config.costMatching.enableCompanding = compandingWidth_>0; - if(compandingWidth_>0) - config.costMatching.disparityWidth = compandingWidth_==64?dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64:dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_96; - stereo->initialConfig.set(config); - } - if(outputMode_ < 2) - { - if(imagesRectified_) - stereo->rectifiedLeft.link(sync->inputs["left"]); - else - monoLeft->out.link(sync->inputs["left"]); - } - else - { - monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); - monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); - colorCam->video.link(sync->inputs["color"]); - } - if(imagesRectified_) { - if(outputMode_) - stereo->depth.link(sync->inputs["depth"]); - else - stereo->rectifiedRight.link(sync->inputs["right"]); - } - else { - monoRight->out.link(sync->inputs["right"]); - } - } - - sync->setSyncThreshold(std::chrono::milliseconds(int(500 / monoLeft->getFps()))); - sync->out.link(xoutCamera->input); - - if(imuPublished_) - { - // enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate - imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200); - // above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available - imu->setBatchReportThreshold(1); - // maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it - // if lower or equal to batchReportThreshold then the sending is always blocking on device - // useful to reduce device's CPU load and number of lost packets, if CPU load is high on device side due to multiple nodes - imu->setMaxBatchReports(10); - - // Link plugins IMU -> XLINK - imu->out.link(xoutIMU->input); - } - - if(detectFeatures_ == 1) - { - gfttDetector->setHardwareResources(1, 2); - gfttDetector->initialConfig.setCornerDetector( - useHarrisDetector_?dai::FeatureTrackerConfig::CornerDetector::Type::HARRIS:dai::FeatureTrackerConfig::CornerDetector::Type::SHI_THOMASI); - gfttDetector->initialConfig.setNumTargetFeatures(numTargetFeatures_); - gfttDetector->initialConfig.setMotionEstimator(false); - auto cfg = gfttDetector->initialConfig.get(); - cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_; - gfttDetector->initialConfig.set(cfg); - if(stereo.get()) - stereo->rectifiedLeft.link(gfttDetector->inputImage); - else - monoLeft->out.link(gfttDetector->inputImage); - gfttDetector->outputFeatures.link(sync->inputs["feat"]); - } - else if(detectFeatures_ >= 2) - { - manip->setKeepAspectRatio(false); - manip->setMaxOutputFrameSize(320 * 200); - manip->initialConfig.setResize(320, 200); - neuralNetwork->setBlobPath(blobPath_); - neuralNetwork->setNumInferenceThreads(2); - neuralNetwork->setNumNCEPerInferenceThread(1); - neuralNetwork->input.setBlocking(false); - if(stereo.get()) - stereo->rectifiedLeft.link(manip->inputImage); - else - monoLeft->out.link(manip->inputImage); - manip->out.link(neuralNetwork->input); - neuralNetwork->out.link(sync->inputs["feat"]); - } - - device_.reset(new dai::Device(p, deviceToUse)); - - UINFO("Device serial: %s", device_->getMxId().c_str()); - UINFO("Available camera sensors: "); + device_ = std::make_unique(deviceToUse); + auto deviceName = device_->getDeviceName(); + auto imuType = device_->getConnectedIMU(); + UINFO("Device Name: %s, Device Serial: %s", deviceName.c_str(), device_->getMxId().c_str()); + UINFO("Available Camera Sensors: "); for(auto& sensor : device_->getCameraSensorNames()) { UINFO("Socket: CAM_%c - %s", 'A'+(unsigned char)sensor.first, sensor.second.c_str()); } + UINFO("IMU Type: %s", imuType.c_str()); UINFO("Loading eeprom calibration data"); - dai::CalibrationHandler calibHandler = device_->readCalibration(); + auto calibHandler = device_->readCalibration(); + auto boardName = calibHandler.getEepromData().boardName; + + stereoModel_ = StereoCameraModel(); + targetSize_ = cv::Size(imageWidth_, imageWidth_/640*((outputMode_==2&&boardName!="BC2087")?360:400)); if(!calibrationFolder.empty() && !cameraName.empty() && imagesRectified_) { @@ -591,10 +351,6 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin std::cout << "Expected K with rectification_alpha=0: " << stereoModel_.left().K()*(double(targetSize_.width)/double(stereoModel_.left().imageWidth())) << std::endl; } device_->flashCalibration2(calibHandler); - UINFO("Closing device..."); - device_->close(); - UINFO("Restarting pipeline..."); - device_.reset(new dai::Device(p, deviceToUse)); } catch(const std::runtime_error & e) { UERROR("Failed flashing calibration: %s", e.what()); @@ -609,10 +365,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin calibHandler = device_->readCalibration(); } - auto eeprom = calibHandler.getEepromData(); - UINFO("Product name: %s, board name: %s", eeprom.productName.c_str(), eeprom.boardName.c_str()); - - auto cameraId = outputMode_<2?dai::CameraBoardSocket::CAM_B:dai::CameraBoardSocket::CAM_A; + auto cameraId = outputMode_==2?dai::CameraBoardSocket::CAM_A:dai::CameraBoardSocket::CAM_B; cv::Mat cameraMatrix, distCoeffs, newCameraMatrix; std::vector > matrix = calibHandler.getCameraIntrinsics(cameraId, targetSize_.width, targetSize_.height); @@ -636,15 +389,15 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin double cy = newCameraMatrix.at(1, 2); UINFO("fx=%f fy=%f cx=%f cy=%f (target size = %dx%d)", fx, fy, cx, cy, targetSize_.width, targetSize_.height); if(outputMode_ == 2) { - stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, 0, this->getLocalTransform(), targetSize_); + stereoModel_ = StereoCameraModel(deviceName, fx, fy, cx, cy, 0, this->getLocalTransform(), targetSize_); } else { double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, false)/100.0; UINFO("baseline=%f", baseline); - stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, outputMode_==0?baseline:0, this->getLocalTransform()*Transform(-calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_A)/100.0, 0, 0), targetSize_); + stereoModel_ = StereoCameraModel(deviceName, fx, fy, cx, cy, outputMode_==0?baseline:0, this->getLocalTransform()*Transform(-calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_A)/100.0, 0, 0), targetSize_); } - if(imuPublished_) + if(imuPublished_ || imuType.empty()) { // Cannot test the following, I get "IMU calibration data is not available on device yet." with my camera // Update: now (as March 6, 2022) it crashes in "dai::CalibrationHandler::getImuToCameraExtrinsics(dai::CameraBoardSocket, bool)" @@ -653,44 +406,266 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin // matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3], // matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3], // matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]); - if(eeprom.boardName == "OAK-D" || - eeprom.boardName == "BW1098OBC") + if(deviceName == "OAK-D") { imuLocalTransform_ = Transform( 0, -1, 0, 0.0525, 1, 0, 0, 0.013662, 0, 0, 1, 0); } - else if(eeprom.boardName == "DM9098") + else if(boardName == "BC2087") // OAK-D LR + { + imuLocalTransform_ = Transform( + 1, 0, 0, 0.021425, + 0, 1, 0, 0.009925, + 0, 0, 1, 0); + } + else if(boardName == "DM2080") // OAK-D SR + { + imuLocalTransform_ = Transform( + -1, 0, 0, 0, + 0, -1, 0, -0.0024, + 0, 0, 1, 0); + } + else if(boardName == "DM9098") // OAK-D S2, OAK-D W, OAK-D Pro, OAK-D Pro W { imuLocalTransform_ = Transform( 0, 1, 0, 0.037945, 1, 0, 0, 0.00079, 0, 0, -1, 0); } - else if(eeprom.boardName == "NG2094") + else if(boardName == "NG2094") // OAK-D Pro W Dev { imuLocalTransform_ = Transform( 0, 1, 0, 0.0374, 1, 0, 0, 0.00176, 0, 0, -1, 0); } - else if(eeprom.boardName == "NG9097") + else if(boardName == "NG9097") // OAK-D S2 PoE, OAK-D W PoE, OAK-D Pro PoE, OAK-D Pro W PoE { - imuLocalTransform_ = Transform( - 0, 1, 0, 0.04, - 1, 0, 0, 0.020265, - 0, 0, -1, 0); + if(imuType == "BMI270") + { + imuLocalTransform_ = Transform( + 0, 1, 0, 0.04, + 1, 0, 0, 0.020265, + 0, 0, -1, 0); + } + else // BNO085/086 + { + imuLocalTransform_ = Transform( + 0, -1, 0, 0.04, + -1, 0, 0, 0.020265, + 0, 0, -1, 0); + } } else { - UWARN("Unknown boardName (%s)! Disabling IMU!", eeprom.boardName.c_str()); + UWARN("Unsupported boardName (%s)! Disabling IMU!", boardName.c_str()); imuPublished_ = false; } } else { UINFO("IMU disabled"); + imuPublished_ = false; + } + + dai::Pipeline pipeline; + + auto sync = pipeline.create(); + sync->setSyncThreshold(std::chrono::milliseconds(int(500 / this->getImageRate()))); + + if(outputMode_ == 2) + { + auto rgbCamera = pipeline.create(); + rgbCamera->setCamera("color"); + if(boardName == "BC2087") + rgbCamera->setSize(1920, 1200); + else if(boardName == "NG2094") + rgbCamera->setSize(1280, 720); + else + rgbCamera->setSize(1920, 1080); + rgbCamera->setSize(1920, 1200); + rgbCamera->setVideoSize(targetSize_.width, targetSize_.height); + rgbCamera->setPreviewSize(targetSize_.width, targetSize_.height); + rgbCamera->setFps(this->getImageRate()); + rgbCamera->setMeshSource(imagesRectified_?dai::CameraProperties::WarpMeshSource::CALIBRATION:dai::CameraProperties::WarpMeshSource::NONE); + if(imagesRectified_ && alphaScaling_>-1.0f) + rgbCamera->setCalibrationAlpha(alphaScaling_); + rgbCamera->properties.ispScale.horizNumerator = rgbCamera->properties.ispScale.vertNumerator = imageWidth_/640; + rgbCamera->properties.ispScale.horizDenominator = rgbCamera->properties.ispScale.vertDenominator = boardName=="NG2094"?2:3; + + auto rgbEncoder = pipeline.create(); + rgbEncoder->setDefaultProfilePreset(this->getImageRate(), dai::VideoEncoderProperties::Profile::MJPEG); + + rgbCamera->video.link(rgbEncoder->input); + rgbEncoder->bitstream.link(sync->inputs["rgb"]); + } + + auto stereoDepth = pipeline.create(); + if(outputMode_ == 2) + stereoDepth->setDepthAlign(dai::CameraBoardSocket::CAM_A); + else + stereoDepth->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT); + if(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5) + { + stereoDepth->setSubpixel(true); + stereoDepth->setSubpixelFractionalBits(subpixelFractionalBits_); + } + stereoDepth->setExtendedDisparity(extendedDisparity_); + stereoDepth->enableDistortionCorrection(true); + stereoDepth->setDisparityToDepthUseSpecTranslation(useSpecTranslation_); + stereoDepth->setDepthAlignmentUseSpecTranslation(useSpecTranslation_); + if(alphaScaling_ > -1.0f) + stereoDepth->setAlphaScaling(alphaScaling_); + stereoDepth->initialConfig.setConfidenceThreshold(confThreshold_); + stereoDepth->initialConfig.setLeftRightCheck(lrcThreshold_>=0); + if(lrcThreshold_>=0) + stereoDepth->initialConfig.setLeftRightCheckThreshold(lrcThreshold_); + stereoDepth->initialConfig.setMedianFilter(dai::MedianFilter(medianFilter_)); + auto config = stereoDepth->initialConfig.get(); + config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9; + config.censusTransform.kernelMask = 0X5092A28C5152428; + config.costMatching.disparityWidth = disparityWidth_==64?dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64:dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_96; + config.costMatching.enableCompanding = enableCompanding_; + config.costMatching.linearEquationParameters.alpha = 2; + config.costMatching.linearEquationParameters.beta = 4; + config.costAggregation.horizontalPenaltyCostP1 = 100; + config.costAggregation.horizontalPenaltyCostP2 = 500; + config.costAggregation.verticalPenaltyCostP1 = 100; + config.costAggregation.verticalPenaltyCostP2 = 500; + config.postProcessing.brightnessFilter.maxBrightness = 255; + stereoDepth->initialConfig.set(config); + + stereoDepth->depth.link(sync->inputs["depth"]); + + if(outputMode_ < 2) + { + auto leftEncoder = pipeline.create(); + leftEncoder->setDefaultProfilePreset(this->getImageRate(), dai::VideoEncoderProperties::Profile::MJPEG); + + if(imagesRectified_) + stereoDepth->rectifiedLeft.link(leftEncoder->input); + else + stereoDepth->syncedLeft.link(leftEncoder->input); + leftEncoder->bitstream.link(sync->inputs["left"]); + } + + if(!outputMode_) + { + auto rightEncoder = pipeline.create(); + rightEncoder->setDefaultProfilePreset(this->getImageRate(), dai::VideoEncoderProperties::Profile::MJPEG); + + if(imagesRectified_) + stereoDepth->rectifiedRight.link(rightEncoder->input); + else + stereoDepth->syncedRight.link(rightEncoder->input); + rightEncoder->bitstream.link(sync->inputs["right"]); + } + + if(boardName == "BC2087") + { + auto leftCamera = pipeline.create(); + leftCamera->setCamera("left"); + leftCamera->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1200_P); + leftCamera->setIspScale(imageWidth_/640, 3); + leftCamera->setFps(this->getImageRate()); + + auto rightCamera = pipeline.create(); + rightCamera->setCamera("right"); + rightCamera->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1200_P); + rightCamera->setIspScale(imageWidth_/640, 3); + rightCamera->setFps(this->getImageRate()); + + leftCamera->isp.link(stereoDepth->left); + rightCamera->isp.link(stereoDepth->right); + } + else + { + auto leftCamera = pipeline.create(); + leftCamera->setCamera("left"); + leftCamera->setResolution(imageWidth_==640?dai::MonoCameraProperties::SensorResolution::THE_400_P:dai::MonoCameraProperties::SensorResolution::THE_800_P); + leftCamera->setFps(this->getImageRate()); + + auto rightCamera = pipeline.create(); + rightCamera->setCamera("right"); + rightCamera->setResolution(imageWidth_==640?dai::MonoCameraProperties::SensorResolution::THE_400_P:dai::MonoCameraProperties::SensorResolution::THE_800_P); + rightCamera->setFps(this->getImageRate()); + + leftCamera->out.link(stereoDepth->left); + rightCamera->out.link(stereoDepth->right); + } + + if(detectFeatures_ == 1) + { + auto gfttDetector = pipeline.create(); + gfttDetector->setHardwareResources(2, 2); + gfttDetector->initialConfig.setCornerDetector( + useHarrisDetector_?dai::FeatureTrackerConfig::CornerDetector::Type::HARRIS:dai::FeatureTrackerConfig::CornerDetector::Type::SHI_THOMASI); + gfttDetector->initialConfig.setNumTargetFeatures(numTargetFeatures_); + gfttDetector->initialConfig.setMotionEstimator(false); + auto cfg = gfttDetector->initialConfig.get(); + cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_; + gfttDetector->initialConfig.set(cfg); + + if(imagesRectified_) + stereoDepth->rectifiedLeft.link(gfttDetector->inputImage); + else + stereoDepth->syncedLeft.link(gfttDetector->inputImage); + gfttDetector->outputFeatures.link(sync->inputs["feat"]); + } + else if(detectFeatures_ >= 2) + { + auto imageManip = pipeline.create(); + imageManip->setKeepAspectRatio(false); + imageManip->setMaxOutputFrameSize(320 * 200); + imageManip->initialConfig.setResize(320, 200); + + auto neuralNetwork = pipeline.create(); + neuralNetwork->setBlobPath(blobPath_); + neuralNetwork->setNumInferenceThreads(2); + neuralNetwork->setNumNCEPerInferenceThread(1); + neuralNetwork->input.setBlocking(false); + + if(imagesRectified_) + stereoDepth->rectifiedLeft.link(imageManip->inputImage); + else + stereoDepth->syncedLeft.link(imageManip->inputImage); + imageManip->out.link(neuralNetwork->input); + neuralNetwork->out.link(sync->inputs["feat"]); + } + + auto xoutCamera = pipeline.create(); + xoutCamera->setStreamName("camera"); + + sync->out.link(xoutCamera->input); + + if(imuPublished_) + { + auto imu = pipeline.create(); + if(imuType == "BMI270") + imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200); + else // BNO085/086 + imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER, dai::IMUSensor::GYROSCOPE_UNCALIBRATED}, 200); + imu->setBatchReportThreshold(boardName=="NG9097"?4:1); + imu->setMaxBatchReports(10); + + auto xoutIMU = pipeline.create(); + xoutIMU->setStreamName("imu"); + + imu->out.link(xoutIMU->input); + } + + device_->startPipeline(pipeline); + if(!device_->getIrDrivers().empty()) + { + UINFO("Setting IR intensity"); + device_->setIrLaserDotProjectorIntensity(dotIntensity_); + device_->setIrFloodLightIntensity(floodIntensity_); + } + else if(dotIntensity_ > 0 || floodIntensity_ > 0) + { + UWARN("No IR drivers were detected! IR intensity cannot be set."); } cameraQueue_ = device_->getOutputQueue("camera", 8, false); @@ -726,17 +701,6 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin }); } - if(!device_->getIrDrivers().empty()) - { - UINFO("Setting IR intensity"); - device_->setIrLaserDotProjectorIntensity(dotIntensity_); - device_->setIrFloodLightIntensity(floodIntensity_); - } - else if(dotIntensity_ > 0 || floodIntensity_ > 0) - { - UWARN("No IR drivers were detected! IR intensity cannot be set."); - } - uSleep(2000); // avoid bad frames on start return true; @@ -769,32 +733,14 @@ SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info) #ifdef RTABMAP_DEPTHAI auto messageGroup = cameraQueue_->get(); - auto rectifLeftOrColor = messageGroup->get(outputMode_<2?"left":"color"); - auto rectifRightOrDepth = messageGroup->get(imagesRectified_ && outputMode_?"depth":"right"); + auto rgbOrLeft = messageGroup->get(outputMode_==2?"rgb":"left"); + auto depthOrRight = messageGroup->get(outputMode_?"depth":"right"); - cv::Mat leftOrColor, depthOrRight; - if(device_->getDeviceInfo().protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos) - { - leftOrColor = cv::imdecode(rectifLeftOrColor->getData(), cv::IMREAD_ANYCOLOR); - depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE); - if(imagesRectified_ && outputMode_) - { - cv::Mat disp; - depthOrRight.convertTo(disp, CV_16UC1); - cv::divide(-stereoModel_.right().Tx() * 1000, disp, depthOrRight); - } - } + double stamp = std::chrono::duration(depthOrRight->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count(); + if(outputMode_) + data = SensorData(cv::imdecode(rgbOrLeft->getData(), cv::IMREAD_ANYCOLOR), depthOrRight->getCvFrame(), stereoModel_.left(), this->getNextSeqID(), stamp); else - { - leftOrColor = rectifLeftOrColor->getCvFrame(); - depthOrRight = rectifRightOrDepth->getCvFrame(); - } - - double stamp = std::chrono::duration(rectifLeftOrColor->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count(); - if(imagesRectified_ && outputMode_) - data = SensorData(leftOrColor, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp); - else - data = SensorData(leftOrColor, depthOrRight, stereoModel_, this->getNextSeqID(), stamp); + data = SensorData(cv::imdecode(rgbOrLeft->getData(), cv::IMREAD_GRAYSCALE), cv::imdecode(depthOrRight->getData(), cv::IMREAD_GRAYSCALE), stereoModel_, this->getNextSeqID(), stamp); if(imuPublished_ && !publishInterIMU_) { diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 9dd7c265..ea798b7d 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -858,13 +858,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->spinBox_stereoMyntEye_contrast, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_stereoMyntEye_irControl, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->comboBox_depthai_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_depthai_image_width, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_depthai_output_mode, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_depthai_conf_threshold, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_depthai_lrc_threshold, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_depthai_extended_disparity, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_depthai_disparity_companding, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_depthai_subpixel_fractional_bits, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->comboBox_depthai_disparity_companding, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_depthai_disparity_width, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_depthai_median_filter, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_depthai_use_spec_translation, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_depthai_alpha_scaling, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_depthai_imu_published, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); @@ -2251,12 +2253,14 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->spinBox_stereoMyntEye_brightness->setValue(120); _ui->spinBox_stereoMyntEye_contrast->setValue(116); _ui->spinBox_stereoMyntEye_irControl->setValue(0); - _ui->comboBox_depthai_resolution->setCurrentIndex(1); + _ui->comboBox_depthai_image_width->setCurrentIndex(1); _ui->comboBox_depthai_output_mode->setCurrentIndex(0); _ui->spinBox_depthai_conf_threshold->setValue(200); _ui->checkBox_depthai_extended_disparity->setChecked(false); - _ui->comboBox_depthai_subpixel_fractional_bits->setCurrentIndex(2); - _ui->comboBox_depthai_disparity_companding->setCurrentIndex(1); + _ui->checkBox_depthai_disparity_companding->setChecked(false); + _ui->comboBox_depthai_subpixel_fractional_bits->setCurrentIndex(1); + _ui->comboBox_depthai_disparity_width->setCurrentIndex(1); + _ui->comboBox_depthai_median_filter->setCurrentIndex(2); _ui->spinBox_depthai_lrc_threshold->setValue(5); _ui->checkBox_depthai_use_spec_translation->setChecked(false); _ui->doubleSpinBox_depthai_alpha_scaling->setValue(-1); @@ -2763,13 +2767,15 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) settings.endGroup(); // MyntEye settings.beginGroup("DepthAI"); - _ui->comboBox_depthai_resolution->setCurrentIndex(settings.value("resolution", _ui->comboBox_depthai_resolution->currentIndex()).toInt()); + _ui->comboBox_depthai_image_width->setCurrentIndex(settings.value("image_width", _ui->comboBox_depthai_image_width->currentIndex()).toInt()); _ui->comboBox_depthai_output_mode->setCurrentIndex(settings.value("output_mode", _ui->comboBox_depthai_output_mode->currentIndex()).toInt()); _ui->spinBox_depthai_conf_threshold->setValue(settings.value("conf_threshold", _ui->spinBox_depthai_conf_threshold->value()).toInt()); _ui->spinBox_depthai_lrc_threshold->setValue(settings.value("lrc_threshold", _ui->spinBox_depthai_lrc_threshold->value()).toInt()); _ui->checkBox_depthai_extended_disparity->setChecked(settings.value("extended_disparity", _ui->checkBox_depthai_extended_disparity->isChecked()).toBool()); + _ui->checkBox_depthai_disparity_companding->setChecked(settings.value("disparity_companding", _ui->checkBox_depthai_disparity_companding->isChecked()).toBool()); _ui->comboBox_depthai_subpixel_fractional_bits->setCurrentIndex(settings.value("subpixel_fractional_bits", _ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()).toInt()); - _ui->comboBox_depthai_disparity_companding->setCurrentIndex(settings.value("companding", _ui->comboBox_depthai_disparity_companding->currentIndex()).toInt()); + _ui->comboBox_depthai_disparity_width->setCurrentIndex(settings.value("disparity_width", _ui->comboBox_depthai_disparity_width->currentIndex()).toInt()); + _ui->comboBox_depthai_median_filter->setCurrentIndex(settings.value("median_filter", _ui->comboBox_depthai_median_filter->currentIndex()).toInt()); _ui->checkBox_depthai_use_spec_translation->setChecked(settings.value("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked()).toBool()); _ui->doubleSpinBox_depthai_alpha_scaling->setValue(settings.value("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value()).toDouble()); _ui->checkBox_depthai_imu_published->setChecked(settings.value("imu_published", _ui->checkBox_depthai_imu_published->isChecked()).toBool()); @@ -3365,20 +3371,22 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.endGroup(); // MyntEye settings.beginGroup("DepthAI"); - settings.setValue("resolution", _ui->comboBox_depthai_resolution->currentIndex()); - settings.setValue("output_mode", _ui->comboBox_depthai_output_mode->currentIndex()); - settings.setValue("conf_threshold", _ui->spinBox_depthai_conf_threshold->value()); - settings.setValue("lrc_threshold", _ui->spinBox_depthai_lrc_threshold->value()); - settings.setValue("extended_disparity", _ui->checkBox_depthai_extended_disparity->isChecked()); + settings.setValue("image_width", _ui->comboBox_depthai_image_width->currentIndex()); + settings.setValue("output_mode", _ui->comboBox_depthai_output_mode->currentIndex()); + settings.setValue("conf_threshold", _ui->spinBox_depthai_conf_threshold->value()); + settings.setValue("lrc_threshold", _ui->spinBox_depthai_lrc_threshold->value()); + settings.setValue("extended_disparity", _ui->checkBox_depthai_extended_disparity->isChecked()); + settings.setValue("disparity_companding", _ui->checkBox_depthai_disparity_companding->isChecked()); settings.setValue("subpixel_fractional_bits", _ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()); - settings.setValue("companding", _ui->comboBox_depthai_disparity_companding->currentIndex()); - settings.setValue("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked()); - settings.setValue("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value()); - settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->isChecked()); - settings.setValue("dot_intensity", _ui->doubleSpinBox_depthai_dot_intensity->value()); - settings.setValue("flood_intensity", _ui->doubleSpinBox_depthai_flood_intensity->value()); - settings.setValue("detect_features", _ui->comboBox_depthai_detect_features->currentIndex()); - settings.setValue("blob_path", _ui->lineEdit_depthai_blob_path->text()); + settings.setValue("disparity_width", _ui->comboBox_depthai_disparity_width->currentIndex()); + settings.setValue("median_filter", _ui->comboBox_depthai_median_filter->currentIndex()); + settings.setValue("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked()); + settings.setValue("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value()); + settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->isChecked()); + settings.setValue("dot_intensity", _ui->doubleSpinBox_depthai_dot_intensity->value()); + settings.setValue("flood_intensity", _ui->doubleSpinBox_depthai_flood_intensity->value()); + settings.setValue("detect_features", _ui->comboBox_depthai_detect_features->currentIndex()); + settings.setValue("blob_path", _ui->lineEdit_depthai_blob_path->text()); settings.endGroup(); // DepthAI settings.beginGroup("Images"); @@ -4287,7 +4295,7 @@ void PreferencesDialog::selectSourceDriver(Src src, int variant) else if(src == kSrcStereoDepthAI) // OAK-D-Pro (variant==2), OAK-D (variant==1), OAK-D Lite (variant==0) { _ui->checkBox_depthai_imu_published->setChecked(variant >= 1); - _ui->comboBox_depthai_resolution->setCurrentIndex(variant >= 1?1:3); + _ui->comboBox_depthai_image_width->setCurrentIndex(1); _ui->comboBox_depthai_output_mode->setCurrentIndex(variant==2?2:0); _ui->doubleSpinBox_depthai_dot_intensity->setValue(variant==2?1:0); } @@ -6863,19 +6871,18 @@ Camera * PreferencesDialog::createCamera( UDEBUG("DepthAI"); camera = new CameraDepthAI( device.toStdString().c_str(), - _ui->comboBox_depthai_resolution->currentIndex(), + _ui->comboBox_depthai_image_width->currentIndex()?1280:640, this->getGeneralInputRate(), this->getSourceLocalTransform()); ((CameraDepthAI*)camera)->setOutputMode(_ui->comboBox_depthai_output_mode->currentIndex()); ((CameraDepthAI*)camera)->setDepthProfile(_ui->spinBox_depthai_conf_threshold->value(), _ui->spinBox_depthai_lrc_threshold->value()); - ((CameraDepthAI*)camera)->setExtendedDisparity(_ui->checkBox_depthai_extended_disparity->isChecked()); + ((CameraDepthAI*)camera)->setExtendedDisparity(_ui->checkBox_depthai_extended_disparity->isChecked(), _ui->checkBox_depthai_disparity_companding->isChecked()); ((CameraDepthAI*)camera)->setSubpixelMode(_ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()!=0, _ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()==2?4:_ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()==3?5:3); - ((CameraDepthAI*)camera)->setCompanding(_ui->comboBox_depthai_disparity_companding->currentIndex()!=0, _ui->comboBox_depthai_disparity_companding->currentIndex()==1?64:96); + ((CameraDepthAI*)camera)->setDisparityWidthAndFilter(_ui->comboBox_depthai_disparity_width->currentIndex()==0?64:96, _ui->comboBox_depthai_median_filter->currentIndex()==2?5:_ui->comboBox_depthai_median_filter->currentIndex()==3?7:3); ((CameraDepthAI*)camera)->setRectification(_ui->checkBox_depthai_use_spec_translation->isChecked(), _ui->doubleSpinBox_depthai_alpha_scaling->value(), !useRawImages); ((CameraDepthAI*)camera)->setIMU(_ui->checkBox_depthai_imu_published->isChecked(), _ui->checkbox_publishInterIMU->isChecked()); ((CameraDepthAI*)camera)->setIrIntensity(_ui->doubleSpinBox_depthai_dot_intensity->value(), _ui->doubleSpinBox_depthai_flood_intensity->value()); - ((CameraDepthAI*)camera)->setDetectFeatures(_ui->comboBox_depthai_detect_features->currentIndex()); - ((CameraDepthAI*)camera)->setBlobPath(_ui->lineEdit_depthai_blob_path->text().toStdString()); + ((CameraDepthAI*)camera)->setDetectFeatures(_ui->comboBox_depthai_detect_features->currentIndex(), _ui->lineEdit_depthai_blob_path->text().toStdString()); if(_ui->comboBox_depthai_detect_features->currentIndex() == 1) { ((CameraDepthAI*)camera)->setGFTTDetector(_ui->checkBox_GFTT_useHarrisDetector->isChecked(), _ui->doubleSpinBox_GFTT_minDistance->value(), _ui->reextract_maxFeatures->value()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index afd95297..805da7ba 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -5975,33 +5975,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki DepthAI - - - - 0 - - - QComboBox::AdjustToContents - - - - Disabled - - - - - 64 pixels - - - - - 96 pixels - - - - - + 0 @@ -6010,123 +5985,20 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 720p + 640 - 800p - - - - - 400p - - - - - 480p - - - - - 1200p + 1280 - - - - -1.000000000000000 - - - 1.000000000000000 - - - 1.000000000000000 - - - 0.000000000000000 - - - - - - - <html><head/><body><p>Intensity on range 0 to 1, that will determine brightness.</p></body></html> - + + - IR laser dot projector intensity. 0 to turn off. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 0 - - - QComboBox::AdjustToContents - - - - None - - - - - GFTT - - - - - SuperPoint - - - - - HF-Net - - - - - - - - Use the translation information from the board design data (not the calibration data). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Output mode. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - On-device feature detector. + Image width. true @@ -6161,13 +6033,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - <html><head/><body><p>Disparity range increased from 0-95 to 0-190, combined from full resolution and downscaled images.</p></body></html> - + + - Extended disparity. Suitable for short range objects. Currently incompatible with sub-pixel disparity. + Output mode. true @@ -6190,26 +6059,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - -1 - - - 255 - - - 5 - - - - - - - - - - @@ -6223,55 +6072,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - <html><head/><body><p>Intensity on range 0 to 1, that will determine brightness.</p></body></html> + + + + -1 - - IR flood light intensity. 0 to turn off. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Path to MyriadX blob file. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - 1.000000000000000 - - - 0.050000000000000 + 255 - 0.000000000000000 + 5 - - + + - <html><head/><body><p>Computes disparity with sub-pixel interpolation (3 fractional bits by default). </p></body></html> + Computes and combines disparities in both L-R and R-L directions, and combine them. - Subpixel mode: number of fractional bits. Suitable for long range. Currently incompatible with extended disparity. + Disparity left-right check threshold. Set -1 to turn off. true @@ -6281,10 +6101,43 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - Resolution. + + + + + + + + <html><head/><body><p>Disparity range increased from 0-95 to 0-190, combined from full resolution and downscaled images.</p></body></html> + + + Extended disparity. Suitable for short range objects. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + <html><head/><body><p> * Matching pixel by pixel for N disparities.</p><p> * Matching every 2nd pixel for M disparitites.</p><p> * Matching every 4th pixel for T disparities.</p><p> * In case of 96 disparities: N=48, M=32, T=16.</p><p> * This way the search range is extended to 176 disparities, by sparse matching.</p><p> * Note: when enabling this flag only depth map will be affected, disparity map is not.</p></body></html> + + + Disparity companding using sparse matching. true @@ -6297,7 +6150,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 0 + 1 QComboBox::AdjustToContents @@ -6324,20 +6177,109 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + + + <html><head/><body><p>Computes disparity with sub-pixel interpolation (3 fractional bits by default). </p></body></html> + + + Subpixel mode: number of fractional bits. Suitable for long range. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + QComboBox::AdjustToContents + + + + 64 pixels + + + + + 96 pixels + + + + + + + + Disparity search range: 64 or 96 pixels are supported by the HW. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 2 + + + QComboBox::AdjustToContents + + + + MEDIAN_OFF + + + + + KERNEL 3x3 + + + + + KERNEL 5x5 + + + + + KERNEL 7x7 + + + + + + + + Set kernel size for disparity/depth median filtering, or disable. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - - - - Computes and combines disparities in both L-R and R-L directions, and combine them. - + + - Disparity left-right check threshold. Set -1 to turn off. + Use the translation information from the board design data (not the calibration data). true @@ -6348,26 +6290,22 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - + + + -1.000000000000000 - - - - 1.000000000000000 - 0.050000000000000 + 1.000000000000000 0.000000000000000 - + On some high distortion lenses, and/or due to rectification (image rotated) invalid areas may appear even with alpha=0, in these cases alpha < 0.0 helps removing invalid areas. @@ -6383,13 +6321,75 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + + + + + + + + + + IMU published + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1.000000000000000 + + + 0.050000000000000 + + + 0.000000000000000 + + + + + - <html><head/><body><p> * Matching pixel by pixel for N disparities.</p><p> * Matching every 2nd pixel for M disparitites.</p><p> * Matching every 4th pixel for T disparities.</p><p> * In case of 96 disparities: N=48, M=32, T=16.</p><p> * This way the search range is extended to 176 disparities, by sparse matching.</p><p> * Note: when enabling this flag only depth map will be affected, disparity map is not.</p></body></html> + <html><head/><body><p>Intensity on range 0 to 1, that will determine brightness.</p></body></html> - Disparity companding using sparse matching. Currently incompatible with extended disparity. + IR laser dot projector intensity. 0 to turn off. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1.000000000000000 + + + 0.050000000000000 + + + 0.000000000000000 + + + + + + + <html><head/><body><p>Intensity on range 0 to 1, that will determine brightness.</p></body></html> + + + IR flood light intensity. 0 to turn off. true @@ -6400,6 +6400,49 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + 0 + + + QComboBox::AdjustToContents + + + + None + + + + + GFTT + + + + + SuperPoint + + + + + HF-Net + + + + + + + + On-device feature detector. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + @@ -6423,10 +6466,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - IMU published + Path to MyriadX blob file. true From a1b602dbe0060c4878d8e952f98e13e1aba1f2ee Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 8 Mar 2025 16:40:04 -0800 Subject: [PATCH 17/19] Adding support for compressed input data and right color image. (#1463) --- corelib/include/rtabmap/core/SensorData.h | 6 +- .../rtabmap/core/camera/CameraStereoImages.h | 3 + .../rtabmap/core/camera/CameraStereoVideo.h | 2 + .../rtabmap/core/camera/CameraStereoZed.h | 2 + .../rtabmap/core/camera/CameraStereoZedOC.h | 3 + corelib/src/Features2d.cpp | 15 +++- corelib/src/Memory.cpp | 73 +++++++++++---- corelib/src/Odometry.cpp | 9 ++ corelib/src/OdometryThread.cpp | 9 +- corelib/src/SensorCaptureThread.cpp | 2 +- corelib/src/SensorData.cpp | 3 +- corelib/src/Stereo.cpp | 6 ++ corelib/src/camera/CameraStereoImages.cpp | 8 +- corelib/src/camera/CameraStereoVideo.cpp | 14 +-- corelib/src/camera/CameraStereoZed.cpp | 12 ++- corelib/src/camera/CameraStereoZedOC.cpp | 12 ++- corelib/src/odometry/OdometryMono.cpp | 14 +-- corelib/src/odometry/OdometryORBSLAM3.cpp | 9 +- corelib/src/stereo/StereoBM.cpp | 17 +++- corelib/src/stereo/StereoSGBM.cpp | 17 +++- corelib/src/util3d.cpp | 28 +++++- .../include/rtabmap/gui/PreferencesDialog.h | 1 + guilib/src/CameraViewer.cpp | 86 ++++++++++++------ guilib/src/DatabaseViewer.cpp | 21 +++-- guilib/src/PreferencesDialog.cpp | 20 ++++- guilib/src/ui/preferencesDialog.ui | 90 +++++++++++-------- 26 files changed, 359 insertions(+), 123 deletions(-) diff --git a/corelib/include/rtabmap/core/SensorData.h b/corelib/include/rtabmap/core/SensorData.h index 1a2ad1ab..8be56aa7 100644 --- a/corelib/include/rtabmap/core/SensorData.h +++ b/corelib/include/rtabmap/core/SensorData.h @@ -207,8 +207,8 @@ public: void setStereoCameraModels(const std::vector & stereoCameraModels) {_stereoCameraModels = stereoCameraModels;} //for convenience - cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();} - cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();} + cv::Mat depthRaw() const {return !(_depthOrRightRaw.type()==CV_8UC1 || _depthOrRightRaw.type()==CV_8UC3) ? _depthOrRightRaw : cv::Mat();} + cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1 || _depthOrRightRaw.type()==CV_8UC3 ? _depthOrRightRaw : cv::Mat();} // Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data. RTABMAP_DEPRECATED void setImageRaw(const cv::Mat & image); @@ -330,7 +330,7 @@ private: LaserScan _laserScanCompressed; // compressed data cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3 - cv::Mat _depthOrRightRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1 + cv::Mat _depthOrRightRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1 or CV_8UC3 LaserScan _laserScanRaw; std::vector _cameraModels; diff --git a/corelib/include/rtabmap/core/camera/CameraStereoImages.h b/corelib/include/rtabmap/core/camera/CameraStereoImages.h index 75b43bbf..6571d8b6 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoImages.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoImages.h @@ -56,6 +56,8 @@ public: const Transform & localTransform = Transform::getIdentity()); virtual ~CameraStereoImages(); + void setRightGrayScale(bool enabled = true) {rightGrayScale_ = enabled;} + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); virtual bool isCalibrated() const; virtual std::string getSerial() const; @@ -69,6 +71,7 @@ protected: private: CameraImages * camera2_; StereoCameraModel stereoModel_; + bool rightGrayScale_; }; diff --git a/corelib/include/rtabmap/core/camera/CameraStereoVideo.h b/corelib/include/rtabmap/core/camera/CameraStereoVideo.h index 72c7ac56..202f7ceb 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoVideo.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoVideo.h @@ -70,6 +70,7 @@ public: void setResolution(int width, int height) {_width=width, _height=height;} void setFOURCC(const std::string & fourcc) { _fourcc = fourcc; } + void setRightGrayScale(bool enabled = true) {rightGrayScale_ = enabled;} protected: virtual SensorData captureImage(SensorCaptureInfo * info = 0); @@ -88,6 +89,7 @@ private: int _width; int _height; std::string _fourcc; + bool rightGrayScale_; }; } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/camera/CameraStereoZed.h b/corelib/include/rtabmap/core/camera/CameraStereoZed.h index de3a94c2..b5e9d50e 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoZed.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoZed.h @@ -79,6 +79,7 @@ public: virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.0); void postInterIMUPublic(const IMU & imu, double stamp); + void setRightGrayScale(bool enabled = true); protected: virtual SensorData captureImage(SensorCaptureInfo * info = 0); @@ -100,6 +101,7 @@ private: bool computeOdometry_; bool lost_; bool force3DoF_; + bool rightGrayScale_; ZedIMUThread * imuPublishingThread_; #endif }; diff --git a/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h b/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h index 2399b7b1..dd8a55c9 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h @@ -62,6 +62,8 @@ public: virtual bool isCalibrated() const; virtual std::string getSerial() const; + void setRightGrayScale(bool enabled = true); + protected: virtual SensorData captureImage(SensorCaptureInfo * info = 0); @@ -74,6 +76,7 @@ private: int usbDevice_; int resolution_; uint64_t lastStamp_; + bool rightGrayScale_; #endif }; diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp index 5aa67230..5e674ee0 100644 --- a/corelib/src/Features2d.cpp +++ b/corelib/src/Features2d.cpp @@ -908,25 +908,36 @@ std::vector Feature2D::generateKeypoints3D( if(d_imageLeft.empty()) { d_imageLeft = cv::cuda::GpuMat(imageLeft); } - // convert to grayscale + // convert to grayscale if not already if(d_imageLeft.channels() > 1) { cv::cuda::GpuMat tmp; cv::cuda::cvtColor(d_imageLeft, tmp, cv::COLOR_BGR2GRAY); d_imageLeft = tmp; } + d_imageRight = data.depthOrRightRawGpu(); if(d_imageRight.empty()) { d_imageRight = cv::cuda::GpuMat(imageRight); } + // convert to grayscale if not already + if(d_imageRight.channels() > 1) { + cv::cuda::GpuMat tmp; + cv::cuda::cvtColor(d_imageRight, tmp, cv::COLOR_BGR2GRAY); + d_imageRight = tmp; + } } else #endif { - // convert to grayscale (right image should be already grayscale) + // convert to grayscale if(imageLeft.channels() > 1) { cv::cvtColor(data.imageRaw(), imageLeft, cv::COLOR_BGR2GRAY); } + if(imageRight.channels() > 1) + { + cv::cvtColor(data.rightRaw(), imageRight, cv::COLOR_BGR2GRAY); + } } std::vector leftCorners; diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 590d4cb0..30bcde29 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -4519,17 +4519,55 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor { UDEBUG(""); SensorData data = inputData; + + bool isIntermediateNode = data.id() < 0; + + // uncompress data if needed + + if(!isIntermediateNode) + { + // We need raw images if we need to extract features and/or do tag detection + bool needRawImages = _feature2D->getMaxFeatures() >= 0 && + (!_useOdometryFeatures || + data.keypoints().empty() || + (int)data.keypoints().size() != data.descriptors().rows || + data.descriptors().empty() || + _detectMarkers || + _rotateImagesUpsideUp || + _imagePostDecimation > 1 || + (_createOccupancyGrid && _localMapMaker->isGridFromDepth())); + + // Note: we could avoid uncompressing scan if we don't do any filtering + // and if we don't use it for local occupancy grid + bool needRawScan = true; + + if( (needRawImages && data.imageRaw().empty() && !data.imageCompressed().empty()) || + (needRawImages && data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty()) || + (needRawScan && data.laserScanRaw().empty() && !data.laserScanCompressed().empty())) + { + cv::Mat left, right; + LaserScan laserScan; + UDEBUG("Uncompressing data..."); + data.uncompressData( + needRawImages && data.imageRaw().empty() && !data.imageCompressed().empty() ? &left : 0, + needRawImages && data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty() ? &right : 0, + needRawScan && data.laserScanRaw().empty() && !data.laserScanCompressed().empty() ? &laserScan : 0); + UDEBUG("Uncompressing data...done!"); + } + } + UASSERT(data.imageRaw().empty() || data.imageRaw().type() == CV_8UC1 || data.imageRaw().type() == CV_8UC3); UASSERT_MSG(data.depthOrRightRaw().empty() || ( ( data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1 || - data.depthOrRightRaw().type() == CV_8UC1) + data.depthOrRightRaw().type() == CV_8UC1 || + data.depthOrRightRaw().type() == CV_8UC3) && - ( (data.imageRaw().empty() && data.depthOrRightRaw().type() != CV_8UC1) || + ( (data.imageRaw().empty() && !(data.depthOrRightRaw().type() == CV_8UC1 || data.depthOrRightRaw().type() == CV_8UC3)) || (data.depthOrRightRaw().rows <= data.imageRaw().rows && data.depthOrRightRaw().cols <= data.imageRaw().cols))), - uFormat("image=(%d/%d, type=%d, [accepted=%d,%d]) depth=(%d/%d, type=%d [accepted=%d(depth mm),%d(depth m),%d(stereo)]). " + uFormat("image=(%d/%d, type=%d, [accepted=%d,%d]) depth=(%d/%d, type=%d [accepted=%d(depth mm),%d(depth m),%d-%d(stereo)]). " "For stereo, left and right images should be same size. " "For RGB-D, depth can be X times smaller than RGB (where X is an integer).", data.imageRaw().cols, @@ -4540,7 +4578,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor data.depthOrRightRaw().cols, data.depthOrRightRaw().rows, data.depthOrRightRaw().type(), - CV_16UC1, CV_32FC1, CV_8UC1).c_str()); + CV_16UC1, CV_32FC1, CV_8UC1, CV_8UC3).c_str()); if(!data.depthOrRightRaw().empty() && data.cameraModels().empty() && @@ -4559,7 +4597,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor float t; std::vector keypoints; cv::Mat descriptors; - bool isIntermediateNode = data.id() < 0; int id = data.id(); if(_generateIds) { @@ -5754,8 +5791,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor } } - // Filter the laser scan? LaserScan laserScan = data.laserScanRaw(); + // Filter the laser scan? if(!isIntermediateNode && laserScan.size()) { if(laserScan.rangeMax() == 0.0f) @@ -5902,12 +5939,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor !stereoCameraModels.empty()? SensorData( laserScan.angleIncrement() == 0.0f? - LaserScan(compressedScan, + LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan, laserScan.maxPoints(), laserScan.rangeMax(), laserScan.format(), laserScan.localTransform()): - LaserScan(compressedScan, + LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan, laserScan.format(), laserScan.rangeMin(), laserScan.rangeMax(), @@ -5915,20 +5952,20 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor laserScan.angleMax(), laserScan.angleIncrement(), laserScan.localTransform()), - compressedImage, - compressedDepth, + compressedImage.empty()?data.imageCompressed():compressedImage, + compressedDepth.empty()?data.depthOrRightCompressed():compressedDepth, stereoCameraModels, id, 0, compressedUserData): SensorData( laserScan.angleIncrement() == 0.0f? - LaserScan(compressedScan, + LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan, laserScan.maxPoints(), laserScan.rangeMax(), laserScan.format(), laserScan.localTransform()): - LaserScan(compressedScan, + LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan, laserScan.format(), laserScan.rangeMin(), laserScan.rangeMax(), @@ -5936,8 +5973,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor laserScan.angleMax(), laserScan.angleIncrement(), laserScan.localTransform()), - compressedImage, - compressedDepth, + compressedImage.empty()?data.imageCompressed():compressedImage, + compressedDepth.empty()?data.depthOrRightCompressed():compressedDepth, cameraModels, id, 0, @@ -5986,12 +6023,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor !stereoCameraModels.empty()? SensorData( laserScan.angleIncrement() == 0.0f? - LaserScan(compressedScan, + LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan, laserScan.maxPoints(), laserScan.rangeMax(), laserScan.format(), laserScan.localTransform()): - LaserScan(compressedScan, + LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan, laserScan.format(), laserScan.rangeMin(), laserScan.rangeMax(), @@ -6007,12 +6044,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor compressedUserData): SensorData( laserScan.angleIncrement() == 0.0f? - LaserScan(compressedScan, + LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan, laserScan.maxPoints(), laserScan.rangeMax(), laserScan.format(), laserScan.localTransform()): - LaserScan(compressedScan, + LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan, laserScan.format(), laserScan.rangeMin(), laserScan.rangeMax(), diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 7d7bd177..f03dc626 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -337,6 +337,15 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet } } + if((data.imageRaw().empty() && !data.imageCompressed().empty()) || + (data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty()) || + (data.laserScanRaw().empty() && !data.laserScanCompressed().empty())) + { + UDEBUG("Received compressed data, uncompressing..."); + data.uncompressData(); + UDEBUG("Received compressed data, uncompressing...done!"); + } + if(!data.imageRaw().empty()) { UDEBUG("Processing image data %dx%d: rgbd models=%ld, stereo models=%ld", diff --git a/corelib/src/OdometryThread.cpp b/corelib/src/OdometryThread.cpp index ccf064a9..4ebbaa70 100644 --- a/corelib/src/OdometryThread.cpp +++ b/corelib/src/OdometryThread.cpp @@ -123,6 +123,9 @@ void OdometryThread::mainLoop() { UDEBUG("Odom pose = %s", pose.prettyPrint().c_str()); // a null pose notify that odometry could not be computed + data.setImageRaw(cv::Mat()); + if(!data.depthOrRightCompressed().empty()) + data.setDepthOrRightRaw(cv::Mat()); this->post(new OdometryEvent(data, pose, info)); } } @@ -155,7 +158,11 @@ void OdometryThread::addData(const SensorData & data) bool notify = true; _dataMutex.lock(); { - if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty() || data.imu().empty()) + if( !data.imageRaw().empty() || + !data.imageCompressed().empty() || + !data.laserScanRaw().isEmpty() || + !data.laserScanCompressed().empty() || + data.imu().empty()) { _dataBuffer.push_back(data); while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize) diff --git a/corelib/src/SensorCaptureThread.cpp b/corelib/src/SensorCaptureThread.cpp index c4ad71e4..8941c3d0 100644 --- a/corelib/src/SensorCaptureThread.cpp +++ b/corelib/src/SensorCaptureThread.cpp @@ -530,7 +530,7 @@ void SensorCaptureThread::mainLoop() info.odomPose.setNull(); } - if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set + if(!data.imageCompressed().empty() || !data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set { postUpdate(&data, &info); info.cameraName = _lidar?_lidar->getSerial():_camera->getSerial(); diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index 147e854d..e2c23311 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -365,7 +365,8 @@ void SensorData::setStereoImage( } else if(!right.empty()) { - UASSERT(right.type() == CV_8UC1); // Mono + UASSERT(right.type() == CV_8UC1 || // Mono + right.type() == CV_8UC3); // RGB _depthOrRightRaw = right; if(clearData) { diff --git a/corelib/src/Stereo.cpp b/corelib/src/Stereo.cpp index 448cf8cb..0269f2b5 100644 --- a/corelib/src/Stereo.cpp +++ b/corelib/src/Stereo.cpp @@ -79,6 +79,8 @@ std::vector Stereo::computeCorrespondences( const std::vector & leftCorners, std::vector & status) const { + UASSERT(leftImage.type() == CV_8UC1); + UASSERT(rightImage.type() == CV_8UC1); std::vector rightCorners; UDEBUG("util2d::calcStereoCorrespondences() begin"); rightCorners = util2d::calcStereoCorrespondences( @@ -145,6 +147,8 @@ std::vector StereoOpticalFlow::computeCorrespondences( const std::vector & leftCorners, std::vector & status) const { + UASSERT(leftImage.type() == CV_8UC1); + UASSERT(rightImage.type() == CV_8UC1); std::vector rightCorners; std::vector err; #ifdef HAVE_OPENCV_CUDAOPTFLOW @@ -184,6 +188,8 @@ std::vector StereoOpticalFlow::computeCorrespondences( { std::vector rightCorners; #ifdef HAVE_OPENCV_CUDAOPTFLOW + UASSERT(leftImage.type() == CV_8UC1); + UASSERT(rightImage.type() == CV_8UC1); UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer host to device begin"); cv::cuda::GpuMat d_leftImage(leftImage); cv::cuda::GpuMat d_rightImage(rightImage); diff --git a/corelib/src/camera/CameraStereoImages.cpp b/corelib/src/camera/CameraStereoImages.cpp index 0a7d3915..c50a3b89 100644 --- a/corelib/src/camera/CameraStereoImages.cpp +++ b/corelib/src/camera/CameraStereoImages.cpp @@ -44,7 +44,8 @@ CameraStereoImages::CameraStereoImages( float imageRate, const Transform & localTransform) : CameraImages(pathLeftImages, imageRate, localTransform), - camera2_(new CameraImages(pathRightImages)) + camera2_(new CameraImages(pathRightImages)), + rightGrayScale_(true) { this->setImagesRectified(rectifyImages); } @@ -55,7 +56,8 @@ CameraStereoImages::CameraStereoImages( float imageRate, const Transform & localTransform) : CameraImages("", imageRate, localTransform), - camera2_(0) + camera2_(0), + rightGrayScale_(true) { std::vector paths = uListToVector(uSplit(pathLeftRightImages, uStrContains(pathLeftRightImages, ":")?':':';')); if(paths.size() >= 1) @@ -179,7 +181,7 @@ SensorData CameraStereoImages::captureImage(SensorCaptureInfo * info) // Rectification cv::Mat leftImage = left.imageRaw(); cv::Mat rightImage = right.imageRaw(); - if(rightImage.type() != CV_8UC1) + if(rightImage.type() != CV_8UC1 && rightGrayScale_) { cv::Mat tmp; cv::cvtColor(rightImage, tmp, CV_BGR2GRAY); diff --git a/corelib/src/camera/CameraStereoVideo.cpp b/corelib/src/camera/CameraStereoVideo.cpp index 3b1f1c41..f31f40b6 100644 --- a/corelib/src/camera/CameraStereoVideo.cpp +++ b/corelib/src/camera/CameraStereoVideo.cpp @@ -56,7 +56,8 @@ CameraStereoVideo::CameraStereoVideo( usbDevice_(0), usbDevice2_(-1), _width(0), - _height(0) + _height(0), + rightGrayScale_(true) { } @@ -74,7 +75,8 @@ CameraStereoVideo::CameraStereoVideo( usbDevice_(0), usbDevice2_(-1), _width(0), - _height(0) + _height(0), + rightGrayScale_(true) { } @@ -89,7 +91,8 @@ CameraStereoVideo::CameraStereoVideo( usbDevice_(device), usbDevice2_(-1), _width(0), - _height(0) + _height(0), + rightGrayScale_(true) { } @@ -105,7 +108,8 @@ CameraStereoVideo::CameraStereoVideo( usbDevice_(deviceLeft), usbDevice2_(deviceRight), _width(0), - _height(0) + _height(0), + rightGrayScale_(true) { } @@ -379,7 +383,7 @@ SensorData CameraStereoVideo::captureImage(SensorCaptureInfo * info) // Rectification bool rightCvt = false; - if(rightImage.type() != CV_8UC1) + if(rightImage.type() != CV_8UC1 && rightGrayScale_) { cv::Mat tmp; cv::cvtColor(rightImage, tmp, CV_BGR2GRAY); diff --git a/corelib/src/camera/CameraStereoZed.cpp b/corelib/src/camera/CameraStereoZed.cpp index c3ea3a6d..2fc57a26 100644 --- a/corelib/src/camera/CameraStereoZed.cpp +++ b/corelib/src/camera/CameraStereoZed.cpp @@ -783,7 +783,10 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info) #endif cv::Mat rgbaRight = slMat2cvMat(tmp); cv::Mat right; - cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY); + if(rightGrayScale_) + cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY); + else + cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2BGR); #if ZED_SDK_MAJOR_VERSION < 3 data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now()); #else @@ -891,4 +894,11 @@ void CameraStereoZed::postInterIMUPublic(const IMU & imu, double stamp) postInterIMU(imu, stamp); } +void CameraStereoZed::setRightGrayScale(bool enabled) +{ +#ifdef RTABMAP_ZED + rightGrayScale_ = enabled; +#endif +} + } // namespace rtabmap diff --git a/corelib/src/camera/CameraStereoZedOC.cpp b/corelib/src/camera/CameraStereoZedOC.cpp index 5b961fe3..014976a6 100644 --- a/corelib/src/camera/CameraStereoZedOC.cpp +++ b/corelib/src/camera/CameraStereoZedOC.cpp @@ -755,7 +755,10 @@ SensorData CameraStereoZedOC::captureImage(SensorCaptureInfo * info) // ----> Extract left and right images from side-by-side left = frameBGR(cv::Rect(0, 0, frameBGR.cols / 2, frameBGR.rows)); - cv::cvtColor(frameBGR(cv::Rect(frameBGR.cols / 2, 0, frameBGR.cols / 2, frameBGR.rows)),right,cv::COLOR_BGR2GRAY); + if(rightGrayScale_) + cv::cvtColor(frameBGR(cv::Rect(frameBGR.cols / 2, 0, frameBGR.cols / 2, frameBGR.rows)),right,cv::COLOR_BGR2GRAY); + else + right = frameBGR(cv::Rect(frameBGR.cols / 2, 0, frameBGR.cols / 2, frameBGR.rows)); // <---- Extract left and right images from side-by-side if(stereoModel_.isValidForRectification()) @@ -792,4 +795,11 @@ SensorData CameraStereoZedOC::captureImage(SensorCaptureInfo * info) return data; } +void CameraStereoZedOC::setRightGrayScale(bool enabled) +{ +#ifdef RTABMAP_ZEDOC + rightGrayScale_ = enabled; +#endif +} + } // namespace rtabmap diff --git a/corelib/src/odometry/OdometryMono.cpp b/corelib/src/odometry/OdometryMono.cpp index 60bf31d5..7df312e7 100644 --- a/corelib/src/odometry/OdometryMono.cpp +++ b/corelib/src/odometry/OdometryMono.cpp @@ -205,17 +205,21 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu int nFeatures = 0; // convert to grayscale - if(data.imageRaw().channels() > 1) + if(data.imageRaw().channels() > 1 || data.rightRaw().channels() > 1) { - cv::Mat newFrame; - cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY); + cv::Mat newFrame = data.imageRaw(); + cv::Mat newFrameRight = data.rightRaw(); + if(data.imageRaw().channels() > 1) + cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY); + if(data.rightRaw().channels() > 1) + cv::cvtColor(data.rightRaw(), newFrameRight, cv::COLOR_BGR2GRAY); if(!data.stereoCameraModels().empty()) { - data.setStereoImage(newFrame, data.rightRaw(), data.stereoCameraModels()); + data.setStereoImage(newFrame, newFrameRight, data.stereoCameraModels()); } else { - data.setRGBDImage(newFrame, data.depthRaw(), data.cameraModels()); + data.setRGBDImage(newFrame, newFrameRight, data.cameraModels()); } } diff --git a/corelib/src/odometry/OdometryORBSLAM3.cpp b/corelib/src/odometry/OdometryORBSLAM3.cpp index 64b09d42..7cde70a5 100644 --- a/corelib/src/odometry/OdometryORBSLAM3.cpp +++ b/corelib/src/odometry/OdometryORBSLAM3.cpp @@ -448,11 +448,16 @@ Transform OdometryORBSLAM3::computeTransform( { localTransform = data.stereoCameraModels()[0].localTransform(); cv::Mat leftMono = data.imageRaw(); - if(data.imageRaw().channels() == 3) { + if(data.imageRaw().channels() == 3) { leftMono = cv::Mat(); cv::cvtColor(data.imageRaw(), leftMono, CV_BGR2GRAY); } - Tcw = orbslam_->TrackStereo(leftMono, data.rightRaw(), data.stamp(), orbslamImus_); + cv::Mat rightMono = data.rightRaw(); + if(data.rightRaw().channels() == 3) { + rightMono = cv::Mat(); + cv::cvtColor(data.imageRaw(), rightMono, CV_BGR2GRAY); + } + Tcw = orbslam_->TrackStereo(leftMono, rightMono, data.stamp(), orbslamImus_); orbslamImus_.clear(); } else diff --git a/corelib/src/stereo/StereoBM.cpp b/corelib/src/stereo/StereoBM.cpp index 93958293..1b390c68 100644 --- a/corelib/src/stereo/StereoBM.cpp +++ b/corelib/src/stereo/StereoBM.cpp @@ -82,7 +82,8 @@ cv::Mat StereoBM::computeDisparity( { UASSERT(!leftImage.empty() && !rightImage.empty()); UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows); - UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1); + UASSERT(leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3); + UASSERT(rightImage.type() == CV_8UC1 || rightImage.type() == CV_8UC3); cv::Mat leftMono; if(leftImage.channels() == 3) @@ -94,6 +95,16 @@ cv::Mat StereoBM::computeDisparity( leftMono = leftImage; } + cv::Mat rightMono; + if(rightImage.channels() == 3) + { + cv::cvtColor(rightImage, rightMono, CV_BGR2GRAY); + } + else + { + rightMono = rightImage; + } + cv::Mat disparity; #if CV_MAJOR_VERSION < 3 cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET); @@ -106,7 +117,7 @@ cv::Mat StereoBM::computeDisparity( stereo.state->textureThreshold = textureThreshold_; stereo.state->speckleWindowSize = speckleWindowSize_; stereo.state->speckleRange = speckleRange_; - stereo(leftMono, rightImage, disparity, CV_16SC1); + stereo(leftMono, rightMono, disparity, CV_16SC1); #else cv::Ptr stereo = cv::StereoBM::create(); stereo->setBlockSize(blockSize_); @@ -119,7 +130,7 @@ cv::Mat StereoBM::computeDisparity( stereo->setSpeckleWindowSize(speckleWindowSize_); stereo->setSpeckleRange(speckleRange_); stereo->setDisp12MaxDiff(disp12MaxDiff_); - stereo->compute(leftMono, rightImage, disparity); + stereo->compute(leftMono, rightMono, disparity); #endif if(minDisparity_>0) diff --git a/corelib/src/stereo/StereoSGBM.cpp b/corelib/src/stereo/StereoSGBM.cpp index 02c2fffe..6763aeb6 100644 --- a/corelib/src/stereo/StereoSGBM.cpp +++ b/corelib/src/stereo/StereoSGBM.cpp @@ -71,7 +71,8 @@ cv::Mat StereoSGBM::computeDisparity( { UASSERT(!leftImage.empty() && !rightImage.empty()); UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows); - UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1); + UASSERT(leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3); + UASSERT(rightImage.type() == CV_8UC1 || rightImage.type() == CV_8UC3); cv::Mat leftMono; if(leftImage.channels() == 3) @@ -83,6 +84,16 @@ cv::Mat StereoSGBM::computeDisparity( leftMono = leftImage; } + cv::Mat rightMono; + if(rightImage.channels() == 3) + { + cv::cvtColor(rightImage, rightMono, CV_BGR2GRAY); + } + else + { + rightMono = rightImage; + } + cv::Mat disparity; #if CV_MAJOR_VERSION < 3 cv::StereoSGBM stereo( @@ -97,7 +108,7 @@ cv::Mat StereoSGBM::computeDisparity( speckleWindowSize_, speckleRange_, mode_==1); - stereo(leftMono, rightImage, disparity); + stereo(leftMono, rightMono, disparity); #else cv::Ptr stereo = cv::StereoSGBM::create( minDisparity_, @@ -111,7 +122,7 @@ cv::Mat StereoSGBM::computeDisparity( speckleWindowSize_, speckleRange_, mode_); - stereo->compute(leftMono, rightImage, disparity); + stereo->compute(leftMono, rightMono, disparity); #endif if(minDisparity_>0) diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index 0fa21329..e70e5c37 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -822,14 +822,14 @@ pcl::PointCloud::Ptr cloudFromStereoImages( const ParametersMap & parameters) { UASSERT(!imageLeft.empty() && !imageRight.empty()); - UASSERT(imageRight.type() == CV_8UC1); + UASSERT(imageRight.type() == CV_8UC1 || imageRight.type() == CV_8UC3); UASSERT(imageLeft.channels() == 3 || imageLeft.channels() == 1); UASSERT(imageLeft.rows == imageRight.rows && imageLeft.cols == imageRight.cols); UASSERT(decimation >= 1.0f); cv::Mat leftColor = imageLeft; - cv::Mat rightMono = imageRight; + cv::Mat rightColor = imageRight; cv::Mat leftMono; if(leftColor.channels() == 3) @@ -841,6 +841,16 @@ pcl::PointCloud::Ptr cloudFromStereoImages( leftMono = leftColor; } + cv::Mat rightMono; + if(rightColor.channels() == 3) + { + cv::cvtColor(rightColor, rightMono, CV_BGR2GRAY); + } + else + { + rightMono = rightColor; + } + return cloudFromDisparityRGB( leftColor, util2d::disparityFromStereoImages(leftMono, rightMono, parameters), @@ -954,7 +964,7 @@ std::vector::Ptr> cloudsFromSensorData( else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && !sensorData.stereoCameraModels().empty()) { //stereo - UASSERT(sensorData.rightRaw().type() == CV_8UC1); + UASSERT(sensorData.rightRaw().type() == CV_8UC1 || sensorData.rightRaw().type() == CV_8UC3); cv::Mat leftMono; if(sensorData.imageRaw().channels() == 3) @@ -966,6 +976,16 @@ std::vector::Ptr> cloudsFromSensorData( leftMono = sensorData.imageRaw(); } + cv::Mat rightMono; + if(sensorData.rightRaw().channels() == 3) + { + cv::cvtColor(sensorData.rightRaw(), rightMono, CV_BGR2GRAY); + } + else + { + rightMono = sensorData.rightRaw(); + } + UASSERT(int((sensorData.imageRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.imageRaw().cols); UASSERT(int((sensorData.rightRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.rightRaw().cols); int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size(); @@ -979,7 +999,7 @@ std::vector::Ptr> cloudsFromSensorData( if(sensorData.stereoCameraModels()[i].isValidForProjection()) { cv::Mat left(leftMono, cv::Rect(subImageWidth*i, 0, subImageWidth, leftMono.rows)); - cv::Mat right(sensorData.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.rightRaw().rows)); + cv::Mat right(rightMono, cv::Rect(subImageWidth*i, 0, subImageWidth, rightMono.rows)); StereoCameraModel model = sensorData.stereoCameraModels()[i]; if( roiRatios.size() == 4 && ((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) || diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index b5bb4104..dfde9f53 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -278,6 +278,7 @@ public: bool isSourceFeatureDetection() const; bool isSourceStereoDepthGenerated() const; bool isSourceStereoExposureCompensation() const; + bool isRightGrayScale() const; bool isSourceScanFromDepth() const; bool isSourceScanDeskewing() const; int getSourceScanDownsampleStep() const; diff --git a/guilib/src/CameraViewer.cpp b/guilib/src/CameraViewer.cpp index 3f21403e..1b1ce5de 100644 --- a/guilib/src/CameraViewer.cpp +++ b/guilib/src/CameraViewer.cpp @@ -132,9 +132,23 @@ void CameraViewer::showImage(const rtabmap::SensorData & data) { processingImages_ = true; QString sizes; - imageView_->setVisible(!data.imageRaw().empty() || !data.imageRaw().empty()); + + cv::Mat left; + cv::Mat depthOrRight; + LaserScan scan; + if( !data.imageRaw().empty() || !data.imageCompressed().empty() || + !data.depthOrRightRaw().empty() || !data.depthOrRightCompressed().empty() || + !data.laserScanRaw().empty() || !data.laserScanCompressed().empty()) + { + data.uncompressDataConst( + !data.imageRaw().empty() || !data.imageCompressed().empty()?&left:0, + !data.depthOrRightRaw().empty() || !data.depthOrRightCompressed().empty()?&depthOrRight:0, + !data.laserScanRaw().empty() || !data.laserScanCompressed().empty()?&scan:0); + } + + imageView_->setVisible(!left.empty() || !left.empty()); std::map detections; - if(!data.imageRaw().empty()) + if(!left.empty()) { std::vector models; if(markerCheckbox_->isEnabled() && markerCheckbox_->isChecked()) @@ -152,36 +166,58 @@ void CameraViewer::showImage(const rtabmap::SensorData & data) if(!models.empty() && models[0].isValidForProjection()) { cv::Mat imageWithDetections; - detections = markerDetector_->detect(data.imageRaw(), models, data.depthRaw(), std::map(), &imageWithDetections); + detections = markerDetector_->detect(left, models, depthOrRight, std::map(), &imageWithDetections); imageView_->setImage(uCvMat2QImage(imageWithDetections)); } else { - imageView_->setImage(uCvMat2QImage(data.imageRaw())); + imageView_->setImage(uCvMat2QImage(left)); } - sizes.append(QString("Color=%1x%2").arg(data.imageRaw().cols).arg(data.imageRaw().rows)); + sizes.append(QString("Color=%1x%2").arg(left.cols).arg(left.rows)); } - if(!data.depthOrRightRaw().empty()) + if(!depthOrRight.empty()) { - imageView_->setImageDepth(data.depthOrRightRaw()); - sizes.append(QString(" Depth=%1x%2").arg(data.depthOrRightRaw().cols).arg(data.depthOrRightRaw().rows)); + imageView_->setImageDepth(depthOrRight); + sizes.append(QString(" Depth=%1x%2").arg(depthOrRight.cols).arg(depthOrRight.rows)); } imageSizeLabel_->setText(sizes); - if(!data.depthOrRightRaw().empty() && + if(!depthOrRight.empty() && ((data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection()) || (data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection()))) { if(showCloudCheckbox_->isChecked()) { - if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty()) + if(!left.empty() && !depthOrRight.empty()) { showCloudCheckbox_->setEnabled(true); - cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_)); + if(data.imageRaw().empty()) + { + if(!data.stereoCameraModels().empty()) + { + cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(SensorData(left, depthOrRight, data.stereoCameraModels()), decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_)); + } + else + { + cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(SensorData(left, depthOrRight, data.cameraModels()), decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_)); + } + + } + else + { + cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_)); + } } - else if(!data.depthOrRightRaw().empty()) + else if(!depthOrRight.empty()) { showCloudCheckbox_->setEnabled(true); - cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_)); + if(data.depthOrRightRaw().empty()) + { + cloudView_->addCloud("cloud", util3d::cloudFromSensorData(SensorData(cv::Mat(), depthOrRight, data.cameraModels()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_)); + } + else + { + cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_)); + } } // Add landmarks to 3D Map view @@ -208,37 +244,37 @@ void CameraViewer::showImage(const rtabmap::SensorData & data) } } - if(!data.laserScanRaw().isEmpty()) + if(!scan.isEmpty()) { showScanCheckbox_->setEnabled(true); if(showScanCheckbox_->isChecked()) { - if(data.laserScanRaw().hasNormals()) + if(scan.hasNormals()) { - if(data.laserScanRaw().hasIntensity()) + if(scan.hasIntensity()) { - cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudINormal(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow); + cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudINormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow); } - else if(data.laserScanRaw().hasRGB()) + else if(scan.hasRGB()) { - cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGBNormal(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow); + cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGBNormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow); } else { - cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudNormal(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow); + cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudNormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow); } } - else if(data.laserScanRaw().hasIntensity()) + else if(scan.hasIntensity()) { - cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudI(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow); + cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudI(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow); } - else if(data.laserScanRaw().hasRGB()) + else if(scan.hasRGB()) { - cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGB(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow); + cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGB(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow); } else { - cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow); + cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow); } } } diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index efd5fbd4..141188f6 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -4850,7 +4850,7 @@ void DatabaseViewer::update(int value, { keypoints.insert(std::make_pair(iter->first, signatures.front()->getWordsKpts()[iter->second])); } - view->setFeatures(keypoints, data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow); + view->setFeatures(keypoints, data.depthOrRightRaw().type() == CV_8UC1||data.depthOrRightRaw().type() == CV_8UC3?cv::Mat():data.depthOrRightRaw(), Qt::yellow); } Transform odomPose, g; @@ -5088,7 +5088,7 @@ void DatabaseViewer::update(int value, } //stereo - if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1) + if(!data.depthOrRightRaw().empty() && (data.depthOrRightRaw().type() == CV_8UC1 || data.depthOrRightRaw().type() == CV_8UC3)) { this->updateStereo(&data); } @@ -5782,7 +5782,7 @@ void DatabaseViewer::updateStereo(const SensorData * data) ui_->dockWidget_stereoView->isVisible() && !data->imageRaw().empty() && !data->depthOrRightRaw().empty() && - data->depthOrRightRaw().type() == CV_8UC1 && + (data->depthOrRightRaw().type() == CV_8UC1 || data->depthOrRightRaw().type() == CV_8UC3) && data->stereoCameraModels().size()==1 && // Not implemented for multiple stereo cameras data->stereoCameraModels()[0].isValidForProjection()) { @@ -5795,6 +5795,15 @@ void DatabaseViewer::updateStereo(const SensorData * data) { leftMono = data->imageRaw(); } + cv::Mat rightMono; + if(data->rightRaw().channels() == 3) + { + cv::cvtColor(data->rightRaw(), rightMono, CV_BGR2GRAY); + } + else + { + rightMono = data->rightRaw(); + } UTimer timer; ParametersMap parameters = ui_->parameters_toolbox->getParameters(); @@ -5826,7 +5835,7 @@ void DatabaseViewer::updateStereo(const SensorData * data) rightCorners = stereo->computeCorrespondences( leftMono, - data->rightRaw(), + rightMono, leftCorners, status); delete stereo; @@ -6435,7 +6444,7 @@ void DatabaseViewer::updateConstraintView( } dataFrom.uncompressData(); UASSERT(dataFrom.imageRaw().empty() || dataFrom.imageRaw().type()==CV_8UC3 || dataFrom.imageRaw().type() == CV_8UC1); - UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1); + UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type()==CV_8UC3 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1); if(signatureTo.id()>0) { @@ -6447,7 +6456,7 @@ void DatabaseViewer::updateConstraintView( } dataTo.uncompressData(); UASSERT(dataTo.imageRaw().empty() || dataTo.imageRaw().type()==CV_8UC3 || dataTo.imageRaw().type() == CV_8UC1); - UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1); + UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type()==CV_8UC3 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1); // get odom pose Transform pose = Transform::getIdentity(); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index ea798b7d..1f7756c6 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -882,6 +882,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->checkbox_source_feature_detection, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_stereo_exposureCompensation, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_stereo_rightGrayScale, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate())); connect(_ui->pushButton_calibrate_simple, SIGNAL(clicked()), this, SLOT(calibrateSimple())); connect(_ui->toolButton_openniOniPath, SIGNAL(clicked()), this, SLOT(selectSourceOniPath())); @@ -2176,6 +2177,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkbox_source_feature_detection->setChecked(false); _ui->checkbox_stereo_depthGenerated->setChecked(false); _ui->checkBox_stereo_exposureCompensation->setChecked(false); + _ui->checkBox_stereo_rightGrayScale->setChecked(true); _ui->openni2_autoWhiteBalance->setChecked(true); _ui->openni2_autoExposure->setChecked(true); _ui->openni2_exposure->setValue(0); @@ -2254,7 +2256,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->spinBox_stereoMyntEye_contrast->setValue(116); _ui->spinBox_stereoMyntEye_irControl->setValue(0); _ui->comboBox_depthai_image_width->setCurrentIndex(1); - _ui->comboBox_depthai_output_mode->setCurrentIndex(0); + _ui->comboBox_depthai_output_mode->setCurrentIndex(1); _ui->spinBox_depthai_conf_threshold->setValue(200); _ui->checkBox_depthai_extended_disparity->setChecked(false); _ui->checkBox_depthai_disparity_companding->setChecked(false); @@ -2652,6 +2654,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->comboBox_cameraStereo->setCurrentIndex(settings.value("driver", _ui->comboBox_cameraStereo->currentIndex()).toInt()); _ui->checkbox_stereo_depthGenerated->setChecked(settings.value("depthGenerated", _ui->checkbox_stereo_depthGenerated->isChecked()).toBool()); _ui->checkBox_stereo_exposureCompensation->setChecked(settings.value("exposureCompensation", _ui->checkBox_stereo_exposureCompensation->isChecked()).toBool()); + _ui->checkBox_stereo_rightGrayScale->setChecked(settings.value("rightGrayScale", _ui->checkBox_stereo_rightGrayScale->isChecked()).toBool()); settings.endGroup(); // stereo settings.beginGroup("rgb"); @@ -3256,6 +3259,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("driver", _ui->comboBox_cameraStereo->currentIndex()); settings.setValue("depthGenerated", _ui->checkbox_stereo_depthGenerated->isChecked()); settings.setValue("exposureCompensation", _ui->checkBox_stereo_exposureCompensation->isChecked()); + settings.setValue("rightGrayScale", _ui->checkBox_stereo_rightGrayScale->isChecked()); settings.endGroup(); // stereo settings.beginGroup("rgb"); @@ -4296,7 +4300,7 @@ void PreferencesDialog::selectSourceDriver(Src src, int variant) { _ui->checkBox_depthai_imu_published->setChecked(variant >= 1); _ui->comboBox_depthai_image_width->setCurrentIndex(1); - _ui->comboBox_depthai_output_mode->setCurrentIndex(variant==2?2:0); + _ui->comboBox_depthai_output_mode->setCurrentIndex(variant==2?2:1); _ui->doubleSpinBox_depthai_dot_intensity->setValue(variant==2?1:0); } } @@ -6436,6 +6440,10 @@ bool PreferencesDialog::isSourceStereoExposureCompensation() const { return _ui->checkBox_stereo_exposureCompensation->isChecked(); } +bool PreferencesDialog::isRightGrayScale() const +{ + return _ui->checkBox_stereo_rightGrayScale->isChecked(); +} bool PreferencesDialog::isSourceScanFromDepth() const { return _ui->checkBox_source_scanFromDepth->isChecked(); @@ -6758,8 +6766,8 @@ Camera * PreferencesDialog::createCamera( _ui->checkBox_cameraImages_timestamps->isChecked(), _ui->lineEdit_cameraImages_timestamps->text().toStdString(), _ui->checkBox_cameraImages_syncTimeStamps->isChecked()); - ((CameraRGBDImages*)camera)->setConfigForEachFrame(_ui->checkBox_cameraImages_configForEachFrame->isChecked()); - + ((CameraStereoImages*)camera)->setConfigForEachFrame(_ui->checkBox_cameraImages_configForEachFrame->isChecked()); + ((CameraStereoImages*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked()); } else if (driver == kSrcStereoUsb) { @@ -6785,6 +6793,7 @@ Camera * PreferencesDialog::createCamera( { ((CameraStereoVideo*)camera)->setFOURCC(_ui->lineEdit_stereousbcam_fourcc->text().toStdString()); } + ((CameraStereoVideo*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked()); } else if(driver == kSrcStereoVideo) { @@ -6807,6 +6816,7 @@ Camera * PreferencesDialog::createCamera( this->getGeneralInputRate(), this->getSourceLocalTransform()); } + ((CameraStereoVideo*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked()); } else if (driver == kSrcStereoTara) @@ -6856,6 +6866,7 @@ Camera * PreferencesDialog::createCamera( _ui->checkbox_publishInterIMU->isChecked(), _ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0? IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0); + ((CameraStereoZed*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked()); } else if (driver == kSrcStereoZedOC) { @@ -6865,6 +6876,7 @@ Camera * PreferencesDialog::createCamera( _ui->comboBox_stereoZedOC_resolution->currentIndex(), this->getGeneralInputRate(), this->getSourceLocalTransform()); + ((CameraStereoZedOC*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked()); } else if (driver == kSrcStereoDepthAI) { diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 805da7ba..81ba0699 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - -594 + -639 713 - 4653 + 4705 @@ -3424,7 +3424,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 3 + 1 @@ -4972,6 +4972,32 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Auto exposure compensation between left and right images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Driver. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -5034,21 +5060,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - Driver. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - + + @@ -5067,6 +5080,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + + + + + + + + Convert right image to gray scale if not already. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -5080,28 +5113,15 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - - - - Auto exposure compensation between left and right images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - + + From 72a20d849d16ac12582a073824c292b59fbd90f0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 8 Mar 2025 17:19:12 -0800 Subject: [PATCH 18/19] removed debugging code from #1463 --- corelib/src/OdometryThread.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/corelib/src/OdometryThread.cpp b/corelib/src/OdometryThread.cpp index 4ebbaa70..33968c31 100644 --- a/corelib/src/OdometryThread.cpp +++ b/corelib/src/OdometryThread.cpp @@ -123,9 +123,6 @@ void OdometryThread::mainLoop() { UDEBUG("Odom pose = %s", pose.prettyPrint().c_str()); // a null pose notify that odometry could not be computed - data.setImageRaw(cv::Mat()); - if(!data.depthOrRightCompressed().empty()) - data.setDepthOrRightRaw(cv::Mat()); this->post(new OdometryEvent(data, pose, info)); } } From 6eebca83b250fe18da99411cd593d5dc437b77e3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 8 Mar 2025 20:30:26 -0800 Subject: [PATCH 19/19] Improved depth estimation of mono features (#1460) * Improved depth estimation of mono features * typo * Added OdomF2M/InitDepthFactor parameter --- corelib/include/rtabmap/core/Parameters.h | 3 + .../rtabmap/core/odometry/OdometryF2M.h | 3 + corelib/src/odometry/OdometryF2M.cpp | 114 +++++- guilib/src/PreferencesDialog.cpp | 3 + guilib/src/ui/preferencesDialog.ui | 379 +++++++++++------- 5 files changed, 334 insertions(+), 168 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 8b4e5336..2bbc506c 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -477,6 +477,8 @@ class RTABMAP_CORE_EXPORT Parameters // Odometry Frame-to-Map RTABMAP_PARAM(OdomF2M, MaxSize, int, 2000, "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words."); RTABMAP_PARAM(OdomF2M, MaxNewFeatures, int, 0, "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit."); + RTABMAP_PARAM(OdomF2M, InitDepthFactor, float, 0.05, "[Visual] Depth factor used to initialize depth of features without depth. Depth = Factor * fx."); + RTABMAP_PARAM(OdomF2M, FloorThreshold, float, 0.0, "[Visual] Only track features in 3D feature map that are over this threshold (height in base frame). Can be useful to ignore reflections on the floor. 0 means disabled."); RTABMAP_PARAM(OdomF2M, ScanMaxSize, int, 2000, "[Geometry] Maximum local scan map size."); RTABMAP_PARAM(OdomF2M, ScanSubtractRadius, float, 0.05, "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans."); RTABMAP_PARAM(OdomF2M, ScanSubtractAngle, float, 45, uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str()); @@ -490,6 +492,7 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map)."); RTABMAP_PARAM(OdomF2M, BundleAdjustmentMinMotion, float, 0.0, "To create a new keyframe with bundle adjustment, a minimum motion (in pixels) can be required. The motion is computed by the average distance between inliers of the previous keyframe and new frame."); RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxKeyFramesPerFeature, int, 0, "Maximum keyframes per feature for bundle adjustment. 0 means not limit."); + RTABMAP_PARAM(OdomF2M, BundleUpdateFeatureMapOnAllFrames, bool, false, uFormat("Update 3D local feature map on every frame with bundle adjustment. Recommended if %s=false and %s=true so that features without depth are better triangulated on every frame (not only on keyframes). If disabled, the feature map is updated only when a new keyframe is added (legacy approach).", kVisDepthAsMask().c_str(), kMemUseOdomFeatures().c_str())); // Odometry Mono RTABMAP_PARAM(OdomMono, InitMinFlow, float, 100, "Minimum optical flow required for the initialization step."); diff --git a/corelib/include/rtabmap/core/odometry/OdometryF2M.h b/corelib/include/rtabmap/core/odometry/OdometryF2M.h index feeb4cc1..356e2d95 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryF2M.h +++ b/corelib/include/rtabmap/core/odometry/OdometryF2M.h @@ -62,6 +62,8 @@ private: float keyFrameThr_; int visKeyFrameThr_; int maxNewFeatures_; + float initDepthFactor_; + float floorThreshold_; float scanKeyFrameThr_; int scanMaximumMapSize_; float scanSubtractRadius_; @@ -71,6 +73,7 @@ private: int bundleMaxFrames_; float bundleMinMotion_; int bundleMaxKeyFramesPerFeature_; + bool bundleUpdateFeatureMapOnAllFrames_; float validDepthRatio_; int pointToPlaneK_; float pointToPlaneRadius_; diff --git a/corelib/src/odometry/OdometryF2M.cpp b/corelib/src/odometry/OdometryF2M.cpp index 73e4961f..1ab4c296 100644 --- a/corelib/src/odometry/OdometryF2M.cpp +++ b/corelib/src/odometry/OdometryF2M.cpp @@ -60,6 +60,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) : keyFrameThr_(Parameters::defaultOdomKeyFrameThr()), visKeyFrameThr_(Parameters::defaultOdomVisKeyFrameThr()), maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()), + initDepthFactor_(Parameters::defaultOdomF2MInitDepthFactor()), + floorThreshold_(Parameters::defaultOdomF2MFloorThreshold()), scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()), scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()), scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()), @@ -69,6 +71,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) : bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()), bundleMinMotion_(Parameters::defaultOdomF2MBundleAdjustmentMinMotion()), bundleMaxKeyFramesPerFeature_(Parameters::defaultOdomF2MBundleAdjustmentMaxKeyFramesPerFeature()), + bundleUpdateFeatureMapOnAllFrames_(Parameters::defaultOdomF2MBundleUpdateFeatureMapOnAllFrames()), validDepthRatio_(Parameters::defaultOdomF2MValidDepthRatio()), pointToPlaneK_(Parameters::defaultIcpPointToPlaneK()), pointToPlaneRadius_(Parameters::defaultIcpPointToPlaneRadius()), @@ -83,6 +86,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_); Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), visKeyFrameThr_); Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_); + Parameters::parse(parameters, Parameters::kOdomF2MInitDepthFactor(), initDepthFactor_); + Parameters::parse(parameters, Parameters::kOdomF2MFloorThreshold(), floorThreshold_); Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_); Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_); Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_); @@ -95,6 +100,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_); Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMinMotion(), bundleMinMotion_); Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxKeyFramesPerFeature(), bundleMaxKeyFramesPerFeature_); + Parameters::parse(parameters, Parameters::kOdomF2MBundleUpdateFeatureMapOnAllFrames(), bundleUpdateFeatureMapOnAllFrames_); Parameters::parse(parameters, Parameters::kOdomF2MValidDepthRatio(), validDepthRatio_); Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), pointToPlaneK_); @@ -124,6 +130,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) : UASSERT(visKeyFrameThr_>=0); UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f); UASSERT(maxNewFeatures_ >= 0); + UASSERT(initDepthFactor_>0.0f); int corType = Parameters::defaultVisCorType(); Parameters::parse(parameters, Parameters::kVisCorType(), corType); @@ -644,6 +651,51 @@ Transform OdometryF2M::computeTransform( std::vector mapPoints = tmpMap.getWords3(); cv::Mat mapDescriptors = tmpMap.getWordsDescriptors(); + // update last frame features without depth (if bundle adjustment was done) + // Do this before adding bundle frames to keep mono observations without depth + bool lastFrameWords3Updated = false; + std::vector lastFrameWords3; + if( regPipeline_->isImageRequired() && + !visDepthAsMask && + bundleAdjustment_>0 && + !lastFrame_->getWords().empty() && + lastFrame_->getWords().size() == lastFrame_->getWords3().size() && + !points3DMap.empty()) + { + lastFrameWords3 = lastFrame_->getWords3(); + Transform newFramePoseInv = newFramePose.inverse(); + for(std::multimap::const_iterator iter=lastFrame_->getWords().begin(); + iter!=lastFrame_->getWords().end(); + ++iter) + { + cv::Point3f & pt = lastFrameWords3.at(iter->second); + if(!util3d::isFinite(pt)) + { + std::map::iterator mapIter = points3DMap.find(iter->first); + if(mapIter != points3DMap.end()) + { + // in base frame + pt = util3d::transformPoint(mapIter->second, newFramePoseInv); + lastFrameWords3Updated = true; + } + } + } + } + + if( regPipeline_->isImageRequired() && + bundleAdjustment_>0 && + bundleUpdateFeatureMapOnAllFrames_ && + !points3DMap.empty()) + { + // update local map 3D points (if bundle adjustment was done) + for(std::map::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter) + { + UASSERT(mapWords.count(iter->first) == 1); + mapPoints[mapWords.find(iter->first)->second] = iter->second; + } + modified = true; + } + bool addVisualKeyFrame = regPipeline_->isImageRequired() && (keyFrameThr_ == 0.0f || visKeyFrameThr_ == 0 || @@ -674,6 +726,7 @@ Transform OdometryF2M::computeTransform( UTimer tmpTimer; UDEBUG("Update local map"); + modified = bundleAdjustment_>0; // We always add new references even if we don't add/remove points // update local map UASSERT(mapWords.size() == mapPoints.size()); @@ -698,12 +751,14 @@ Transform OdometryF2M::computeTransform( bundleModels_.insert(*bundleModels.find(lastFrame_->id())); iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id()); - // update local map 3D points (if bundle adjustment was done) - for(std::map::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter) + if(!bundleUpdateFeatureMapOnAllFrames_) { - UASSERT(mapWords.count(iter->first) == 1); - //UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints[mapWords.find(iter->first)->second].x, mapPoints[mapWords.find(iter->first)->second].y, mapPoints[mapWords.find(iter->first)->second].z, iter->second.x, iter->second.y, iter->second.z); - mapPoints[mapWords.find(iter->first)->second] = iter->second; + // update local map 3D points (if bundle adjustment was done) + for(std::map::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter) + { + UASSERT(mapWords.count(iter->first) == 1); + mapPoints[mapWords.find(iter->first)->second] = iter->second; + } } } @@ -818,6 +873,28 @@ Transform OdometryF2M::computeTransform( if(maxNewFeatures_ == 0 || added < maxNewFeatures_) { int cameraIndex = iter->second.second.second.second.second; + cv::Point3f pt = iter->second.second.second.first; + if(!util3d::isFinite(pt)) + { + // get the ray instead + float x = iter->second.second.first.pt.x; //subImageWidth should be already removed + float y = iter->second.second.first.pt.y; + Eigen::Vector3f ray = util3d::projectDepthTo3DRay( + lastFrameModels[cameraIndex].imageSize(), + x, + y, + lastFrameModels[cameraIndex].cx(), + lastFrameModels[cameraIndex].cy(), + lastFrameModels[cameraIndex].fx(), + lastFrameModels[cameraIndex].fy()); + float scaleInf = initDepthFactor_ * lastFrameModels[cameraIndex].fx(); + pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), lastFrameModels[cameraIndex].localTransform()); // in base_link frame + } + if(floorThreshold_ != 0.0f && pt.z < floorThreshold_) + { + continue; + } + if(bundleAdjustment_>0) { if(lastFrame_->getWords().count(iter->second.first) == 1) @@ -846,23 +923,6 @@ Transform OdometryF2M::computeTransform( mapWords.insert(mapWords.end(), std::make_pair(iter->second.first, mapWords.size())); mapWordsKpts.push_back(iter->second.second.first); - cv::Point3f pt = iter->second.second.second.first; - if(!util3d::isFinite(pt)) - { - // get the ray instead - float x = iter->second.second.first.pt.x; //subImageWidth should be already removed - float y = iter->second.second.first.pt.y; - Eigen::Vector3f ray = util3d::projectDepthTo3DRay( - lastFrameModels[cameraIndex].imageSize(), - x, - y, - lastFrameModels[cameraIndex].cx(), - lastFrameModels[cameraIndex].cy(), - lastFrameModels[cameraIndex].fx(), - lastFrameModels[cameraIndex].fy()); - float scaleInf = (0.05 * lastFrameModels[cameraIndex].fx()) / 0.01; - pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), lastFrameModels[cameraIndex].localTransform()); // in base_link frame - } mapPoints.push_back(util3d::transformPoint(pt, newFramePose)); mapDescriptors.push_back(iter->second.second.second.second.first); if(lastFrameOldestNewId_ > iter->second.first) @@ -871,6 +931,10 @@ Transform OdometryF2M::computeTransform( } ++added; } + else + { + break; + } } UDEBUG(""); @@ -1193,6 +1257,12 @@ Transform OdometryF2M::computeTransform( map_->setWords(mapWords, mapWordsKpts, mapPoints, mapDescriptors); } + + if(lastFrameWords3Updated) + { + // update output with refined 3d points from bundle adjustment + data.setFeatures(lastFrame_->getWordsKpts(), lastFrameWords3, lastFrame_->getWordsDescriptors()); + } } if(info) diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 1f7756c6..86dd0eff 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1406,15 +1406,18 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : //Odometry Frame to Map _ui->odom_localHistory->setObjectName(Parameters::kOdomF2MMaxSize().c_str()); _ui->spinBox_odom_f2m_maxNewFeatures->setObjectName(Parameters::kOdomF2MMaxNewFeatures().c_str()); + _ui->doubleSpinBox_odom_f2m_floorThreshold->setObjectName(Parameters::kOdomF2MFloorThreshold().c_str()); _ui->spinBox_odom_f2m_scanMaxSize->setObjectName(Parameters::kOdomF2MScanMaxSize().c_str()); _ui->doubleSpinBox_odom_f2m_scanRadius->setObjectName(Parameters::kOdomF2MScanSubtractRadius().c_str()); _ui->doubleSpinBox_odom_f2m_scanAngle->setObjectName(Parameters::kOdomF2MScanSubtractAngle().c_str()); _ui->doubleSpinBox_odom_f2m_scanRange->setObjectName(Parameters::kOdomF2MScanRange().c_str()); _ui->odom_f2m_validDepthRatio->setObjectName(Parameters::kOdomF2MValidDepthRatio().c_str()); + _ui->odom_f2m_initDepthFactor->setObjectName(Parameters::kOdomF2MInitDepthFactor().c_str()); _ui->odom_f2m_bundleStrategy->setObjectName(Parameters::kOdomF2MBundleAdjustment().c_str()); _ui->odom_f2m_bundleMaxFrames->setObjectName(Parameters::kOdomF2MBundleAdjustmentMaxFrames().c_str()); _ui->odom_f2m_bundleMinMotion->setObjectName(Parameters::kOdomF2MBundleAdjustmentMinMotion().c_str()); _ui->odom_f2m_bundleMaxKeyFramesPerFeature->setObjectName(Parameters::kOdomF2MBundleAdjustmentMaxKeyFramesPerFeature().c_str()); + _ui->odom_f2m_bundleUpdateFeatureMapOnAllFrames->setObjectName(Parameters::kOdomF2MBundleUpdateFeatureMapOnAllFrames().c_str()); //Odometry Frame To Frame _ui->comboBox_odomf2f_corType->setObjectName(Parameters::kVisCorType().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 81ba0699..999c1412 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -639 + -989 713 4705 @@ -95,7 +95,7 @@ QFrame::Raised - 5 + 19 @@ -15880,10 +15880,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - [Visual] If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map. + [Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when Radius above is >0). 0 means any angle. true @@ -15893,23 +15893,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - m - - - 0 - - - 9999.000000000000000 - - - 0.000000000000000 - - - - + [Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the laser scans. @@ -15922,6 +15906,32 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + [Visual] Local bundle adjustment. See Optimizer panel. This will not work if Optical Flow correspondences strategy is selected in Visual Registration panel. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + [Visual] Maximum frames used for bundle adjustment (0=inf or all current frames in the local map). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -15938,10 +15948,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - [Geometry] Maximum local scan map size (points). + [Visual] Maximum keyframes per feature for bundle adjustment. 0 means not limit. true @@ -15951,6 +15961,33 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + + + + + [Visual] To create a new keyframe with bundle adjustment, a minimum motion (in pixels) can be required. The motion is computed by the average distance between inliers of the previous keyframe and new frame. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 999999999 + + + @@ -15964,46 +16001,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - [Visual] To create a new keyframe with bundle adjustment, a minimum motion (in pixels) can be required. The motion is computed by the average distance between inliers of the previous keyframe and new frame. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - [Visual] Maximum frames used for bundle adjustment (0=inf or all current frames in the local map). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - [Visual] Gravity sigma used for bundle adjustment (<0, use same value than Optimizer/GravitySigma parameter) - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - + deg @@ -16019,17 +16017,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 999999 - - - - - + + - [Visual] Local bundle adjustment. See Optimizer panel. This will not work if Optical Flow correspondences strategy is selected in Visual Registration panel. + [Geometry] Distance Range used to filter points of local map (when > 0). 0 means local map is updated using time and not range. true @@ -16039,23 +16030,36 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + + + + 4 + - 0 + -1.000000000000000 - 999999 + 10.000000000000000 - 1 + 0.100000000000000 - 0 + -1.000000000000000 + + + 999999 + + + + pixels @@ -16075,13 +16079,90 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - 999999999 + + + m + + + 2 + + + -99.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 - + + + + [Visual] Gravity sigma used for bundle adjustment (<0, use same value than Optimizer/GravitySigma parameter) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + [Visual] Depth factor used to initialize depth of features without depth. Depth = Factor * fx. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + [Visual] If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 3 + + + 0.025000000000000 + + + + + + + [Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + @@ -16100,20 +16181,72 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + [Visual] Floor threshold. Only track features in 3D feature map that are over this threshold (height in base frame). Can be useful to ignore reflections on the floor. 0 means disabled. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 999999 + + + + + + + 0 + + + 999999 + + + 1 + + + 0 + + + + + m - 3 + 0 + + + 9999.000000000000000 - 0.025000000000000 + 0.000000000000000 - + + + + [Visual] Update 3D local feature map on every frames with bundle adjustment. Recommended if Vis/DepthAsMask=false and Mem/UseOdomFeatures=true so that features without depth are better triangulated on every frame (not only on keyframes). If disabled, the feature map is updated only when a new keyframe is added (legacy approach). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + QComboBox::AdjustToContents @@ -16140,10 +16273,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - [Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit. + [Geometry] Maximum local scan map size (points). true @@ -16153,71 +16286,25 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - [Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when Radius above is >0). 0 means any angle. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - + + - 4 + 3 - -1.000000000000000 + 0.001000000000000 - 10.000000000000000 + 99.000000000000000 - 0.100000000000000 + 0.050000000000000 - -1.000000000000000 - - - - - - - [Geometry] Distance Range used to filter points of local map (when > 0). 0 means local map is updated using time and not range. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - [Visual] Maximum keyframes per feature for bundle adjustment. 0 means not limit. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 999999 + 0.050000000000000