Correctly working with REMODE

raulmur · Feb 17, 2017 · 2aab8de · 2aab8de
1 parent 2fdf0f9
commit 2aab8de
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 7 deletions.
diff --git a/README.md b/README.md
@@ -1,11 +1,17 @@
 # ORB-SLAM2
 Modified version of ORB-SLAM2 
 
-**Changelog** Compatible with ROS Kinetic (OpenCV 3, Ubuntu 16.04)
- 	      Stereo node pose and tf publisher
+**Change Log**  
+[Author: Ayush Gaud]  
+Compatible with ROS Kinetic (OpenCV 3, Ubuntu 16.04)  
+Stereo node pose and tf publisher  
+Monocular pose publish  
+Compatible with REMODE (Topic: /ORB/DenseInput)  
+
+![orb remode](https://cloud.githubusercontent.com/assets/4923897/23068075/20bd81d4-f548-11e6-824d-ace54982f74b.png)                
 
 
-**Authors:** [Ayush Gaud] [Raul Mur-Artal (Original Author)](http://webdiis.unizar.es/~raulmur/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/), [J. M. M. Montiel](http://webdiis.unizar.es/~josemari/) and [Dorian Galvez-Lopez](http://doriangalvez.com/) ([DBoW2](https://github.com/dorian3d/DBoW2))
+**Authors:** [Raul Mur-Artal (Original Author)](http://webdiis.unizar.es/~raulmur/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/), [J. M. M. Montiel](http://webdiis.unizar.es/~josemari/) and [Dorian Galvez-Lopez](http://doriangalvez.com/) ([DBoW2](https://github.com/dorian3d/DBoW2))
 
 **Current version:** 1.0.0 
 

diff --git a/src/Frame.cc b/src/Frame.cc
@@ -696,25 +696,26 @@ namespace ORB_SLAM2
                         mpPosHg.at<double>(1) = mpPos.at<double>(1);
                         mpPosHg.at<double>(2) = mpPos.at<double>(2);
                         cv::Mat v3Temp = mpPosHg;
+                        //std::cout << "Mat: " << mpPos.at<float>(0) << " " << mpPos.at<float>(1) << " " << mpPos.at<float>(2) << std::endl;
                         const double z = v3Temp.at<float>(2);
                         if (z > 0)
                         {
                             depth_vec.push_back(z);
                             //std::cout << "Mat: " << mpPos << std::endl;
                             // std::cout << "at: " << v3Temp.at<float>(2) << std::endl;
-                            std::cout << "Z: " << z << std::endl;
+                            //std::cout << "Z: " << z << std::endl;
                             depth_min = fmin(z, depth_min);
                             //depth_min = fmax(0, depth_min);
-                            //depth_max = fmax(z, depth_max);
+                            depth_max = fmax(z, depth_max);
                         }
                     }
 
                     if(depth_vec.empty())
                     {
                         return false;
                     }
-                    std::nth_element(depth_vec.begin(), depth_vec.begin() + depth_vec.size()/2, depth_vec.end());
-                    depth_max = depth_vec[depth_vec.size()/2];
+                    // std::nth_element(depth_vec.begin(), depth_vec.begin() + depth_vec.size()/2, depth_vec.end());
+                    // depth_max = depth_vec[depth_vec.size()/2];
                 }
                 return true;
             }