From c4ca6a0bac56a6af2996b6c94329096591881685 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 5 Jun 2017 19:04:23 -0400 Subject: [PATCH] Integrated ORB_SLAM2 as an odometry approach Odom/Strategy=5. --- CMakeLists.txt | 47 +- Version.h.in | 1 + cmake_modules/FindORB_SLAM2.cmake | 30 + corelib/include/rtabmap/core/Odometry.h | 3 +- .../include/rtabmap/core/OdometryORBSLAM2.h | 62 ++ corelib/include/rtabmap/core/Parameters.h | 13 +- corelib/src/CMakeLists.txt | 12 + corelib/src/Graph.cpp | 6 +- corelib/src/Odometry.cpp | 4 + corelib/src/OdometryORBSLAM2.cpp | 935 +++++++++++++++++ .../include/rtabmap/gui/PreferencesDialog.h | 1 + guilib/src/AboutDialog.cpp | 30 + guilib/src/MainWindow.cpp | 8 +- guilib/src/OdometryViewer.cpp | 4 +- guilib/src/PreferencesDialog.cpp | 31 +- guilib/src/ui/aboutDialog.ui | 988 ++++++++++++------ guilib/src/ui/preferencesDialog.ui | 209 +++- 17 files changed, 2036 insertions(+), 348 deletions(-) create mode 100644 cmake_modules/FindORB_SLAM2.cmake create mode 100644 corelib/include/rtabmap/core/OdometryORBSLAM2.h create mode 100644 corelib/src/OdometryORBSLAM2.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ba4cb46e..cb1bb551 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -155,6 +155,7 @@ option(WITH_CPUTSDF "Include CPUTSDF support" ON) option(WITH_FOVIS "Include FOVIS support" ON) option(WITH_VISO2 "Include VISO2 support" ON) option(WITH_DVO "Include DVO support" ON) +option(WITH_ORB_SLAM2 "Include ORB_SLAM2 support" ON) option(PCL_OMP "With PCL OMP implementations" ON) FIND_PACKAGE(OpenCV REQUIRED QUIET) @@ -368,7 +369,26 @@ IF(WITH_DVO) ENDIF(dvo_core_FOUND) ENDIF(WITH_DVO) -IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND) +IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND AND NOT RealSense_FOUND) + FIND_PACKAGE(ORB_SLAM2 QUIET) + IF(ORB_SLAM2_FOUND) + MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_INCLUDE_DIRS}") + FIND_PACKAGE(Pangolin QUIET) + IF(NOT Pangolin_FOUND) + SET(ORB_SLAM2_FOUND FALSE) + MESSAGE(STATUS "Found ORB_SLAM2 but not Pangolin, disabling ORB_SLAM2.") + ELSE() + MESSAGE(STATUS "Found Pangolin: ${Pangolin_INCLUDE_DIRS}") + SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS}) + SET(ORB_SLAM2_LIBRARIES ${ORB_SLAM2_LIBRARIES} ${Pangolin_LIBRARIES}) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=native") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") + # There could be crash issues if built with RealSense + ENDIF() + ENDIF(ORB_SLAM2_FOUND) +ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND AND NOT RealSense_FOUND) + +IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND) #Newest versions require std11 IF(NOT MSVC) include(CheckCXXCompilerFlag) @@ -382,7 +402,7 @@ IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND) message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).") ENDIF() ENDIF() -ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND) +ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND) ####### OSX BUNDLE CMAKE_INSTALL_PREFIX ####### IF(APPLE AND BUILD_AS_BUNDLE) @@ -505,6 +525,11 @@ IF(NOT dvo_core_FOUND) ELSE() SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${dvo_core_LIBRARIES}) ENDIF() +IF(NOT ORB_SLAM2_FOUND) + SET(ORB_SLAM2 "//") +ELSE() + SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ORB_SLAM2_LIBRARIES}) +ENDIF() IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF) SET(CONF_VTK_QT true) SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt) @@ -771,11 +796,15 @@ ELSE() MESSAGE(STATUS " With GTSAM = NO (GTSAM not found)") ENDIF() +IF(G2O_FOUND OR GTSAM_FOUND) IF(WITH_VERTIGO) MESSAGE(STATUS " With VERTIGO = YES (License: GPLv3)") ELSE() MESSAGE(STATUS " With VERTIGO = NO (WITH_VERTIGO=OFF)") ENDIF() +ELSE() +MESSAGE(STATUS " With VERTIGO = NO (GTSAM or g2o required)") +ENDIF() IF(cvsba_FOUND) MESSAGE(STATUS " With cvsba = YES (License: GPLv2)") @@ -852,6 +881,20 @@ ELSE() MESSAGE(STATUS " With dvo_core = NO (dvo_core not found)") ENDIF() +IF(ORB_SLAM2_FOUND) +MESSAGE(STATUS " With ORB_SLAM2 = YES (License: GPLv3)") +ELSEIF(NOT WITH_ORB_SLAM2) +MESSAGE(STATUS " With ORB_SLAM2 = NO (WITH_ORB_SLAM2=OFF)") +ELSEIF(G2O_FOUND AND RealSense_FOUND) +MESSAGE(STATUS " With ORB_SLAM2 = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version, WITH_REALSENSE should also be off to avoid eigen errors)") +ELSEIF(G2O_FOUND) +MESSAGE(STATUS " With ORB_SLAM2 = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version)") +ELSEIF(RealSense_FOUND) +MESSAGE(STATUS " With ORB_SLAM2 = NO (WITH_REALSENSE should be off to avoid eigen errors)") +ELSE() +MESSAGE(STATUS " With ORB_SLAM2 = NO (ORB_SLAM2 not found, make sure environment variable ORB_SLAM2_ROOT_DIR is set)") +ENDIF() + IF(QT4_FOUND) MESSAGE(STATUS " With Qt4 = YES (License: Open Source or Commercial)") ELSEIF(Qt5_FOUND) diff --git a/Version.h.in b/Version.h.in index a7f9cab9..8046b86b 100644 --- a/Version.h.in +++ b/Version.h.in @@ -57,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @FOVIS@#define RTABMAP_FOVIS @VISO2@#define RTABMAP_VISO2 @DVO@#define RTABMAP_DVO +@ORB_SLAM2@#define RTABMAP_ORB_SLAM2 #endif /* VERSION_H_ */ diff --git a/cmake_modules/FindORB_SLAM2.cmake b/cmake_modules/FindORB_SLAM2.cmake new file mode 100644 index 00000000..f0238703 --- /dev/null +++ b/cmake_modules/FindORB_SLAM2.cmake @@ -0,0 +1,30 @@ +# - Find ORB_SLAM2 +# +# It sets the following variables: +# ORB_SLAM2_FOUND - Set to false, or undefined, if ORB_SLAM2 isn't found. +# ORB_SLAM2_INCLUDE_DIRS - The ORB_SLAM2 include directory. +# ORB_SLAM2_LIBRARIES - The ORB_SLAM2 library to link against. +# +# Set ORB_SLAM2_ROOT_DIR environment variable as the path to ORB_SLAM2 root folder. + +find_path(ORB_SLAM2_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/include) +find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/lib) + +IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY) + SET(ORB_SLAM2_FOUND TRUE) + SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIR} $ENV{ORB_SLAM2_ROOT_DIR}) + SET(ORB_SLAM2_LIBRARIES ${ORB_SLAM2_LIBRARY}) +ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY) + +IF (ORB_SLAM2_FOUND) + # show which ORB_SLAM2 was found only if not quiet + IF (NOT ORB_SLAM2_FIND_QUIETLY) + MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_LIBRARIES}") + ENDIF (NOT ORB_SLAM2_FIND_QUIETLY) +ELSE (ORB_SLAM2_FOUND) + # fatal error if ORB_SLAM2 is required but not found + IF (ORB_SLAM2_FIND_REQUIRED) + MESSAGE(FATAL_ERROR "Could not find ORB_SLAM2") + ENDIF (ORB_SLAM2_FIND_REQUIRED) +ENDIF (ORB_SLAM2_FOUND) + diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index 363b80f3..d378979f 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -48,7 +48,8 @@ public: kTypeF2F = 1, kTypeFovis = 2, kTypeViso2 = 3, - kTypeDVO = 4 + kTypeDVO = 4, + kTypeORBSLAM2 = 5 }; public: diff --git a/corelib/include/rtabmap/core/OdometryORBSLAM2.h b/corelib/include/rtabmap/core/OdometryORBSLAM2.h new file mode 100644 index 00000000..6b395fb0 --- /dev/null +++ b/corelib/include/rtabmap/core/OdometryORBSLAM2.h @@ -0,0 +1,62 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef ODOMETRYORBSLAM2_H_ +#define ODOMETRYORBSLAM2_H_ + +#include + +namespace ORB_SLAM2 { +class System; +} + +class ORBSLAM2System; + +namespace rtabmap { + +class RTABMAP_EXP OdometryORBSLAM2 : public Odometry +{ +public: + OdometryORBSLAM2(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); + virtual ~OdometryORBSLAM2(); + + virtual void reset(const Transform & initialPose = Transform::getIdentity()); + virtual Odometry::Type getType() {return Odometry::kTypeORBSLAM2;} + +private: + virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0); + +private: + ORBSLAM2System * orbslam2_; + ORB_SLAM2::System * system_; + bool firstFrame_; + +}; + +} + +#endif /* ODOMETRYORBSLAM2_H_ */ diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index b7c5f54f..6627afbf 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -262,11 +262,11 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(BRIEF, Bytes, int, 32, "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes."); - RTABMAP_PARAM(FAST, Threshold, int, 10, "Threshold on difference between intensity of the central pixel and pixels of a circle around this pixel."); + RTABMAP_PARAM(FAST, Threshold, int, 20, "Threshold on difference between intensity of the central pixel and pixels of a circle around this pixel."); RTABMAP_PARAM(FAST, NonmaxSuppression, bool, true, "If true, non-maximum suppression is applied to detected corners (keypoints)."); RTABMAP_PARAM(FAST, Gpu, bool, false, "GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected."); RTABMAP_PARAM(FAST, GpuKeypointsRatio, double, 0.05, "Used with FAST GPU."); - RTABMAP_PARAM(FAST, MinThreshold, int, 1, "Minimum threshold. Used only when FAST/GridRows and FAST/GridCols are set."); + RTABMAP_PARAM(FAST, MinThreshold, int, 7, "Minimum threshold. Used only when FAST/GridRows and FAST/GridCols are set."); RTABMAP_PARAM(FAST, MaxThreshold, int, 200, "Maximum threshold. Used only when FAST/GridRows and FAST/GridCols are set."); RTABMAP_PARAM(FAST, GridRows, int, 4, "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell."); RTABMAP_PARAM(FAST, GridCols, int, 4, "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell."); @@ -363,7 +363,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(GTSAM, Optimizer, int, 1, "0=Levenberg 1=GaussNewton 2=Dogleg"); // Odometry - RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2"); + RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2"); RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset)."); RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw))."); RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features)."); @@ -444,6 +444,11 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(OdomViso2, BucketWidth, double, 50, "Width of bucket."); RTABMAP_PARAM(OdomViso2, BucketHeight, double, 50, "Height of bucket."); + // Odometry ORB_SLAM2 + RTABMAP_PARAM_STR(OdomORBSLAM2, VocPath, "", "Path to ORB vocabulary (*.txt)."); + RTABMAP_PARAM(OdomORBSLAM2, Bf, double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used."); + RTABMAP_PARAM(OdomORBSLAM2, ThDepth, double, 40.0, "Close/Far threshold. Baseline times."); + // Common registration parameters RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false, "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers."); RTABMAP_PARAM(Reg, VarianceNormalized, bool, false, "Normalize covariance values. Position variances are multiplied by norm of the transform and orientation variances are multiplied by angle of the transform."); @@ -479,7 +484,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Vis, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining."); RTABMAP_PARAM(Vis, SubPixEps, float, 0.02, "See cv::cornerSubPix()."); RTABMAP_PARAM(Vis, CorType, int, 0, "Correspondences computation approach: 0=Features Matching, 1=Optical Flow"); - RTABMAP_PARAM(Vis, CorNNType, int, 1, uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorNNType, int, 1, uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.", kVisCorType().c_str())); RTABMAP_PARAM(Vis, CorNNDR, float, 0.6, uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.", kVisCorType().c_str())); RTABMAP_PARAM(Vis, CorGuessWinSize, int, 20, uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str())); RTABMAP_PARAM(Vis, CorFlowWinSize, int, 16, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 6ca3c1e3..90216152 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -63,6 +63,7 @@ SET(SRC_FILES OdometryFovis.cpp OdometryViso2.cpp OdometryDVO.cpp + OdometryORBSLAM2.cpp Stereo.cpp StereoDense.cpp @@ -311,6 +312,17 @@ IF(dvo_core_FOUND) ) ENDIF(dvo_core_FOUND) +IF(ORB_SLAM2_FOUND) + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${ORB_SLAM2_INCLUDE_DIRS} + ) + SET(LIBRARIES + ${LIBRARIES} + ${ORB_SLAM2_LIBRARIES} + ) +ENDIF(ORB_SLAM2_FOUND) + #################################### # Generate resources files #################################### diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 97e8b8e2..542e9fdb 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -442,7 +442,7 @@ struct errors { std::vector trajectoryDistances (const std::vector &poses) { std::vector dist; dist.push_back(0); - for (int32_t i=1; i trajectoryDistances (const std::vector &poses) { } int32_t lastFrameFromSegmentLength(std::vector &dist,int32_t first_frame,float len) { - for (int32_t i=first_frame; idist[first_frame]+len) return i; return -1; @@ -493,7 +493,7 @@ void calcKittiSequenceErrors ( std::vector dist = trajectoryDistances(poses_gt); // for all start positions do - for (int32_t first_frame=0; first_frame +#include + +using namespace std; + +namespace ORB_SLAM2 { +// Override original Tracking object to comment all rendering stuff +class Tracker: public Tracking +{ +public: + Tracker(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap, + KeyFrameDatabase* pKFDB, const std::string &strSettingPath, const int sensor) : + Tracking(pSys, pVoc, pFrameDrawer, pMapDrawer, pMap, pKFDB, strSettingPath, sensor) + { + + } + +protected: + void Track() + { + if(mState==NO_IMAGES_YET) + { + mState = NOT_INITIALIZED; + } + + mLastProcessedState=mState; + + // Get Map Mutex -> Map cannot be changed + unique_lock lock(mpMap->mMutexMapUpdate); + + if(mState==NOT_INITIALIZED) + { + // if(mSensor==System::STEREO || mSensor==System::RGBD) + StereoInitialization(); + //else + // MonocularInitialization(); + + //mpFrameDrawer->Update(this); + + if(mState!=OK) + return; + } + else + { + // System is initialized. Track Frame. + bool bOK; + + // Initial camera pose estimation using motion model or relocalization (if tracking is lost) + if(!mbOnlyTracking) + { + // Local Mapping is activated. This is the normal behaviour, unless + // you explicitly activate the "only tracking" mode. + + if(mState==OK || mState==LOST) + { + // Local Mapping might have changed some MapPoints tracked in last frame + CheckReplacedInLastFrame(); + + if(mVelocity.empty() || mCurrentFrame.mnId vpMPsMM; + std::vector vbOutMM; + cv::Mat TcwMM; + if(!mVelocity.empty()) + { + bOKMM = TrackWithMotionModel(); + vpMPsMM = mCurrentFrame.mvpMapPoints; + vbOutMM = mCurrentFrame.mvbOutlier; + TcwMM = mCurrentFrame.mTcw.clone(); + } + bOKReloc = Relocalization(); + + if(bOKMM && !bOKReloc) + { + mCurrentFrame.SetPose(TcwMM); + mCurrentFrame.mvpMapPoints = vpMPsMM; + mCurrentFrame.mvbOutlier = vbOutMM; + + if(mbVO) + { + for(int i =0; iIncreaseFound(); + } + } + } + } + else if(bOKReloc) + { + mbVO = false; + } + + bOK = bOKReloc || bOKMM; + } + } + } + + mCurrentFrame.mpReferenceKF = mpReferenceKF; + + // If we have an initial estimation of the camera pose and matching. Track the local map. + if(!mbOnlyTracking) + { + if(bOK) + bOK = TrackLocalMap(); + } + else + { + // mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve + // a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes + // the camera we will use the local map again. + if(bOK && !mbVO) + bOK = TrackLocalMap(); + } + + if(bOK) + mState = OK; + else + mState=LOST; + + // Update drawer + //mpFrameDrawer->Update(this); + + // If tracking were good, check if we insert a keyframe + if(bOK) + { + // Update motion model + if(!mLastFrame.mTcw.empty()) + { + cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F); + mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3)); + mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3)); + mVelocity = mCurrentFrame.mTcw*LastTwc; + } + else + mVelocity = cv::Mat(); + + //mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw); + + // Clean VO matches + for(int i=0; iObservations()<1) + { + mCurrentFrame.mvbOutlier[i] = false; + mCurrentFrame.mvpMapPoints[i]=static_cast(NULL); + } + } + + // Delete temporal MapPoints + for(list::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++) + { + MapPoint* pMP = *lit; + delete pMP; + } + mlpTemporalPoints.clear(); + + // Check if we need to insert a new keyframe + if(NeedNewKeyFrame()) + CreateNewKeyFrame(); + + // We allow points with high innovation (considererd outliers by the Huber Function) + // pass to the new keyframe, so that bundle adjustment will finally decide + // if they are outliers or not. We don't want next frame to estimate its position + // with those points so we discard them in the frame. + for(int i=0; i(NULL); + } + } + + // Reset if the camera get lost soon after initialization + if(mState==LOST) + { + //if(mpMap->KeyFramesInMap()<=5) + { + UWARN("Track lost..."); + return; + + } + } + + if(!mCurrentFrame.mpReferenceKF) + mCurrentFrame.mpReferenceKF = mpReferenceKF; + + mLastFrame = Frame(mCurrentFrame); + } + + // Store frame pose information to retrieve the complete camera trajectory afterwards. + if(!mCurrentFrame.mTcw.empty()) + { + cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse(); + mlRelativeFramePoses.push_back(Tcr); + mlpReferences.push_back(mpReferenceKF); + mlFrameTimes.push_back(mCurrentFrame.mTimeStamp); + mlbLost.push_back(mState==LOST); + } + else + { + // This can happen if tracking is lost + mlRelativeFramePoses.push_back(mlRelativeFramePoses.back()); + mlpReferences.push_back(mlpReferences.back()); + mlFrameTimes.push_back(mlFrameTimes.back()); + mlbLost.push_back(mState==LOST); + } + } + + void StereoInitialization() + { + if(mCurrentFrame.N>500) + { + // Set Frame pose to the origin + mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F)); + + // Create KeyFrame + KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); + + // Insert KeyFrame in the map + mpMap->AddKeyFrame(pKFini); + + // Create MapPoints and asscoiate to KeyFrame + for(int i=0; i0) + { + cv::Mat x3D = mCurrentFrame.UnprojectStereo(i); + MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap); + pNewMP->AddObservation(pKFini,i); + pKFini->AddMapPoint(pNewMP,i); + pNewMP->ComputeDistinctiveDescriptors(); + pNewMP->UpdateNormalAndDepth(); + mpMap->AddMapPoint(pNewMP); + + mCurrentFrame.mvpMapPoints[i]=pNewMP; + } + } + + cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl; + + mpLocalMapper->InsertKeyFrame(pKFini); + + mLastFrame = Frame(mCurrentFrame); + mnLastKeyFrameId=mCurrentFrame.mnId; + mpLastKeyFrame = pKFini; + + mvpLocalKeyFrames.push_back(pKFini); + mvpLocalMapPoints=mpMap->GetAllMapPoints(); + mpReferenceKF = pKFini; + mCurrentFrame.mpReferenceKF = pKFini; + + mpMap->SetReferenceMapPoints(mvpLocalMapPoints); + + mpMap->mvpKeyFrameOrigins.push_back(pKFini); + + //mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw); + + mState=OK; + } + } + +public: + cv::Mat GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double ×tamp) + { + mImGray = imRectLeft; + cv::Mat imGrayRight = imRectRight; + + if(mImGray.channels()==3) + { + if(mbRGB) + { + cvtColor(mImGray,mImGray,CV_RGB2GRAY); + } + else + { + cvtColor(mImGray,mImGray,CV_BGR2GRAY); + } + } + else if(mImGray.channels()==4) + { + if(mbRGB) + { + cvtColor(mImGray,mImGray,CV_RGBA2GRAY); + } + else + { + cvtColor(mImGray,mImGray,CV_BGRA2GRAY); + } + } + if(imGrayRight.channels()==3) + { + if(mbRGB) + { + cvtColor(imGrayRight,imGrayRight,CV_RGB2GRAY); + } + else + { + cvtColor(imGrayRight,imGrayRight,CV_BGR2GRAY); + } + } + else if(imGrayRight.channels()==4) + { + if(mbRGB) + { + cvtColor(imGrayRight,imGrayRight,CV_RGBA2GRAY); + } + else + { + cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY); + } + } + + mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); + + Track(); + + return mCurrentFrame.mTcw.clone(); + } + + cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp) + { + mImGray = imRGB; + cv::Mat imDepth = imD; + + if(mImGray.channels()==3) + { + if(mbRGB) + cvtColor(mImGray,mImGray,CV_RGB2GRAY); + else + cvtColor(mImGray,mImGray,CV_BGR2GRAY); + } + else if(mImGray.channels()==4) + { + if(mbRGB) + cvtColor(mImGray,mImGray,CV_RGBA2GRAY); + else + cvtColor(mImGray,mImGray,CV_BGRA2GRAY); + } + + UASSERT(imDepth.type()==CV_32F); + + mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); + + Track(); + + return mCurrentFrame.mTcw.clone(); + } +}; + +// Hack to disable loop closing +class LoopCloser: public LoopClosing +{ +public: + LoopCloser(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale) : + LoopClosing(pMap, pDB, pVoc, bFixScale) + { + } + +public: + void RunNoLoop() + { + mbFinished =false; + + while(1) + { + // just clear the buffer + { + unique_lock lock(mMutexLoopQueue); + mlpLoopKeyFrameQueue.clear(); + } + + ResetIfRequested(); + + if(CheckFinish()) + break; + + usleep(30000); + } + + SetFinish(); + } +}; + +} // namespace ORB_SLAM2 + +class ORBSLAM2System +{ +public: + ORBSLAM2System(const rtabmap::ParametersMap & parameters) : + mpVocabulary(0), + mpKeyFrameDatabase(0), + mpMap(0), + mpTracker(0), + mpLocalMapper(0), + mpLoopCloser(0), + mptLocalMapping(0), + mptLoopClosing(0), + parameters_(parameters) + { + std::string vocabularyPath; + rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kOdomORBSLAM2VocPath(), vocabularyPath); + + if(!vocabularyPath.empty()) + { + //Load ORB Vocabulary + UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str()); + mpVocabulary = new ORB_SLAM2::ORBVocabulary(); + bool bVocLoad = mpVocabulary->loadFromTextFile(vocabularyPath); + if(!bVocLoad) + { + UERROR("Failed to open vocabulary at %s", vocabularyPath.c_str()); + delete mpVocabulary; + mpVocabulary = 0; + } + else + { + UWARN("Vocabulary loaded!"); + } + } + else + { + UERROR("ORBSLAM2 vocabulary path should be set! (Parameter name=\"%s\")", rtabmap::Parameters::kOdomORBSLAM2VocPath().c_str()); + } + } + + bool init(const rtabmap::CameraModel & model, bool stereo, double baseline) + { + if(!mpVocabulary) + { + UERROR("Vocabulary not loaded!"); + return false; + } + + this->shutdown(); + + // Create configuration file + std::string workingDir; + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kRtabmapWorkingDirectory(), workingDir); + if(workingDir.empty()) + { + workingDir = "."; + } + std::string configPath = workingDir+"/rtabmap_orbslam2.yaml"; + std::ofstream ofs (configPath, std::ofstream::out); + ofs << "%YAML:1.0" << std::endl; + ofs << std::endl; + + //# Camera calibration and distortion parameters (OpenCV) + ofs << "Camera.fx: " << model.fx() << std::endl; + ofs << "Camera.fy: " << model.fy() << std::endl; + ofs << "Camera.cx: " << model.cx() << std::endl; + ofs << "Camera.cy: " << model.cy() << std::endl; + ofs << std::endl; + + if(model.D().cols < 4) + { + ofs << "Camera.k1: " << 0 << std::endl; + ofs << "Camera.k2: " << 0 << std::endl; + ofs << "Camera.p1: " << 0 << std::endl; + ofs << "Camera.p2: " << 0 << std::endl; + if(!stereo) + { + ofs << "Camera.k3: " << 0 << std::endl; + } + } + if(model.D().cols >= 4) + { + ofs << "Camera.k1: " << model.D().at(0,0) << std::endl; + ofs << "Camera.k2: " << model.D().at(0,1) << std::endl; + ofs << "Camera.p1: " << model.D().at(0,2) << std::endl; + ofs << "Camera.p2: " << model.D().at(0,3) << std::endl; + } + if(model.D().cols >= 5) + { + ofs << "Camera.k3: " << model.D().at(0,4) << std::endl; + } + if(model.D().cols > 5) + { + UWARN("Unhandled camera distortion size %d, only 5 first coefficients used", model.D().cols); + } + ofs << std::endl; + + ofs << "Camera.width: " << model.imageWidth() << std::endl; + ofs << "Camera.height: " << model.imageHeight() << std::endl; + ofs << std::endl; + + //# IR projector baseline times fx (aprox.) + if(baseline <= 0.0) + { + baseline = rtabmap::Parameters::defaultOdomORBSLAM2Bf(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2Bf(), baseline); + } + ofs << "Camera.bf: " << model.fx()*baseline << std::endl; + ofs << std::endl; + + //# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) + //Camera.RGB: 1 + ofs << "Camera.RGB: 1" << std::endl; + ofs << std::endl; + + //# Close/Far threshold. Baseline times. + double thDepth = rtabmap::Parameters::defaultOdomORBSLAM2ThDepth(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2ThDepth(), thDepth); + ofs << "ThDepth: " << thDepth << std::endl; + ofs << std::endl; + + //# Deptmap values factor + ofs << "DepthMapFactor: " << 1000.0 << std::endl; + ofs << std::endl; + + //#-------------------------------------------------------------------------------------------- + //# ORB Parameters + //#-------------------------------------------------------------------------------------------- + //# ORB Extractor: Number of features per image + int features = rtabmap::Parameters::defaultVisMaxFeatures(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kVisMaxFeatures(), features); + ofs << "ORBextractor.nFeatures: " << features << std::endl; + ofs << std::endl; + + //# ORB Extractor: Scale factor between levels in the scale pyramid + double scaleFactor = rtabmap::Parameters::defaultORBScaleFactor(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kORBScaleFactor(), scaleFactor); + ofs << "ORBextractor.scaleFactor: " << scaleFactor << std::endl; + ofs << std::endl; + + //# ORB Extractor: Number of levels in the scale pyramid + int levels = rtabmap::Parameters::defaultORBNLevels(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kORBNLevels(), levels); + ofs << "ORBextractor.nLevels: " << levels << std::endl; + ofs << std::endl; + + //# ORB Extractor: Fast threshold + //# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. + //# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST + //# You can lower these values if your images have low contrast + int iniThFAST = rtabmap::Parameters::defaultFASTThreshold(); + int minThFAST = rtabmap::Parameters::defaultFASTMinThreshold(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kFASTThreshold(), iniThFAST); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kFASTMinThreshold(), minThFAST); + ofs << "ORBextractor.iniThFAST: " << iniThFAST << std::endl; + ofs << "ORBextractor.minThFAST: " << minThFAST << std::endl; + ofs << std::endl; + + ofs.close(); + + //Create KeyFrame Database + mpKeyFrameDatabase = new ORB_SLAM2::KeyFrameDatabase(*mpVocabulary); + + //Create the Map + mpMap = new ORB_SLAM2::Map(); + + //Initialize the Tracking thread + //(it will live in the main thread of execution, the one that called this constructor) + mpTracker = new ORB_SLAM2::Tracker(0, mpVocabulary, 0, 0, mpMap, mpKeyFrameDatabase, configPath, stereo?ORB_SLAM2::System::STEREO:ORB_SLAM2::System::RGBD); + + //Initialize the Local Mapping thread and launch + mpLocalMapper = new ORB_SLAM2::LocalMapping(mpMap, false); + + //Initialize the Loop Closing thread and launch + mpLoopCloser = new ORB_SLAM2::LoopCloser(mpMap, mpKeyFrameDatabase, mpVocabulary, true); + + mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, mpLocalMapper); + mptLoopClosing = new thread(&ORB_SLAM2::LoopCloser::RunNoLoop, mpLoopCloser); + + //Set pointers between threads + mpTracker->SetLocalMapper(mpLocalMapper); + mpTracker->SetLoopClosing(mpLoopCloser); + mpTracker->SetViewer(0); + + mpLocalMapper->SetTracker(mpTracker); + mpLocalMapper->SetLoopCloser(mpLoopCloser); + + mpLoopCloser->SetTracker(mpTracker); + mpLoopCloser->SetLocalMapper(mpLocalMapper); + + return true; + } + + virtual ~ORBSLAM2System() + { + shutdown(); + if(mpVocabulary) + { + delete mpVocabulary; + } + } + + void shutdown() + { + if(mpMap) + { + mpLocalMapper->RequestFinish(); + mpLoopCloser->RequestFinish(); + + // Wait until all thread have effectively stopped + while(!mpLocalMapper->isFinished() || !mpLoopCloser->isFinished() || mpLoopCloser->isRunningGBA()) + { + usleep(5000); + } + + //cleanup! + mptLoopClosing->join(); + delete mptLoopClosing; + mptLoopClosing = 0; + mptLocalMapping->join(); + delete mptLocalMapping; + mptLocalMapping = 0; + delete mpLoopCloser; + mpLoopCloser=0; + delete mpLocalMapper; + mpLocalMapper=0; + delete mpTracker; + mpTracker=0; + delete mpMap; + mpMap=0; + delete mpKeyFrameDatabase; + mpKeyFrameDatabase=0; + } + } + +public: + // ORB vocabulary used for place recognition and feature matching. + ORB_SLAM2::ORBVocabulary* mpVocabulary; + + // KeyFrame database for place recognition (relocalization and loop detection). + ORB_SLAM2::KeyFrameDatabase* mpKeyFrameDatabase; + + // Map structure that stores the pointers to all KeyFrames and MapPoints. + ORB_SLAM2::Map* mpMap; + + // Tracker. It receives a frame and computes the associated camera pose. + // It also decides when to insert a new keyframe, create some new MapPoints and + // performs relocalization if tracking fails. + ORB_SLAM2::Tracker* mpTracker; + + // Local Mapper. It manages the local map and performs local bundle adjustment. + ORB_SLAM2::LocalMapping* mpLocalMapper; + + // Loop Closer. It searches loops with every new keyframe. If there is a loop it performs + // a pose graph optimization and full bundle adjustment (in a new thread) afterwards. + ORB_SLAM2::LoopCloser* mpLoopCloser; + + // System threads: Local Mapping, Loop Closing, Viewer. + // The Tracking thread "lives" in the main execution thread that creates the System object. + std::thread* mptLocalMapping; + std::thread* mptLoopClosing; + + rtabmap::ParametersMap parameters_; +}; +#endif + +namespace rtabmap { + +OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) : + Odometry(parameters), + orbslam2_(0), + system_(0), + firstFrame_(true) +{ + orbslam2_ = new ORBSLAM2System(parameters); +} + +OdometryORBSLAM2::~OdometryORBSLAM2() +{ +#ifdef RTABMAP_ORB_SLAM2 + if(orbslam2_) + { + delete orbslam2_; + } +#endif +} + +void OdometryORBSLAM2::reset(const Transform & initialPose) +{ + Odometry::reset(initialPose); +#ifdef RTABMAP_ORB_SLAM2 + if(orbslam2_) + { + orbslam2_->shutdown(); + } + firstFrame_ = true; +#endif +} + +// return not null transform if odometry is correctly computed +Transform OdometryORBSLAM2::computeTransform( + SensorData & data, + const Transform & guess, + OdometryInfo * info) +{ + Transform t; + +#ifdef RTABMAP_ORB_SLAM2 + UTimer timer; + + if(data.imageRaw().empty() || + data.imageRaw().rows != data.depthOrRightRaw().rows || + data.imageRaw().cols != data.depthOrRightRaw().cols) + { + UERROR("Not supported input!"); + return t; + } + + if(!((data.cameraModels().size() == 1 && + data.cameraModels()[0].isValidForReprojection()) || + (data.stereoCameraModel().isValidForProjection() && + data.stereoCameraModel().left().isValidForReprojection() && + data.stereoCameraModel().right().isValidForReprojection()))) + { + UERROR("Invalid camera model!"); + return t; + } + + bool stereo = data.cameraModels().size() == 0; + + cv::Mat covariance; + if(orbslam2_->mpTracker == 0) + { + CameraModel model = data.cameraModels().size()==1?data.cameraModels()[0]:data.stereoCameraModel().left(); + if(!orbslam2_->init(model, stereo, data.stereoCameraModel().baseline())) + { + return t; + } + } + + cv::Mat Tcw; + Transform localTransform; + if(stereo) + { + localTransform = data.stereoCameraModel().localTransform(); + Tcw = ((ORB_SLAM2::Tracker*)orbslam2_->mpTracker)->GrabImageStereo(data.imageRaw(), data.rightRaw(), data.stamp()); + } + else + { + localTransform = data.cameraModels()[0].localTransform(); + cv::Mat depth; + if(data.depthRaw().type() == CV_32FC1) + { + depth = data.depthRaw(); + } + else if(data.depthRaw().type() == CV_16UC1) + { + depth = util2d::cvtDepthToFloat(data.depthRaw()); + } + Tcw = ((ORB_SLAM2::Tracker*)orbslam2_->mpTracker)->GrabImageRGBD(data.imageRaw(), depth, data.stamp()); + } + + if(orbslam2_->mpTracker->mState == ORB_SLAM2::Tracking::LOST) + { + covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f; + } + else if(Tcw.cols == 4 && Tcw.rows == 4) + { + t = Transform(cv::Mat(Tcw, cv::Range(0,3), cv::Range(0,4)).clone()); + + if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull()) + { + // from camera frame to base frame + t = localTransform * t.inverse() * localTransform.inverse(); + } + t = this->getPose().inverse() * t; + + if(firstFrame_) + { + // just recovered of being lost, set high covariance + covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f; + firstFrame_ = false; + } + else + { + //based on values set in viso2_ros + covariance = cv::Mat::eye(6,6, CV_64FC1); + covariance.at(0,0) = 0.002; + covariance.at(1,1) = 0.002; + covariance.at(2,2) = 0.05; + covariance.at(3,3) = 0.09; + covariance.at(4,4) = 0.09; + covariance.at(5,5) = 0.09; + } + } + + int totalMapPoints= 0; + int totalKfs= 0; + if(orbslam2_->mpMap) + { + totalMapPoints = orbslam2_->mpMap->MapPointsInMap(); + totalKfs = orbslam2_->mpMap->KeyFramesInMap(); + } + + if(info) + { + info->lost = t.isNull(); + info->type = (int)kTypeORBSLAM2; + info->covariance = covariance; + info->localMapSize = totalMapPoints; + info->localKeyFrames = totalKfs; + + if(this->isInfoDataFilled() && orbslam2_->mpTracker && orbslam2_->mpMap) + { + const std::vector & kpts = orbslam2_->mpTracker->mCurrentFrame.mvKeys; + info->wordMatches.resize(kpts.size()); + info->wordInliers.resize(kpts.size()); + int oi = 0; + for (unsigned int i = 0; i < kpts.size(); ++i) + { + int wordId; + if(orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0) + { + wordId = orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i]->mnId; + } + else + { + wordId = -(i+1); + } + info->words.insert(std::make_pair(wordId, kpts[i])); + if(orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0) + { + info->wordMatches[oi] = wordId; + info->wordInliers[oi] = wordId; + ++oi; + } + } + info->wordMatches.resize(oi); + info->wordInliers.resize(oi); + + std::vector mapPoints = orbslam2_->mpMap->GetAllMapPoints(); + for (unsigned int i = 0; i < mapPoints.size(); ++i) + { + cv::Mat pt = mapPoints[i]->GetWorldPos(); + info->localMap.insert(std::make_pair(mapPoints[i]->mnId, util3d::transformPoint(cv::Point3f(pt), localTransform))); + } + } + } + + UINFO("Odom update time = %fs, map points=%d, keyframes=%d, lost=%s", timer.elapsed(), totalMapPoints, totalKfs, t.isNull()?"true":"false"); + +#else + UERROR("RTAB-Map is not built with ORB_SLAM2 support! Select another visual odometry approach."); +#endif + return t; +} + +} // namespace rtabmap diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 67db35a4..9e186e73 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -305,6 +305,7 @@ private slots: void useGridProjRayTracing(); void changeWorkingDirectory(); void changeDictionaryPath(); + void changeOdometryORBSLAM2Vocabulary(); void readSettingsEnd(); void setupTreeView(); void updateBasicParameter(); diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index 4f1184cb..7cb5d728 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -49,49 +49,79 @@ AboutDialog::AboutDialog(QWidget * parent) : QString cv_version = CV_VERSION; #ifdef RTABMAP_NONFREE cv_version.append(" [With nonfree]"); + _ui->label_opencv_license->setText("Not Commercial"); #else cv_version.append(" [Without nonfree]"); + _ui->label_opencv_license->setText("BSD"); #endif _ui->label_version->setText(version); _ui->label_opencv_version->setText(cv_version); _ui->label_pcl_version->setText(PCL_VERSION_PRETTY); _ui->label_vtk_version->setText(vtkVersion::GetVTKVersion()); + _ui->label_qt_version->setText(qVersion()); #ifdef RTABMAP_OCTOMAP _ui->label_octomap->setText("Yes"); + _ui->label_octomap_license->setEnabled(true); #else _ui->label_octomap->setText("No"); + _ui->label_octomap_license->setEnabled(false); #endif #ifdef RTABMAP_CPUTSDF _ui->label_cputsdf->setText("Yes"); + _ui->label_cputsdf_license->setEnabled(true); #else _ui->label_cputsdf->setText("No"); + _ui->label_cputsdf_license->setEnabled(false); #endif _ui->label_freenect->setText(CameraFreenect::available()?"Yes":"No"); + _ui->label_freenect_license->setEnabled(CameraFreenect::available()); _ui->label_openni2->setText(CameraOpenNI2::available()?"Yes":"No"); + _ui->label_openni2_license->setEnabled(CameraOpenNI2::available()); _ui->label_freenect2->setText(CameraFreenect2::available()?"Yes":"No"); + _ui->label_freenect2_license->setEnabled(CameraFreenect2::available()); _ui->label_realsense->setText(CameraRealSense::available() ? "Yes" : "No"); + _ui->label_realsense_license->setEnabled(CameraRealSense::available()); _ui->label_dc1394->setText(CameraStereoDC1394::available()?"Yes":"No"); + _ui->label_dc1394_license->setEnabled(CameraStereoDC1394::available()); _ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No"); _ui->label_zed->setText(CameraStereoZed::available()?"Yes":"No"); + _ui->label_toro->setText(Optimizer::isAvailable(Optimizer::kTypeTORO)?"Yes":"No"); + _ui->label_toro_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeTORO)?true:false); _ui->label_g2o->setText(Optimizer::isAvailable(Optimizer::kTypeG2O)?"Yes":"No"); + _ui->label_g2o_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeG2O)?true:false); _ui->label_gtsam->setText(Optimizer::isAvailable(Optimizer::kTypeGTSAM)?"Yes":"No"); + _ui->label_gtsam_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeGTSAM)?true:false); _ui->label_cvsba->setText(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?"Yes":"No"); + _ui->label_cvsba_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?true:false); #ifdef RTABMAP_FOVIS _ui->label_fovis->setText("Yes"); + _ui->label_fovis_license->setEnabled(true); #else _ui->label_fovis->setText("No"); + _ui->label_fovis_license->setEnabled(false); #endif #ifdef RTABMAP_VISO2 _ui->label_viso2->setText("Yes"); + _ui->label_viso2_license->setEnabled(true); #else _ui->label_viso2->setText("No"); + _ui->label_viso2_license->setEnabled(false); #endif #ifdef RTABMAP_DVO _ui->label_dvo->setText("Yes"); + _ui->label_dvo_license->setEnabled(true); #else _ui->label_dvo->setText("No"); + _ui->label_dvo_license->setEnabled(false); +#endif +#ifdef RTABMAP_ORB_SLAM2 + _ui->label_orbslam2->setText("Yes"); + _ui->label_orbslam2_license->setEnabled(true); +#else + _ui->label_orbslam2->setText("No"); + _ui->label_orbslam2_license->setEnabled(false); #endif } diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index e2687975..406cbfc8 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -1188,7 +1188,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI { if(_ui->imageView_odometry->isFeaturesShown()) { - if(odom.info().type == (int)Odometry::kTypeF2M) + if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM2) { if(_preferencesDialog->isOdomOnlyInliersShown()) { @@ -1223,9 +1223,9 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI } } - //detect if it is OdometryMono intitialization + //detect if it is OdometryMono initialization bool monoInitialization = false; - if(_preferencesDialog->getOdomStrategy() == 5 && odom.info().type == (int)Odometry::kTypeF2F) + if(_preferencesDialog->getOdomStrategy() == 6 && odom.info().type == (int)Odometry::kTypeF2F) { monoInitialization = true; } @@ -1258,7 +1258,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI _ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw())); } - if(odom.info().type == (int)Odometry::kTypeF2M) + if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM2) { if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown()) { diff --git a/guilib/src/OdometryViewer.cpp b/guilib/src/OdometryViewer.cpp index e25de8a1..eff1662e 100644 --- a/guilib/src/OdometryViewer.cpp +++ b/guilib/src/OdometryViewer.cpp @@ -380,7 +380,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom) if(!odom.data().imageRaw().empty()) { - if(odom.info().type == (int)Odometry::kTypeF2M) + if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM2) { imageView_->setFeatures(odom.info().words, odom.data().depthRaw(), Qt::yellow); } @@ -421,7 +421,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom) imageView_->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw())); } - if(odom.info().type == Odometry::kTypeF2M) + if(odom.info().type == Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM2) { if(imageView_->isFeaturesShown()) { diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 28ad179c..70fe0762 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -168,6 +168,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : #ifndef RTABMAP_DVO _ui->odom_strategy->setItemData(4, 0, Qt::UserRole - 1); #endif +#ifndef RTABMAP_ORB_SLAM2 + _ui->odom_strategy->setItemData(5, 0, Qt::UserRole - 1); +#endif #ifndef RTABMAP_NONFREE _ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1); @@ -972,6 +975,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->doubleSpinBox_OdomViso2BucketWidth->setObjectName(Parameters::kOdomViso2BucketWidth().c_str()); _ui->doubleSpinBox_OdomViso2BucketHeight->setObjectName(Parameters::kOdomViso2BucketHeight().c_str()); + // Odometry ORBSLAM2 + _ui->lineEdit_OdomORBSLAM2VocPath->setObjectName(Parameters::kOdomORBSLAM2VocPath().c_str()); + connect(_ui->toolButton_OdomORBSLAM2VocPath, SIGNAL(clicked()), this, SLOT(changeOdometryORBSLAM2Vocabulary())); + _ui->doubleSpinBox_OdomORBSLAM2Bf->setObjectName(Parameters::kOdomORBSLAM2Bf().c_str()); + _ui->doubleSpinBox_OdomORBSLAM2ThDepth->setObjectName(Parameters::kOdomORBSLAM2ThDepth().c_str()); + //Stereo _ui->stereo_winWidth->setObjectName(Parameters::kStereoWinWidth().c_str()); _ui->stereo_winHeight->setObjectName(Parameters::kStereoWinHeight().c_str()); @@ -3885,11 +3894,14 @@ void PreferencesDialog::setupKpRoiPanel() void PreferencesDialog::updateOdometryVisibility() { + UASSERT(_ui->odom_strategy->count() == 6); _ui->groupBox_odomF2M->setVisible(_ui->odom_strategy->currentIndex()==0); _ui->groupBox_odomF2F->setVisible(_ui->odom_strategy->currentIndex()==1); _ui->groupBox_odomFovis->setVisible(_ui->odom_strategy->currentIndex()==2); _ui->groupBox_odomViso2->setVisible(_ui->odom_strategy->currentIndex()==3); - _ui->groupBox_odomMono->setVisible(_ui->odom_strategy->currentIndex()==4); + _ui->groupBox_odomDVO->setVisible(_ui->odom_strategy->currentIndex()==4); + _ui->groupBox_odomORBSLAM2->setVisible(_ui->odom_strategy->currentIndex()==5); + _ui->groupBox_odomMono->setVisible(_ui->odom_strategy->currentIndex()==6); } void PreferencesDialog::updateKpROI() @@ -3986,6 +3998,23 @@ void PreferencesDialog::changeDictionaryPath() } } +void PreferencesDialog::changeOdometryORBSLAM2Vocabulary() +{ + QString path; + if(_ui->lineEdit_OdomORBSLAM2VocPath->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("ORBSLAM2 Vocabulary"), this->getWorkingDirectory()); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("ORBSLAM2 Vocabulary"), _ui->lineEdit_OdomORBSLAM2VocPath->text()); + } + if(!path.isEmpty()) + { + _ui->lineEdit_OdomORBSLAM2VocPath->setText(path); + } +} + void PreferencesDialog::updateSourceGrpVisibility() { _ui->groupBox_sourceRGBD->setVisible(_ui->comboBox_sourceType->currentIndex() == 0); diff --git a/guilib/src/ui/aboutDialog.ui b/guilib/src/ui/aboutDialog.ui index b289cff7..a597356e 100644 --- a/guilib/src/ui/aboutDialog.ui +++ b/guilib/src/ui/aboutDialog.ui @@ -7,7 +7,7 @@ 0 0 831 - 800 + 832 @@ -52,7 +52,7 @@ QFrame::Raised - + @@ -82,197 +82,14 @@ p, li { white-space: pre-wrap; } - - + + - With FOVIS : + Author : - - - - Links : - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - With Freenect2 : - - - - - - - With OpenNI2 : - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - PCL version : - - - - - - - With stereo Zed : - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - With g2o : - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - With cvsba : - - - - - - - OpenCV version : - - - - - - - With stereo dc1394 : - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - + Mathieu LabbĂ©, matlabbe@gmail.com @@ -282,100 +99,14 @@ p, li { white-space: pre-wrap; } - - - - With RealSense : - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - Version : - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - With Octomap : - - - - - - - With Freenect : - - - - + - Author : + Links : - - - - With stereo FlyCapture2 : - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - VTK version : - - - - - - - With GTSAM : - - - - - - - With CPU-TSDF : - - - - + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> @@ -392,14 +123,7 @@ p, li { white-space: pre-wrap; } - - - - With Viso2 : - - - - + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> @@ -416,7 +140,120 @@ p, li { white-space: pre-wrap; } - + + + + Version : + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + + + + + BSD + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + VTK version : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With Freenect2 : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With CPU-TSDF : + + + true + + + + @@ -424,9 +261,111 @@ p, li { white-space: pre-wrap; } Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + - + + + + With FOVIS : + + + true + + + + + + + PCL version : + + + true + + + + + + + With Octomap : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With ORB SLAM 2 : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With stereo Zed : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With stereo FlyCapture2 : + + + true + + + + @@ -434,9 +373,395 @@ p, li { white-space: pre-wrap; } Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + - + + + + With Viso2 : + + + true + + + + + + + With g2o : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With GTSAM : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With cvsba : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + OpenCV version : + + + true + + + + + + + With Freenect : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With stereo dc1394 : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + With RealSense : + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + BSD + + + true + + + + + + + Apache v2 and/or GPLv2 + + + true + + + + + + + <html><head/><body><p><span style=" font-weight:600;">License</span></p></body></html> + + + true + + + + + + + Apache v2 + + + true + + + + + + + <html><head/><body><p><span style=" font-weight:600;">Third Party Libraries</span></p></body></html> + + + Qt::RichText + + + true + + + + + + + Apache v2 and/or GPLv2 + + + true + + + + + + + BSD + + + true + + + + + + + Apache-2 + + + true + + + + + + + BSD + + + true + + + + + + + BSD + + + true + + + + + + + GPLv2 + + + true + + + + + + + GPLv3 + + + true + + + + + + + GPLv3 + + + true + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + LGPL + + + true + + + + + + + Creative Commons [Attribution-NonCommercial-ShareAlike] + + + true + + + + + + + With TORO : + + + true + + + + + + + With OpenNI2 : + + + true + + + + @@ -444,23 +769,92 @@ p, li { white-space: pre-wrap; } Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - With Viso2 : + + true - - + + + + With DVO : + + + true + + + + + + + Qt version : + + + true + + + + + + + Open Source or Commercial + + + true + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + BSD + + + true + + + + + + + BSD + + + true + + + + + + + GPLv2 + + + true + + + + + + + GPLv3 + + + true + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index f920ac23..dce0e765 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -34 + -637 678 2739 @@ -9714,6 +9714,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag DVO + + + ORB SLAM 2 + + @@ -9985,7 +9990,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 4 + 5 @@ -11512,7 +11517,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + DVO @@ -11551,6 +11556,145 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + ORB SLAM 2 + + + + + + <html><head/><body><p>ORB SLAM 2: <a href="https://github.com/raulmur/ORB_SLAM2"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/raulmur/ORB_SLAM2</span></a></p><p>Tracking and local mapping from ORB SLAM 2 is done but loop closure detection is disabled, using RTAB-Map loop closure detection instead. See ORB panel for ORB parameters (pyramid scale and number of pyramids). See FAST panel for FAST parameters (init and min thresholds). Vis/MaxFeatures (see Visual Registration panel) is used as the number of ORB features extracted.<br/></p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + m + + + 4 + + + 0.000000000000000 + + + 9.999900000000000 + + + 0.010000000000000 + + + 0.076000000000000 + + + + + + + Path to ORB vocabulary (*.txt). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + Close/Far threshold. Baseline times. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 1 + + + 0.100000000000000 + + + 999.000000000000000 + + + 1.000000000000000 + + + 40.000000000000000 + + + + + + + Fake IR projector baseline used only when stereo is not used. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + @@ -14933,10 +15077,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - - QFormLayout::AllNonFixedFieldsGrow - + @@ -14964,6 +15105,32 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare + + + 1 + + + 9999 + + + 50 + + + + + + + Minimum threshold. Used when FAST/GridRows and FAST/GridCols are set or if ORBSLAM2 odometry is used. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + @@ -14973,7 +15140,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + If true, non-maximum suppression is applied to detected corners (keypoints). @@ -14994,32 +15161,6 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare OpenCV 2 - - - - 1 - - - 9999 - - - 50 - - - - - - - Minimum threshold. Used only when FAST/GridRows and FAST/GridCols are set. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - -