From c1b8f788d6928c8823a05bd1efa746c2f993cd0b Mon Sep 17 00:00:00 2001
From: pizzoli <matia.pizzoli@gmail.com>
Date: Thu, 29 Oct 2015 23:54:57 +0100
Subject: [PATCH] tested depth estimation node

---
 include/rmd/depthmap.h |  2 ++
 src/depthmap.cpp       | 13 +++++++++++++
 src/depthmap_node.cpp  | 12 +++++++++++-
 test/dataset.cpp       | 13 -------------
 test/dataset.h         |  1 -
 test/dataset_main.cpp  |  4 ++--
 6 files changed, 28 insertions(+), 17 deletions(-)

diff --git a/include/rmd/depthmap.h b/include/rmd/depthmap.h
index d7307e6..9b313cc 100644
--- a/include/rmd/depthmap.h
+++ b/include/rmd/depthmap.h
@@ -58,6 +58,8 @@ class Depthmap
   void outputDepthmap(cv::Mat &depth_32fc1) const;
   void outputDenoisedDepthmap(cv::Mat &depth_32fc1, float lambda, int iterations);
 
+  static cv::Mat scaleMat(const cv::Mat &depthmap);
+
 private:
   void inputImage(const cv::Mat &img_8uc1);
 
diff --git a/src/depthmap.cpp b/src/depthmap.cpp
index a10f30d..15ef8e6 100644
--- a/src/depthmap.cpp
+++ b/src/depthmap.cpp
@@ -109,3 +109,16 @@ void rmd::Depthmap::outputDenoisedDepthmap(cv::Mat &depth_32fc1, float lambda, i
         lambda,
         iterations);
 }
+
+cv::Mat rmd::Depthmap::scaleMat(const cv::Mat &depthmap)
+{
+  cv::Mat scaled_depthmap = depthmap.clone();
+  double min_val, max_val;
+  cv::minMaxLoc(scaled_depthmap, &min_val, &max_val);
+  cv::Mat converted;
+  scaled_depthmap = (scaled_depthmap - min_val) * 1.0 / (max_val - min_val);
+  scaled_depthmap.convertTo(converted, CV_8UC1, 255);
+  cv::Mat colored(converted.rows, converted.cols, CV_8UC3);
+  cv::cvtColor(converted, colored, CV_GRAY2BGR);
+  return colored;
+}
diff --git a/src/depthmap_node.cpp b/src/depthmap_node.cpp
index 5bfe711..ff82d51 100644
--- a/src/depthmap_node.cpp
+++ b/src/depthmap_node.cpp
@@ -60,6 +60,7 @@ void rmd::DepthmapNode::denseInputCallback(
 
   switch (state_) {
   case rmd::State::TAKE_REFERENCE_FRAME:
+  {
     if(depthmap_.setReferenceImage(
          img_8uC1,
          T_world_curr.inv(),
@@ -73,9 +74,18 @@ void rmd::DepthmapNode::denseInputCallback(
       std::cerr << "ERROR: could not set reference image" << std::endl;
     }
     break;
+  }
   case rmd::State::UPDATE:
-      depthmap_.update(img_8uC1, T_world_curr.inv());
+  {
+    depthmap_.update(img_8uC1, T_world_curr.inv());
+#if 1
+    cv::Mat curr_depth;
+    depthmap_.outputDepthmap(curr_depth);
+    cv::imshow("curr_depth", rmd::Depthmap::scaleMat(curr_depth));
+    cv::waitKey(2);
+#endif
     break;
+  }
   default:
     break;
   }
diff --git a/test/dataset.cpp b/test/dataset.cpp
index 989e98e..d29e0ec 100644
--- a/test/dataset.cpp
+++ b/test/dataset.cpp
@@ -196,19 +196,6 @@ const rmd::test::DatasetEntry & rmd::test::Dataset::operator()(size_t index) con
   return dataset_.at(index);
 }
 
-cv::Mat rmd::test::Dataset::scaleMat(const cv::Mat &depthmap)
-{
-  cv::Mat scaled_depthmap = depthmap.clone();
-  double min_val, max_val;
-  cv::minMaxLoc(scaled_depthmap, &min_val, &max_val);
-  cv::Mat converted;
-  scaled_depthmap = (scaled_depthmap - min_val) * 1.0 / (max_val - min_val);
-  scaled_depthmap.convertTo(converted, CV_8UC1, 255);
-  cv::Mat colored(converted.rows, converted.cols, CV_8UC3);
-  cv::cvtColor(converted, colored, CV_GRAY2BGR);
-  return colored;
-}
-
 bool rmd::test::Dataset::loadPathFromEnv()
 {
   const char *env_path = std::getenv(data_path_env_var);
diff --git a/test/dataset.h b/test/dataset.h
index f5db70a..a93395a 100644
--- a/test/dataset.h
+++ b/test/dataset.h
@@ -72,7 +72,6 @@ class Dataset
   std::vector<DatasetEntry>::const_iterator end() const;
   const DatasetEntry & operator()(size_t index) const;
 
-  static cv::Mat scaleMat(const cv::Mat &depthmap);
   bool loadPathFromEnv();
   static const char * getDataPathEnvVar();
 
diff --git a/test/dataset_main.cpp b/test/dataset_main.cpp
index ebf4cfb..c724b35 100644
--- a/test/dataset_main.cpp
+++ b/test/dataset_main.cpp
@@ -89,13 +89,13 @@ int main(int argc, char **argv)
   // show depthmap
   cv::Mat result;
   depthmap.outputDepthmap(result);
-  cv::Mat colored = rmd::test::Dataset::scaleMat(result);
+  cv::Mat colored = rmd::Depthmap::scaleMat(result);
   cv::imshow("result", colored);
 
   // denoise
   cv::Mat denoised_result;
   depthmap.outputDenoisedDepthmap(denoised_result, 0.4f, 200);
-  cv::Mat colored_denoised = rmd::test::Dataset::scaleMat(denoised_result);
+  cv::Mat colored_denoised = rmd::Depthmap::scaleMat(denoised_result);
   cv::imshow("denoised_result", colored_denoised);
 
   cv::waitKey();