test capabilities are a go

konanrobot · Feb 24, 2016 · 3f86602 · 3f86602
1 parent 8b63cc4
commit 3f86602
Show file tree

Hide file tree

Showing 4 changed files with 42 additions and 14 deletions.
diff --git a/include/LocalMapping.h b/include/LocalMapping.h
@@ -27,8 +27,11 @@
 #include "Tracking.h"
 #include <boost/thread.hpp>
 #include "KeyFrameDatabase.h"
+//#include "ProbabilityMapping.h"
 
 
+class ProbabilityMapping;
+
 namespace ORB_SLAM
 {
 
@@ -89,14 +92,15 @@ class LocalMapping
 
     LoopClosing* mpLoopCloser;
     Tracking* mpTracker;
-
+    ProbabilityMapping* mpProbabilityMapper;
+
     std::list<KeyFrame*> mlNewKeyFrames;
 
     KeyFrame* mpCurrentKeyFrame;
 
     std::list<MapPoint*> mlpRecentAddedMapPoints;
 
-    boost::mutex mMutexNewKFs;    
+   boost::mutex mMutexNewKFs;    
 
     bool mbAbortBA;
 

diff --git a/include/ProbabilityMapping.h b/include/ProbabilityMapping.h
@@ -38,19 +38,20 @@
 
 namespace ORB_SLAM {
 class KeyFrame;
-}
+} 
 
 namespace cv {
 class Mat;
 }
 
+
 class ProbabilityMapping {
 public:
 
 	struct depthHo {
 		float depth;
 		float sigma;
-	};
+        };
 
         ProbabilityMapping();
 	/* * \brief void first_loop(ORB_SLAM::KeyFrame kf, depthHo**, std::vector<depthHo>*): return results of epipolar search (depth hypotheses) */

diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc
@@ -30,8 +30,8 @@ namespace ORB_SLAM
 
 LocalMapping::LocalMapping(Map *pMap):
     mbResetRequested(false), mpMap(pMap),  mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbAcceptKeyFrames(true)
-{
-}
+  {
+  }
 
 void LocalMapping::SetLoopCloser(LoopClosing* pLoopCloser)
 {
@@ -82,14 +82,23 @@ void LocalMapping::Run()
                 // Tracking will see Local Mapping idle
                 if(!CheckNewKeyFrames())
                 {
+                    cout << "No new keyframes\n";
                     SetAcceptKeyFrames(true);
+
+                    //TODO actually turn this into a Thread
+                  /*  vector<KeyFrame*> vpKFs = mpMap -> GetAllKeyFrames();
+                    ProbabilityMapping::detphHo*** hypothesisMatrix;
+                    vector<ProbabilityMapping::depthHo*>* hypothesisSupport; 
                     for(size_t i=0; i<vpKFs.size(); i++)
                     {
+                        cout << "We have keyframes\n";
                         ORB_SLAM::KeyFrame* pKF = vpKFs[i];
                         if(pKF->isBad())                      
-                            continue; 
-                        ProbabilityMapper.FirstLoop(pKF,hypothesisMatrix,hypothesisSupport); 
-                    }           
+                            continue;
+                        cout << "we are doing the Mapping\n";
+                        mpProbabilityMapper -> FirstLoop(pKF,hypothesisMatrix,hypothesisSupport); 
+                    } 
+                    */
                 }
             }
 

diff --git a/src/main.cc b/src/main.cc
@@ -152,13 +152,30 @@ int main(int argc, char **argv)
         fps=30;
 
     ros::Rate r(fps);
-
+
+    int count = 0;
+    std::vector<ProbabilityMapping::depthHo*> *hypothesisSupport;
+    ProbabilityMapping::depthHo ***hypothesisMatrix;//FIXME 
     while (ros::ok())
     {
         FramePub.Refresh();
         MapPub.Refresh();
         Tracker.CheckResetByPublishers();
-        r.sleep(); 
+        r.sleep();
+        if(count%300000 == 0){
+            cout << "Loop!\n";
+            vector<ORB_SLAM::KeyFrame*> vpKFs = World.GetAllKeyFrames();
+            for(size_t i = 0; i <vpKFs.size(); i++)
+            {
+              cout << "We have keyframes!\n";
+              ORB_SLAM::KeyFrame* pKF = vpKFs[i];
+              if(pKF->isBad())
+                  continue;
+              cout << "Mapping...\n";
+              ProbabilityMapper.FirstLoop(pKF,hypothesisMatrix,hypothesisSupport);
+            }
+
+        }
     }
 
     // Save keyframe poses at the end of the execution
@@ -187,9 +204,6 @@ int main(int argc, char **argv)
 
     }
 
-    std::vector<ProbabilityMapping::depthHo*> *hypothesisSupport;
-    ProbabilityMapping::depthHo ***hypothesisMatrix;//FIXME
-
     f.close();
 
     ros::shutdown();