From fbd416324e73c7552b64920091156b30021ae151 Mon Sep 17 00:00:00 2001
From: TinyOh <tinyoh@slightech.com>
Date: Thu, 10 Jan 2019 17:30:28 +0800
Subject: [PATCH] feat(calib models): float change to ushort ,fix the T_mul_f
 number

---
 src/mynteye/api/processor/depth_processor.cc   |  9 ++++-----
 src/mynteye/api/processor/points_processor.cc  | 13 ++++++-------
 src/mynteye/api/processor/rectify_processor.cc |  2 +-
 3 files changed, 11 insertions(+), 13 deletions(-)

diff --git a/src/mynteye/api/processor/depth_processor.cc b/src/mynteye/api/processor/depth_processor.cc
index 10ce715..a1f9251 100644
--- a/src/mynteye/api/processor/depth_processor.cc
+++ b/src/mynteye/api/processor/depth_processor.cc
@@ -48,15 +48,14 @@ bool DepthProcessor::OnProcess(
   ObjMat *output = Object::Cast<ObjMat>(out);
   int rows = input->value.rows;
   int cols = input->value.cols;
-  std::cout << calib_infos_->T_mul_f << std::endl;
+  // std::cout << calib_infos_->T_mul_f << std::endl;
   // 0.0793434
-  // TODO(MYNTEYE): Put the corresponding parameters(T,f)
-  cv::Mat depth_mat = cv::Mat::zeros(rows, cols, CV_32F);
+  cv::Mat depth_mat = cv::Mat::zeros(rows, cols, CV_16U);
   for (int i = 0; i < rows; i++) {
     for (int j = 0; j < cols; j++) {
       float disparity_value = input->value.at<float>(i, j);
-      float depth = calib_infos_->T_mul_f * 1000.0 / disparity_value ;
-      depth_mat.at<float>(i, j) = depth;
+      float depth = calib_infos_->T_mul_f / disparity_value;
+      depth_mat.at<ushort>(i, j) = depth * 1000;
     }
   }
   output->value = depth_mat;
diff --git a/src/mynteye/api/processor/points_processor.cc b/src/mynteye/api/processor/points_processor.cc
index e41c3f8..1d40ae1 100644
--- a/src/mynteye/api/processor/points_processor.cc
+++ b/src/mynteye/api/processor/points_processor.cc
@@ -32,7 +32,7 @@ struct DepthTraits<uint16_t> {
   static inline bool valid(uint16_t depth) { return depth != 0; }
   static inline float toMeters(uint16_t depth) { return depth * 0.001f; } // originally mm
   static inline uint16_t fromMeters(float depth) { return (depth * 1000.0f) + 0.5f; }
-  static inline void initializeBuffer(std::vector<uint8_t>& buffer) {} // Do nothing - already zero-filled
+  static inline void initializeBuffer(std::vector<uint16_t>& buffer) {} // Do nothing - already zero-filled
 };
 
 template<>
@@ -100,16 +100,15 @@ bool PointsProcessor::OnProcess(
   for (int v = 0; v < height; ++v) {
     cv::Vec3f *dptr = output->value.ptr<cv::Vec3f>(v);
     for (int u = 0; u < width; ++u) {
-      float depth = input->value.at<float>(v, u);
+      float depth = input->value.at<uint16_t>(v, u);
 
       // Missing points denoted by NaNs
-      if (!DepthTraits<float>::valid(depth)) {
+      if (!DepthTraits<uint16_t>::valid(depth)) {
         continue;
       }
-
-      dptr[u][0] = (u - center_x) * depth * constant_x;
-      dptr[u][1] = (v - center_y) * depth * constant_y;
-      dptr[u][2] = DepthTraits<float>::toMeters(depth);
+      dptr[u][0] = (u - center_x) * depth * constant_x * 1000.0;
+      dptr[u][1] = (v - center_y) * depth * constant_y * 1000.0;
+      dptr[u][2] = depth * 1000.0;
     }
   }
 
diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc
index b246005..731935f 100644
--- a/src/mynteye/api/processor/rectify_processor.cc
+++ b/src/mynteye/api/processor/rectify_processor.cc
@@ -167,7 +167,7 @@ void RectifyProcessor::stereoRectify(camodocal::CameraPtr leftOdo,
   _pp[0][2] = cc_new[1].x;
   _pp[1][2] = cc_new[1].y;
   _pp[idx][3] = _t[idx]*fc_new;  // baseline * focal length
-  *T_mul_f = 0. - _t[idx];
+  *T_mul_f = 0. - _t[idx] * fc_new;
   cvConvert(&pp, _P2);
 
   alpha = MIN(alpha, 1.);