depth hypothesis fusion

konanrobot · Feb 23, 2016 · cdfffda · cdfffda
1 parent 530efbc
commit cdfffda
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 7 deletions.
diff --git a/include/ProbabilityMapping.h b/include/ProbabilityMapping.h
@@ -54,7 +54,7 @@ class ProbabilityMapping {
 
         ProbabilityMapping();
 	/* * \brief void first_loop(ORB_SLAM::KeyFrame kf, depthHo**, std::vector<depthHo>*): return results of epipolar search (depth hypotheses) */
-	void FirstLoop(ORB_SLAM::KeyFrame *kf, depthHo*** ho, std::vector<depthHo*>* support);
+	void FirstLoop(ORB_SLAM::KeyFrame *kf, depthHo*** ho);
         /* * \brief void stereo_search_constraints(): return min, max inverse depth */
         void StereoSearchConstraints(ORB_SLAM::KeyFrame* kf, float* min_depth, float* max_depth);
 	/* * \brief void epipolar_search(): return distribution of inverse depths/sigmas for each pixel */

diff --git a/src/ProbabilityMapping.cc b/src/ProbabilityMapping.cc
@@ -48,9 +48,7 @@ void ProbabilityMapping::GetIntensityGradient(cv::Mat im, float* g) {}
 
 
 //  depthHo ho[image.rows][image.cols];
-void ProbabilityMapping::FirstLoop(ORB_SLAM::KeyFrame *kf, depthHo*** ho, std::vector<depthHo*>* depth_ho){
-
-  depth_ho->clear();
+void ProbabilityMapping::FirstLoop(ORB_SLAM::KeyFrame *kf, depthHo*** ho){
 
   std::vector<ORB_SLAM::KeyFrame*> closestMatches = kf->GetBestCovisibilityKeyFrames(covisN);
 
@@ -62,19 +60,29 @@ void ProbabilityMapping::FirstLoop(ORB_SLAM::KeyFrame *kf, depthHo*** ho, std::v
   cv::Mat image = kf->GetImage();
   GetImageGradient(image, &gradx, &grady, &grad);
 
+  std::vector<depthHo*> depth_ho;
   for(int x = 0; x < image.rows; x++){
     for(int y = 0; y < image.cols; y++){
       ho[x][y] = NULL;
       if(grad.at<float>(x,y) < lambdaG)
         continue;
-
+
+      depth_ho.clear(); 
       for(size_t i=0; i<closestMatches.size(); i++){
         ORB_SLAM::KeyFrame* kf2 = closestMatches[i];
 
-        struct depthHo* dh;
+        struct depthHo* dh = NULL;
         EpipolarSearch(kf, kf2, x, y, gradx, grady, grad, min_depth, max_depth, dh);
-        depth_ho->push_back(dh);
+        if (dh != NULL)
+            depth_ho.push_back(dh);
+      }
+
+      if (depth_ho.size()) {
+        struct depthHo* dh;
+        InverseDepthHypothesisFusion(depth_ho, dh);
         ho[x][y] = dh;
+      } else {
+        ho[x][y] = NULL;
       }
     }
   }