diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 29623ac2..75b7631c 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: @@ -49,21 +59,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 @@ -74,7 +84,7 @@ jobs: cache-to: type=inline docker: - needs: docker_deps + #needs: docker_deps runs-on: ubuntu-latest strategy: @@ -172,21 +182,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 diff --git a/CMakeLists.txt b/CMakeLists.txt index ac2c2e24..fe77f6a8 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}) @@ -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) 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 diff --git a/app/ios/RTABMapApp.xcodeproj/project.pbxproj b/app/ios/RTABMapApp.xcodeproj/project.pbxproj index f6b73d9e..570630d1 100644 --- a/app/ios/RTABMapApp.xcodeproj/project.pbxproj +++ b/app/ios/RTABMapApp.xcodeproj/project.pbxproj @@ -157,6 +157,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 = ""; }; 4E1E9CF22D617D08000CB881 /* Measure.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = Measure.h; path = ../android/jni/Measure.h; sourceTree = ""; }; 4E1E9CF32D618079000CB881 /* quad_color.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = quad_color.h; path = ../android/jni/quad_color.h; sourceTree = ""; }; @@ -483,6 +484,7 @@ 4EE01634259BDCC7008CCE65 /* RTABMapApp */ = { isa = PBXGroup; children = ( + 44D4681D2D538A4100B094BA /* RTABMapApp.entitlements */, 4E20B24F266AB95600316EE6 /* Images.xcassets */, 4EFD0BB0259D503200575D88 /* NativeWrapper */, 4EFD0B62259D501E00575D88 /* tango-gl */, @@ -1002,6 +1004,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; @@ -1059,6 +1062,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 + + + 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/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..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()); @@ -488,6 +490,9 @@ 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."); + 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/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/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/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/include/rtabmap/core/odometry/OdometryF2M.h b/corelib/include/rtabmap/core/odometry/OdometryF2M.h index ce5c1173..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_; @@ -69,6 +71,9 @@ private: float scanMapMaxRange_; int bundleAdjustment_; int bundleMaxFrames_; + float bundleMinMotion_; + int bundleMaxKeyFramesPerFeature_; + bool bundleUpdateFeatureMapOnAllFrames_; float validDepthRatio_; int pointToPlaneK_; float pointToPlaneRadius_; 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/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..33968c31 100644 --- a/corelib/src/OdometryThread.cpp +++ b/corelib/src/OdometryThread.cpp @@ -155,7 +155,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/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/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/OdometryF2M.cpp b/corelib/src/odometry/OdometryF2M.cpp index 6239a8d0..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()), @@ -67,6 +69,9 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) : scanMapMaxRange_(Parameters::defaultOdomF2MScanRange()), bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()), bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()), + bundleMinMotion_(Parameters::defaultOdomF2MBundleAdjustmentMinMotion()), + bundleMaxKeyFramesPerFeature_(Parameters::defaultOdomF2MBundleAdjustmentMaxKeyFramesPerFeature()), + bundleUpdateFeatureMapOnAllFrames_(Parameters::defaultOdomF2MBundleUpdateFeatureMapOnAllFrames()), validDepthRatio_(Parameters::defaultOdomF2MValidDepthRatio()), pointToPlaneK_(Parameters::defaultIcpPointToPlaneK()), pointToPlaneRadius_(Parameters::defaultIcpPointToPlaneRadius()), @@ -81,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_); @@ -91,6 +98,9 @@ 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::kOdomF2MBundleUpdateFeatureMapOnAllFrames(), bundleUpdateFeatureMapOnAllFrames_); Parameters::parse(parameters, Parameters::kOdomF2MValidDepthRatio(), validDepthRatio_); Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), pointToPlaneK_); @@ -120,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); @@ -280,6 +291,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 +398,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 +489,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 +552,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(); } } } @@ -594,19 +651,73 @@ 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 || 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 @@ -615,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()); @@ -639,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; + } } } @@ -733,7 +847,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))); } } } @@ -750,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) @@ -778,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) @@ -803,6 +931,10 @@ Transform OdometryF2M::computeTransform( } ++added; } + else + { + break; + } } UDEBUG(""); @@ -1125,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/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/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) { 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 f7617f82..0ec47c53 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/ @@ -86,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 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/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..86dd0eff 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))); @@ -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())); @@ -880,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())); @@ -1403,13 +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()); @@ -2134,7 +2142,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 @@ -2172,6 +2180,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); @@ -2249,12 +2258,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_output_mode->setCurrentIndex(0); + _ui->comboBox_depthai_image_width->setCurrentIndex(1); + _ui->comboBox_depthai_output_mode->setCurrentIndex(1); _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); @@ -2646,6 +2657,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"); @@ -2761,13 +2773,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()); @@ -2865,7 +2879,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 @@ -3248,6 +3262,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"); @@ -3363,20 +3378,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"); @@ -3465,7 +3482,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 @@ -4285,8 +4302,8 @@ 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_output_mode->setCurrentIndex(variant==2?2:0); + _ui->comboBox_depthai_image_width->setCurrentIndex(1); + _ui->comboBox_depthai_output_mode->setCurrentIndex(variant==2?2:1); _ui->doubleSpinBox_depthai_dot_intensity->setValue(variant==2?1:0); } } @@ -4387,7 +4404,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(""); } } @@ -6426,6 +6443,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(); @@ -6748,8 +6769,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) { @@ -6775,6 +6796,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) { @@ -6797,6 +6819,7 @@ Camera * PreferencesDialog::createCamera( this->getGeneralInputRate(), this->getSourceLocalTransform()); } + ((CameraStereoVideo*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked()); } else if (driver == kSrcStereoTara) @@ -6846,6 +6869,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) { @@ -6855,25 +6879,25 @@ Camera * PreferencesDialog::createCamera( _ui->comboBox_stereoZedOC_resolution->currentIndex(), this->getGeneralInputRate(), this->getSourceLocalTransform()); + ((CameraStereoZedOC*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked()); } else if (driver == kSrcStereoDepthAI) { 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()); @@ -6951,13 +6975,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 91a45e44..999c1412 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - 0 + -989 713 - 4653 + 4705 @@ -95,7 +95,7 @@ QFrame::Raised - 9 + 19 @@ -3424,7 +3424,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 2 + 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 - - - - - + + @@ -5975,33 +5995,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 +6005,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 +6053,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 +6079,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - -1 - - - 255 - - - 5 - - - - - - - - - - @@ -6223,55 +6092,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 +6121,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 +6170,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 0 + 1 QComboBox::AdjustToContents @@ -6324,20 +6197,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 +6310,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 +6341,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 +6420,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 +6486,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - IMU published + Path to MyriadX blob file. true @@ -6777,7 +6840,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki false - + @@ -6930,7 +6993,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 +7044,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - -1 - - - 9999 - - - @@ -7041,6 +7094,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + @@ -13437,90 +13493,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 +13522,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 +13535,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 +13568,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 +13607,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 +13867,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 +13883,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 +14255,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 - - - @@ -15799,7 +15855,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 1 + 0 @@ -15824,36 +15880,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 0 + + + + [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. - - 999999 + + true - - 1 - - - 0 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - m - - - 3 - - - 0.025000000000000 - - - - + [Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the laser scans. @@ -15866,10 +15906,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - [Geometry] Maximum local scan map size (points). + [Visual] Local bundle adjustment. See Optimizer panel. This will not work if Optical Flow correspondences strategy is selected in Visual Registration panel. true @@ -15879,10 +15919,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - [Visual] Maximum map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum features. This will decrease odometry drifting when the camera is not moving. + [Visual] Maximum frames used for bundle adjustment (0=inf or all current frames in the local map). true @@ -15908,13 +15948,207 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + + [Visual] Maximum keyframes per feature for bundle adjustment. 0 means not limit. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + [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 + + + + [Visual] Maximum map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum features. This will decrease odometry drifting when the camera is not moving. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + deg + + + 0 + + + 180.000000000000000 + + + 0.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 + + + + + + + + + + 4 + + + -1.000000000000000 + + + 10.000000000000000 + + + 0.100000000000000 + + + -1.000000000000000 + + + + + + + 999999 + + + + + + + pixels + + + 1 + + + 99.000000000000000 + + + 0.500000000000000 + + + 0.000000000000000 + + + + + + + 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 + + + @@ -15928,10 +16162,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + + + + 2 + + + 1.000000000000000 + + + 0.050000000000000 + + + 0.750000000000000 + + + + + - [Visual] Local bundle adjustment. See Optimizer panel. This will not work if Optical Flow correspondences strategy is selected in Visual Registration panel. + [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 @@ -15941,17 +16194,49 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + 999999 - - + + + + 0 + + + 999999 + + + 1 + + + 0 + + + + + + + m + + + 0 + + + 9999.000000000000000 + + + 0.000000000000000 + + + + + - [Visual] Maximum frames used for bundle adjustment (0=inf or all current frames in the local map). + [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 @@ -15961,7 +16246,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + QComboBox::AdjustToContents @@ -15988,10 +16273,10 @@ 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. + [Geometry] Maximum local scan map size (points). true @@ -16001,115 +16286,25 @@ 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 + 3 + + + 0.001000000000000 - 1.000000000000000 + 99.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 - - - - - - - - - - 4 - - - -1.000000000000000 - - - 10.000000000000000 - - - 0.100000000000000 - - - -1.000000000000000 + 0.050000000000000 diff --git a/package.xml b/package.xml index a9835453..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 @@ -22,7 +22,7 @@ libsqlite3-dev liboctomap-dev - grid_map_core + qt_gui_cpp zlib 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,