several fixes

abhineet123 · Apr 9, 2017 · 3c1f433 · 3c1f433
1 parent b97530f
commit 3c1f433
Show file tree

Hide file tree

Showing 30 changed files with 8 additions and 3 deletions.
diff --git a/Examples/ROS/ORB_SLAM2/Monopub b/Examples/ROS/ORB_SLAM2/Monopub
diff --git a/Examples/ROS/ORB_SLAM2/Monosub b/Examples/ROS/ORB_SLAM2/Monosub
diff --git a/Examples/ROS/ORB_SLAM2/src/ros_mono_pub.cc b/Examples/ROS/ORB_SLAM2/src/ros_mono_pub.cc
@@ -159,8 +159,8 @@ void publish(ORB_SLAM2::System &SLAM, ros::Publisher &pub_pts_and_pose,
 		pub_all_pts = true;
 		pub_count = 0;
 	}
-	if (pub_all_pts || SLAM.getLoopClosing()->loop_detected) {
-		pub_all_pts = SLAM.getLoopClosing()->loop_detected = false;
+	if (pub_all_pts || SLAM.getLoopClosing()->loop_detected || SLAM.getTracker()->loop_detected) {
+		pub_all_pts = SLAM.getTracker()->loop_detected = SLAM.getLoopClosing()->loop_detected = false;
 		geometry_msgs::PoseArray kf_pt_array;
 		vector<ORB_SLAM2::KeyFrame*> key_frames = SLAM.getMap()->GetAllKeyFrames();
 		//! placeholder for number of keyframes

diff --git a/Examples/ROS/ORB_SLAM2/src/ros_mono_sub.cc b/Examples/ROS/ORB_SLAM2/src/ros_mono_sub.cc
@@ -131,6 +131,7 @@ void kfCallback(const geometry_msgs::PoseStamped::ConstPtr& camera_pose){
 		camera_pose->header.seq);
 }
 void saveMap(unsigned int id) {
+	printf("saving maps with id: %u\n", id);
 	if (id > 0) {
 		cv::imwrite("grid_map_" + to_string(id) + ".jpg", grid_map);
 		cv::imwrite("grid_map_thresh_" + to_string(id) + ".jpg", grid_map_thresh);
@@ -395,6 +396,7 @@ void getGridMap() {
 }
 void showGridMap(unsigned int id) {
 	cv::imshow("grid_map_thresh_resized", grid_map_thresh_resized);
+	cv::imshow("grid_map", grid_map);
 	int key = cv::waitKey(1) % 256;
 	if (key == 27) {
 		cv::destroyAllWindows();

diff --git a/ORB_SLAM2.v12.suo b/ORB_SLAM2.v12.suo
diff --git a/grid_map.jpg b/grid_map.jpg
diff --git a/grid_map_1208.jpg b/grid_map_1208.jpg
diff --git a/grid_map_1213.jpg b/grid_map_1213.jpg
diff --git a/grid_map_1214.jpg b/grid_map_1214.jpg
diff --git a/grid_map_1215.jpg b/grid_map_1215.jpg
diff --git a/grid_map_1216.jpg b/grid_map_1216.jpg
diff --git a/grid_map_1217.jpg b/grid_map_1217.jpg
diff --git a/grid_map_thresh.jpg b/grid_map_thresh.jpg
diff --git a/grid_map_thresh_1208.jpg b/grid_map_thresh_1208.jpg
diff --git a/grid_map_thresh_1213.jpg b/grid_map_thresh_1213.jpg
diff --git a/grid_map_thresh_1214.jpg b/grid_map_thresh_1214.jpg
diff --git a/grid_map_thresh_1215.jpg b/grid_map_thresh_1215.jpg
diff --git a/grid_map_thresh_1216.jpg b/grid_map_thresh_1216.jpg
diff --git a/grid_map_thresh_1217.jpg b/grid_map_thresh_1217.jpg
diff --git a/grid_map_thresh_resized.jpg b/grid_map_thresh_resized.jpg
diff --git a/grid_map_thresh_resized1208.jpg b/grid_map_thresh_resized1208.jpg
diff --git a/grid_map_thresh_resized1213.jpg b/grid_map_thresh_resized1213.jpg
diff --git a/grid_map_thresh_resized1214.jpg b/grid_map_thresh_resized1214.jpg
diff --git a/grid_map_thresh_resized1215.jpg b/grid_map_thresh_resized1215.jpg
diff --git a/grid_map_thresh_resized1216.jpg b/grid_map_thresh_resized1216.jpg
diff --git a/grid_map_thresh_resized1217.jpg b/grid_map_thresh_resized1217.jpg
diff --git a/include/Tracking.h b/include/Tracking.h
@@ -76,6 +76,7 @@ class Tracking
 
 
 public:
+	bool loop_detected;
 
     // Tracking states
     enum eTrackingState{

diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc
@@ -401,6 +401,7 @@ bool LoopClosing::ComputeSim3()
 
 void LoopClosing::CorrectLoop()
 {
+	mpTracker->loop_detected = true;
 	loop_detected = true;
 	cout << "Loop detected!" << endl;
 

diff --git a/src/Tracking.cc b/src/Tracking.cc
@@ -47,7 +47,7 @@ namespace ORB_SLAM2
 Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor):
     mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc),
     mpKeyFrameDB(pKFDB), mpInitializer(static_cast<Initializer*>(NULL)), mpSystem(pSys), mpViewer(NULL),
-    mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0)
+	mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0), loop_detected(0)
 {
     // Load camera parameters from settings file
 

diff --git a/steps b/steps
@@ -77,6 +77,7 @@ rosrun ORB_SLAM2 Monopub Vocabulary/ORBvoc.txt Examples/Monocular/demo_cam.yaml
 
 start subscriber on kitti 00:
 rosrun ORB_SLAM2 Monosub 5 3 29 -25 48 -12 0.55 0.50 1 5
+rosrun ORB_SLAM2 Monosub 100 0.2 29 -25 48 -12 0.55 0.50 1 5
 
 start subscriber on rosbag:
 rosrun ORB_SLAM2 Monosub 30 5 2 -2 2 -2 0.55 0.50 1 5
-Original file line number
+Diff line change
@@ Expand Up / @@ -76,6 +76,7 @@ class Tracking @@
     public:
+    	bool loop_detected;
         // Tracking states
         enum eTrackingState{
@@ Expand Down @@