v2.0

YWL0720 · YWL0720 · commit 3dd1c60ab77d · 2022-06-28T15:21:34.000+08:00
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -40,7 +40,7 @@ add_definitions(-w)
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
 set(CMAKE_PREFIX_PATH ${PROJECT_SOURCE_DIR}/Thirdparty/libtorch)
 
-find_package(OpenCV 4.4)
+find_package(OpenCV 4.2)
 if(NOT OpenCV_FOUND)
     message(FATAL_ERROR "OpenCV > 4.4 not found.")
 endif()
diff --git a/Examples/RGB-D/TUM3.yaml b/Examples/RGB-D/TUM3.yaml
@@ -39,7 +39,7 @@ RGBD.DepthMapFactor: 5000.0
 #--------------------------------------------------------------------------------------------
 
 # ORB Extractor: Number of features per image
-ORBextractor.nFeatures: 1000
+ORBextractor.nFeatures: 2000
 
 # ORB Extractor: Scale factor between levels in the scale pyramid 	
 ORBextractor.scaleFactor: 1.2
diff --git a/Examples/RGB-D/rgbd_tum.cc b/Examples/RGB-D/rgbd_tum.cc
@@ -121,7 +121,7 @@ int main(int argc, char **argv)
             T = tframe-vTimestamps[ni-1];
 
         if(ttrack<T)
-            usleep((T-ttrack)*1e6);
+            usleep((T-ttrack)*10);
     }
 
     // Stop all threads
diff --git a/Examples/ROS/YOLO_ORB_SLAM3/launch/camera_topic_remap.launch b/Examples/ROS/YOLO_ORB_SLAM3/launch/camera_topic_remap.launch
@@ -1,5 +1,5 @@
 <launch>
-	<node pkg="topic_tools" name="topic_1" type="throttle" args="messages camera/rgb/image_raw 15 camera/rgb/image_raw_low"/>
-	<node pkg="topic_tools" name="topic_2" type="throttle" args="messages camera/depth_registered/image_raw 15 camera/depth_registered/image_raw_low"/>
+	<node pkg="topic_tools" name="topic_1" type="throttle" args="messages camera/color/image_raw 15 camera/color/image_raw_low"/>
+	<node pkg="topic_tools" name="topic_2" type="throttle" args="messages camera/aligned_depth_to_color/image_raw 15 camera/aligned_depth_to_color/image_raw_low"/>
 </launch>
 
diff --git a/Examples/ROS/YOLO_ORB_SLAM3/src/ros_rgbd.cc b/Examples/ROS/YOLO_ORB_SLAM3/src/ros_rgbd.cc
@@ -63,8 +63,8 @@ int main(int argc, char **argv)
 
     ros::NodeHandle nh;
 
-    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw_low", 100);
-    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/depth_registered/image_raw_low", 100);
+    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/color/image_raw_low", 100);
+    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/aligned_depth_to_color/image_raw_low", 100);
     typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
     message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,depth_sub);
     sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));
diff --git a/README.md b/README.md
@@ -1,4 +1,10 @@
 # YOLO_ORB_SLAM3
