From f92ecd437133340320373f7e6e2c1a23481d5cd5 Mon Sep 17 00:00:00 2001
From: Osenberg-Y <osenberg@qq.com>
Date: Thu, 9 Aug 2018 12:14:54 +0800
Subject: [PATCH] optimized pointscloud

---
 src/api/processor/disparity_processor.cc | 72 +++++++-----------------
 src/api/processor/disparity_processor.h  |  6 +-
 2 files changed, 26 insertions(+), 52 deletions(-)

diff --git a/src/api/processor/disparity_processor.cc b/src/api/processor/disparity_processor.cc
index dbb5ab3..83e7349 100644
--- a/src/api/processor/disparity_processor.cc
+++ b/src/api/processor/disparity_processor.cc
@@ -26,38 +26,26 @@ const char DisparityProcessor::NAME[] = "DisparityProcessor";
 DisparityProcessor::DisparityProcessor(std::int32_t proc_period)
     : Processor(std::move(proc_period)) {
   VLOG(2) << __func__ << ": proc_period=" << proc_period;
-  int sgbmWinSize = 3;
-  int numberOfDisparities = 64;
+	int blockSize_ = 15;           // 15
+	int minDisparity_ = 0;         // 0
+	int numDisparities_ = 64;      // 64
+	int preFilterSize_ = 9;        // 9
+	int preFilterCap_ = 31;        // 31
+	int uniquenessRatio_ = 15;     // 15
+	int textureThreshold_ = 10;    // 10
+	int speckleWindowSize_ = 100;  // 100
+	int speckleRange_ = 4;         // 4
 
-#ifdef USE_OPENCV2
-  // StereoSGBM
-  //   http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?#stereosgbm
-  sgbm_ = cv::Ptr<cv::StereoSGBM>(
-      new cv::StereoSGBM(
-          0,                               // minDisparity
-          numberOfDisparities,             // numDisparities
-          sgbmWinSize,                     // SADWindowSize
-          8 * sgbmWinSize * sgbmWinSize,   // P1
-          32 * sgbmWinSize * sgbmWinSize,  // P2
-          1,                               // disp12MaxDiff
-          63,                              // preFilterCap
-          10,                              // uniquenessRatio
-          100,                             // speckleWindowSize
-          32,                              // speckleRange
-          false));                         // fullDP
-#else
-  sgbm_ = cv::StereoSGBM::create(0, 16, 3);
-  sgbm_->setPreFilterCap(63);
-  sgbm_->setBlockSize(sgbmWinSize);
-  sgbm_->setP1(8 * sgbmWinSize * sgbmWinSize);
-  sgbm_->setP2(32 * sgbmWinSize * sgbmWinSize);
-  sgbm_->setMinDisparity(0);
-  sgbm_->setNumDisparities(numberOfDisparities);
-  sgbm_->setUniquenessRatio(10);
-  sgbm_->setSpeckleWindowSize(100);
-  sgbm_->setSpeckleRange(32);
-  sgbm_->setDisp12MaxDiff(1);
-#endif
+	bm_ = cv::StereoBM::create();
+	bm_->setBlockSize(blockSize_);
+	bm_->setMinDisparity(minDisparity_);
+	bm_->setNumDisparities(numDisparities_);
+	bm_->setPreFilterSize(preFilterSize_);
+	bm_->setPreFilterCap(preFilterCap_);
+	bm_->setUniquenessRatio(uniquenessRatio_);
+	bm_->setTextureThreshold(textureThreshold_);
+	bm_->setSpeckleWindowSize(speckleWindowSize_);
+	bm_->setSpeckleRange(speckleRange_);
 }
 
 DisparityProcessor::~DisparityProcessor() {
@@ -79,26 +67,8 @@ bool DisparityProcessor::OnProcess(
   ObjMat *output = Object::Cast<ObjMat>(out);
 
   cv::Mat disparity;
-#ifdef USE_OPENCV2
-  // StereoSGBM::operator()
-  //   http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#stereosgbm-operator
-  // Output disparity map. It is a 16-bit signed single-channel image of the
-  // same size as the input image.
-  // It contains disparity values scaled by 16. So, to get the floating-point
-  // disparity map,
-  // you need to divide each disp element by 16.
-  (*sgbm_)(input->first, input->second, disparity);
-#else
-  // compute()
-  //   http://docs.opencv.org/master/d2/d6e/classcv_1_1StereoMatcher.html
-  // Output disparity map. It has the same size as the input images.
-  // Some algorithms, like StereoBM or StereoSGBM compute 16-bit fixed-point
-  // disparity map
-  // (where each disparity value has 4 fractional bits),
-  // whereas other algorithms output 32-bit floating-point disparity map.
-  sgbm_->compute(input->first, input->second, disparity);
-#endif
-  output->value = disparity / 16 + 1;
+  bm_->compute(input->first, input->second, disparity);
+  output->value = disparity;
   return true;
 }
 
diff --git a/src/api/processor/disparity_processor.h b/src/api/processor/disparity_processor.h
index 82b075f..da4f78b 100644
--- a/src/api/processor/disparity_processor.h
+++ b/src/api/processor/disparity_processor.h
@@ -17,13 +17,17 @@
 
 #include <string>
 
+#include <opencv2/calib3d/calib3d.hpp>
+
 #include "api/processor/processor.h"
 
+#if 0
 namespace cv {
 
 class StereoSGBM;
 
 }  // namespace cv
+#endif
 
 MYNTEYE_BEGIN_NAMESPACE
 
@@ -42,7 +46,7 @@ class DisparityProcessor : public Processor {
       Object *const in, Object *const out, Processor *const parent) override;
 
  private:
-  cv::Ptr<cv::StereoSGBM> sgbm_;
+  cv::Ptr<cv::StereoBM> bm_;
 };
 
 MYNTEYE_END_NAMESPACE