(Upgrade) ClosedLoop Benchmark

ivalab · Feb 8, 2023 · b3ae47a · b3ae47a
1 parent 7768ea3
commit b3ae47a
Show file tree

Hide file tree

Showing 8 changed files with 78 additions and 21 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 2.8)
 project(gf_orb_slam2)
 
+option(ENABLE_CLOSED_LOOP "Build Closed-Loop" OFF)
+
 find_package(catkin REQUIRED COMPONENTS
     rospy
     roscpp
@@ -16,6 +18,24 @@ find_package(catkin REQUIRED COMPONENTS
     # trajectory_state_predictor
 )
 
+if(ENABLE_CLOSED_LOOP)
+    add_definitions(-DENABLE_CLOSED_LOOP)
+    find_package(catkin REQUIRED COMPONENTS
+        rospy
+        roscpp
+        sensor_msgs
+        cv_bridge
+        image_transport
+        tf
+        tf2
+        tf2_geometry_msgs
+        tf2_ros
+        geometry_msgs
+        # for closed-loop evaluation of Good Graph only
+        trajectory_state_predictor
+    )
+endif()
+
 message("ROS version = " $ENV{ROS_DISTRO})
 
 IF(NOT CMAKE_BUILD_TYPE)

diff --git a/Examples/ROS/GF_ORB_SLAM2/src/ros_stereo.cc b/Examples/ROS/GF_ORB_SLAM2/src/ros_stereo.cc
@@ -369,6 +369,15 @@ void ImageGrabber::GrabPath(const nav_msgs::Path::ConstPtr& msg) {
 
 void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight)
 {
+#ifdef ENABLE_CLOSED_LOOP
+    // @NOTE (yanwei) throw the first few garbage images from gazebo
+    static size_t skip_imgs = 0;
+    if (skip_imgs < 10)
+    {
+        ++skip_imgs;
+        return;
+    }
+#endif
 
 double latency_trans = ros::Time::now().toSec() - msgLeft->header.stamp.toSec();
 // ROS_INFO("ORB-SLAM Initial Latency: %.03f sec", ros::Time::now().toSec() - msgLeft->header.stamp.toSec());

diff --git a/README.md b/README.md
@@ -1,6 +1,6 @@
 # Ubuntu 20.04 + ROS noetic
 
-## Build &  Run
+## Build & Run
 
 *@NOTE Feel free to follow the building stpes of Ubuntu16.04 below if you have a lower version.*
 
@@ -34,23 +34,29 @@ Build 3rdparties.
 Now build the GF-ORB-SLAM2 package
 
     cd ~/catkin_ws
-    
+
     catkin build -j8 -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS="-O3 -DNDEBUG" -DCMAKE_C_FLAGS_RELEASE="-O3 -DNDEBUG" -DCMAKE_CXX_STANDARD=14 -DENABLE_CUDA_IN_OPENCV=False
 
 Convert vocabulary file to binary for fast loading
 
     ./tools/bin_vocabulary
 
-EuRoC benchmark.
+### EuRoC benchmark.
 
 [Link to Dataset (rosbags)](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets)
 
 1. Running.
     ```bash
-    cd batch_scripts/noetic
+    # start roscore in 1st terminal
+    roscore
 
-    # remember to configure your path
+    # !!! in 2nd terminal !!!
+    # export ROS environment
+    cd ~/catkin_ws
+    source devel/setup.bash
 
+    # remember to configure your path
+    cd batch_scripts/noetic
     python Run_EuRoC_Stereo_ROS.py
     ```
 2. Evaluation.
@@ -59,7 +65,7 @@ EuRoC benchmark.
     ```bash
     # remember to configure your path
 
-    python Evaluate_EuRoC_Stereo.py
+    python Evaluate_EuRoC_Stereo.py # will call evo library
     ```
 3. Collect stats.
     ```bash
@@ -74,6 +80,15 @@ EuRoC benchmark.
     # 2) xxx_vis.txt, with (num_SpeedMode) rows, each row is: (Seq1_RMSE, Seq2_RMSE, xxx, SeqM_RMSE, mean_RMSE, seq_completeness, mean_latency, median_latency)
     ```
 
+### ClosedLoop Benchmark.
+
+To run GF-ORB-SLAM2 on closed-loop navigation tasks, e.g., as the state estimator of [gazebo_turtlebot_simulator](https://github.com/ivalab/gazebo_turtlebot_simulator/tree/feature/ubuntu20.04), make sure to enable the compiling option `-DENABLE_CLOSED_LOOP=ON`,
+
+    cd ~/catkin_ws
+
+    catkin build -j8 -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS="-O3 -DNDEBUG" -DCMAKE_C_FLAGS_RELEASE="-O3 -DNDEBUG" -DCMAKE_CXX_STANDARD=14 -DENABLE_CUDA_IN_OPENCV=False -DENABLE_CLOSED_LOOP=ON
+
+and refer to closed-loop evaluation scripts at [gazebo_turtlebot_simulator/script_ros_noetic](https://github.com/ivalab/gazebo_turtlebot_simulator/tree/feature/ubuntu20.04/script_ros_noetic) on how to config the evaluation.
 
 ---
 ![](https://github.com/ivalab/demo_gif/blob/master/office_slam_demo.gif)

diff --git a/...ripts/noetic/Collect_evaluation_result.py → batch_scripts/noetic/Collect_EuRoC_Stereo.py b/...ripts/noetic/Collect_evaluation_result.py → batch_scripts/noetic/Collect_EuRoC_Stereo.py
diff --git a/include/Frame.h b/include/Frame.h
@@ -67,9 +67,9 @@
     #define VIRTUAL_FRAME_NUM       1 // 2 //
     // number of regular frames between virtual KF
     // for EuRoC (20 fps)
-    #define VIRTUAL_FRAME_STEP      10 // 5 //
+    // #define VIRTUAL_FRAME_STEP      10 // 5 //
     // for Gazebo (30 fps)
-    // #define VIRTUAL_FRAME_STEP      15
+    #define VIRTUAL_FRAME_STEP      15
 
     // #define ENABLE_PERTURBATION_TO_ODOM
     // level of gaussian noise added to ground truth anticipation

diff --git a/include/MacroDefinitions.h b/include/MacroDefinitions.h
@@ -48,18 +48,6 @@
 
 // #define PRED_WITH_ODOM
 
-/* --- options of anticipating poses with closed-loop planner  --- */
-//
-// NOTE
-// For closed-loop navigation application ONLY
-// By ENABLE_PLANNER_PREDICTION, please make sure the the trajectory state
-// predictor package is included in your catkin workspace:
-// https://github.gatech.edu/ivabots/trajectory_state_predictor
-// Otherwise, you might write your own predictor by grabbing output from the
-// controller
-
-// #define ENABLE_PLANNER_PREDICTION
-
 ////////////////////////////////////////////////////////////////////////////////
 // !!! YOU NEED NOT WORRY ABOUT THE PARAMETERS DEFINED BELOW !!!              //
 ////////////////////////////////////////////////////////////////////////////////
@@ -113,4 +101,26 @@
 
 #endif
 
+#ifdef ENABLE_CLOSED_LOOP
+
+    //-------------- Tracking.h -----------------//
+    #define SPARSE_KEYFRAME_COND
+
+    #ifdef GF_GG_MODE
+        //-------------- Tracking.h -----------------//
+        /* --- options of anticipating poses with closed-loop planner  --- */
+        //
+        // NOTE
+        // For closed-loop navigation application ONLY
+        // By ENABLE_PLANNER_PREDICTION, please make sure the the trajectory state
+        // predictor package is included in your catkin workspace:
+        // https://github.gatech.edu/ivabots/trajectory_state_predictor
+        // Otherwise, you might write your own predictor by grabbing output from the
+        // controller
+
+        // @TODO (yanwei) it is a bit unstable when turning this macro on
+        // #define ENABLE_PLANNER_PREDICTION
+    #endif
+#endif
+
 #endif  // GF_ORB_SLAM2_MACRO_DEFINITIONS_H_
diff --git a/src/System.cc b/src/System.cc
@@ -153,6 +153,9 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS
 #else
     std::cout << "System: Unknown mode enabled! Be caution." << std::endl;
 #endif
+#ifdef ENABLE_CLOSED_LOOP
+    std::cout << "System: Closed-Loop mode enabled!" << std::endl;
+#endif
 }
 
 cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp)

diff --git a/src/Tracking.cc b/src/Tracking.cc
@@ -302,7 +302,7 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer,
     // disturbed gt pose or planner fails to predict new camera pose,
     // see @line1873 for code detail.
 #else
-    std::cout << "Tracking: constant velocity motion model is used!" << std::endl;
+    std::cout << "Tracking: constant velocity motion model is used in tracking!" << std::endl;
 #endif
 
 #ifdef ENABLE_PLANNER_PREDICTION