+Combine YOLOv5 with ORB-SLAM3
+- Object Detect
+- Dynamic SLAM
+
+![Fig1](/home/yuwenlu/MyCode/Fig1.png)
+![Fig2](/home/yuwenlu/MyCode/Fig2.png)
 ## Geting Started
 ### 1. Install libtorch
 ```bash
diff --git a/groundtruth.txt b/groundtruth.txt
diff --git a/include/ORBextractor.h b/include/ORBextractor.h
@@ -81,7 +81,7 @@ class ORBextractor
     }
 
     std::vector<cv::Mat> mvImagePyramid;
-    std::vector<cv::Rect2i> mvPersonArea;
+    std::vector<cv::Rect2i> mvDynamicArea;
 
 protected:
 
diff --git a/include/Viewer.h b/include/Viewer.h
@@ -66,6 +66,9 @@ class Viewer
     bool both;
 
     std::vector<cv::Rect2i> mvPersonArea;
+    map<string, vector<cv::Rect2i>> mmDetectMap;
+
+
     std::mutex mMutexPAFinsh;
 
 private:
diff --git a/include/YoloDetect.h b/include/YoloDetect.h
@@ -30,6 +30,12 @@ class YoloDetection
     torch::jit::script::Module mModule;
     std::vector<std::string> mClassnames;
 
+    // 6-28
+    vector<string> mvDynamicNames;
+    vector<cv::Rect2i> mvDynamicArea;
+    map<string, vector<cv::Rect2i>> mmDetectMap;
+
+
 };
 
 
diff --git a/src/ORBextractor.cc b/src/ORBextractor.cc
@@ -880,7 +880,7 @@ namespace ORB_SLAM3
                     bool Find = false;
                     for (auto vit_kp = vToDistributeKeys.begin(); vit_kp != vToDistributeKeys.end();)
                     {
-                        for (auto vit_area = mvPersonArea.begin(); vit_area != mvPersonArea.end(); vit_area++)
+                        for (auto vit_area = mvDynamicArea.begin(); vit_area != mvDynamicArea.end(); vit_area++)
                         {
                             Find = false;
                             if (vit_area->contains(vit_kp->pt))
diff --git a/src/Tracking.cc b/src/Tracking.cc
@@ -1532,13 +1532,13 @@ Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, co
     InputImage = imRGB.clone();
     mpDetector->GetImage(InputImage);
     mpDetector->Detect();
-    mpORBextractorLeft->mvPersonArea = mpDetector->mvPersonArea;
+    mpORBextractorLeft->mvDynamicArea = mpDetector->mvDynamicArea;
     {
         std::unique_lock<std::mutex> lock(mpViewer->mMutexPAFinsh);
-        mpViewer->mvPersonArea = mpDetector->mvPersonArea;
+        mpViewer->mmDetectMap = mpDetector->mmDetectMap;
     }
-    mpDetector->mvPersonArea.clear();
-
+    mpDetector->mvDynamicArea.clear();
+    mpDetector->mmDetectMap.clear();
 
     if(mImGray.channels()==3)
     {
diff --git a/src/Viewer.cc b/src/Viewer.cc
@@ -164,7 +164,7 @@ void Viewer::Run()
     mbFinished = false;
     mbStopped = false;
 
-    pangolin::CreateWindowAndBind("ORB-SLAM3: Map Viewer",1024,768);
+    pangolin::CreateWindowAndBind("YOLO-ORB-SLAM3: Map Viewer",1024,768);
 
     // 3D Mouse handler requires depth testing to be enabled
     glEnable(GL_DEPTH_TEST);
@@ -204,7 +204,7 @@ void Viewer::Run()
     Twc.SetIdentity();
     pangolin::OpenGlMatrix Ow; // Oriented with g in the z axis
     Ow.SetIdentity();
-    cv::namedWindow("ORB-SLAM3: Current Frame");
+    cv::namedWindow("YOLO-ORB-SLAM3: Current Frame");
 
     bool bFollow = true;
     bool bLocalizationMode = false;
@@ -338,12 +338,24 @@ void Viewer::Run()
         // Yolo
         {
             std::unique_lock<std::mutex> lock(mMutexPAFinsh);
-            for (auto vit = mvPersonArea.begin(); vit != mvPersonArea.end(); vit++) {
-                cv::rectangle(toShow, *vit, cv::Scalar(0, 0, 255), 1);
+            for (auto vit = mmDetectMap.begin(); vit != mmDetectMap.end(); vit++)
+            {
+                if (vit->second.size() != 0)
+                {
+                    for (auto area : vit->second)
+                    {
+                        cv::rectangle(toShow, area, cv::Scalar(0, 0, 255), 1);
+                        cv::putText(toShow,
+                                    vit->first,
+                                    cv::Point(area.x, area.y),
+                                    cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 0), 2);
+                    }
+                }
+
             }
         }
 
-        cv::imshow("ORB-SLAM3: Current Frame",toShow);
+        cv::imshow("YOLO-ORB-SLAM3: Current Frame",toShow);
         cv::waitKey(mT);
 
         if(menuReset)
diff --git a/src/YoloDetect.cpp b/src/YoloDetect.cpp
@@ -13,6 +13,8 @@ YoloDetection::YoloDetection()
     {
         mClassnames.push_back(name);
     }
+    mvDynamicNames = {"person", "car", "motorbike", "bus", "train", "truck", "boat", "bird", "cat",
+                      "dog", "horse", "sheep", "crow", "bear"};
 }
 
 YoloDetection::~YoloDetection()
@@ -54,17 +56,21 @@ bool YoloDetection::Detect()
             float bottom = dets[0][i][3].item().toFloat() * mRGB.rows / 384;
             int classID = dets[0][i][5].item().toInt();
 
-            if (mClassnames[classID] == "person")
+
+            cv::Rect2i DetectArea(left, top, (right - left), (bottom - top));
+            mmDetectMap[mClassnames[classID]].push_back(DetectArea);
+
+            if (count(mvDynamicNames.begin(), mvDynamicNames.end(), mClassnames[classID]))
             {
-                cv::Rect2i PersonArea(left, top, (right - left), (bottom - top));
-                mvPersonArea.push_back(PersonArea);
+                cv::Rect2i DynamicArea(left, top, (right - left), (bottom - top));
+                mvDynamicArea.push_back(DynamicArea);
             }
 
         }
-        if (mvPersonArea.size() == 0)
+        if (mvDynamicArea.size() == 0)
         {
-            cv::Rect2i tPersonArea(1, 1, 1, 1);
-            mvPersonArea.push_back(tPersonArea);
+            cv::Rect2i tDynamicArea(1, 1, 1, 1);
+            mvDynamicArea.push_back(tDynamicArea);
         }
     }
     return true;

Original file line number	Diff line number	Diff line change
`@@ -121,7 +121,7 @@ int main(int argc, char **argv)`
`121`	`121`	`T = tframe-vTimestamps[ni-1];`
`122`	`122`
`123`	`123`	`if(ttrack<T)`
`124`		`- usleep((T-ttrack)*1e6);`
	`124`	`+ usleep((T-ttrack)*10);`
`125`	`125`	`}`
`126`	`126`
`127`	`127`	`// Stop all threads`
Original file line number	Diff line number	Diff line change
`@@ -81,7 +81,7 @@ class ORBextractor`
`81`	`81`	`}`
`82`	`82`
`83`	`83`	`std::vector<cv::Mat> mvImagePyramid;`
`84`		`- std::vector<cv::Rect2i> mvPersonArea;`
	`84`	`+ std::vector<cv::Rect2i> mvDynamicArea;`
`85`	`85`
`86`	`86`	`protected:`
`87`	`87`