diff --git a/README.md b/README.md index 7f24989591..dfb4f3e79f 100644 --- 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 index b9103b42b7..b84e4e34ba 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -696,16 +696,17 @@ namespace ORB_SLAM2 mpPosHg.at(1) = mpPos.at(1); mpPosHg.at(2) = mpPos.at(2); cv::Mat v3Temp = mpPosHg; + //std::cout << "Mat: " << mpPos.at(0) << " " << mpPos.at(1) << " " << mpPos.at(2) << std::endl; const double z = v3Temp.at(2); if (z > 0) { depth_vec.push_back(z); //std::cout << "Mat: " << mpPos << std::endl; // std::cout << "at: " << v3Temp.at(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); } } @@ -713,8 +714,8 @@ namespace ORB_SLAM2 { 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; }