fix(calib): covert params change.

include/mynteye/types.h

@@ -775,6 +775,7 @@ struct CameraROSMsgInfoPair {
   struct CameraROSMsgInfo left;
   struct CameraROSMsgInfo right;
   double T_mul_f = -1.f;
+  double cx1_minus_cx2 = 0.f;
   double  R[9] = {0};
   double  P[12] = {0};
 };

src/mynteye/api/processor/disparity_processor.cc

@@ -27,8 +27,14 @@ MYNTEYE_BEGIN_NAMESPACE
 const char DisparityProcessor::NAME[] = "DisparityProcessor";
 
 DisparityProcessor::DisparityProcessor(DisparityComputingMethod type,
+    std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos,
     std::int32_t proc_period)
     : Processor(std::move(proc_period)), type_(type) {
+  if (calib_infos) {
+    cx1_minus_cx2_ = calib_infos->cx1_minus_cx2;
+  } else {
+    cx1_minus_cx2_ = 1.f;
+  }
   VLOG(2) << __func__ << ": proc_period=" << proc_period;
   int sgbmWinSize = 3;
   int numberOfDisparities = 64;
@@ -156,6 +162,7 @@ bool DisparityProcessor::OnProcess(
   } else if (type_ == DisparityComputingMethod::BM) {
     // LOG(ERROR) << "not supported in opencv 2.x";
     (*sgbm_matcher)(input->first, input->second, disparity);
+    disparity.convertTo(output->value, CV_32F, 1./16, 1);
     // cv::Mat tmp1, tmp2;
     // cv::cvtColor(input->first, tmp1, CV_RGB2GRAY);
     // cv::cvtColor(input->second, tmp2, CV_RGB2GRAY);
@@ -188,7 +195,7 @@ bool DisparityProcessor::OnProcess(
     sgbm_matcher->compute(input->first, input->second, disparity);
   }
 #endif
-  disparity.convertTo(output->value, CV_32F, 1./16, 1);
+  disparity.convertTo(output->value, CV_32F, 1./16, cx1_minus_cx2_);
   output->id = input->first_id;
   output->data = input->first_data;
   return true;

src/mynteye/api/processor/disparity_processor.h

@@ -32,6 +32,7 @@ class DisparityProcessor : public Processor {
   static const char NAME[];
 
   explicit DisparityProcessor(DisparityComputingMethod type,
+      std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos,
       std::int32_t proc_period = 0);
   virtual ~DisparityProcessor();
 
@@ -54,6 +55,7 @@ class DisparityProcessor : public Processor {
   cv::Ptr<cv::StereoSGBM> sgbm_matcher;
   cv::Ptr<cv::StereoBM> bm_matcher;
   DisparityComputingMethod type_;
+  double cx1_minus_cx2_;
 };
 
 MYNTEYE_END_NAMESPACE

src/mynteye/api/processor/rectify_processor.cc

@@ -42,6 +42,7 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo,
     const CvMat* D1, const CvMat* D2, CvSize imageSize,
     const CvMat* matR, const CvMat* matT,
     CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, double* T_mul_f,
+    double *cx1_min_cx2,
     int flags, double alpha, CvSize newImgSize) {
   // std::cout << _alpha << std::endl;
   alpha = _alpha;
@@ -218,6 +219,8 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo,
     cvmSet(_P2, 0, 2, cx2);
     cvmSet(_P2, 1, 2, cy2);
     cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3));
+
+    *cx1_min_cx2 = -(cx1 - cx2);
   }
 }
 
@@ -299,8 +302,10 @@ std::shared_ptr<struct CameraROSMsgInfoPair> RectifyProcessor::stereoRectify(
   CvMat c_K1 = K1, c_K2 = K2, c_D1 = D1, c_D2 = D2;
   CvMat c_R1 = R1, c_R2 = R2, c_P1 = P1, c_P2 = P2;
   double T_mul_f;
+  double cx1_min_cx2;
   stereoRectify(leftOdo, rightOdo, &c_K1, &c_K2, &c_D1, &c_D2,
-      image_size1, &c_R, &c_t, &c_R1, &c_R2, &c_P1, &c_P2, &T_mul_f);
+      image_size1, &c_R, &c_t, &c_R1, &c_R2, &c_P1, &c_P2, &T_mul_f,
+      &cx1_min_cx2);
 
   VLOG(2) << "K1: " << K1 << std::endl;
   VLOG(2) << "D1: " << D1 << std::endl;
@@ -334,6 +339,7 @@ std::shared_ptr<struct CameraROSMsgInfoPair> RectifyProcessor::stereoRectify(
   info_pair.left = calib_mat_data_left;
   info_pair.right = calib_mat_data_right;
   info_pair.T_mul_f = T_mul_f;
+  info_pair.cx1_minus_cx2 = cx1_min_cx2;
   for (std::size_t i = 0; i< 3 * 4; i++) {
     info_pair.P[i] = calib_mat_data_left.P[i];
   }

src/mynteye/api/processor/rectify_processor.h

@@ -87,6 +87,7 @@ class RectifyProcessor : public Processor {
       const CvMat* D1, const CvMat* D2, CvSize imageSize,
       const CvMat* matR, const CvMat* matT,
       CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, double* T_mul_f,
+      double *cx1_min_cx2,
       int flags = cv::CALIB_ZERO_DISPARITY, double alpha = -1,
       CvSize newImgSize = cv::Size());
 

src/mynteye/api/synthetic.cc

@@ -313,10 +313,8 @@ void Synthetic::InitProcessors() {
   std::shared_ptr<Processor> rectify_processor = nullptr;
   std::shared_ptr<Processor> points_processor = nullptr;
   std::shared_ptr<Processor> depth_processor = nullptr;
+  std::shared_ptr<Processor> disparity_processor = nullptr;
 
-  auto &&disparity_processor =
-      std::make_shared<DisparityProcessor>(DisparityComputingMethod::BM,
-                                           DISPARITY_PROC_PERIOD);
   auto &&disparitynormalized_processor =
       std::make_shared<DisparityNormalizedProcessor>(
           DISPARITY_NORM_PROC_PERIOD);
@@ -333,6 +331,10 @@ void Synthetic::InitProcessors() {
     points_processor = std::make_shared<PointsProcessorOCV>(
         rectify_processor_ocv->Q, POINTS_PROC_PERIOD);
     depth_processor = std::make_shared<DepthProcessorOCV>(DEPTH_PROC_PERIOD);
+    disparity_processor =
+      std::make_shared<DisparityProcessor>(DisparityComputingMethod::BM,
+                                           nullptr,
+                                           DISPARITY_PROC_PERIOD);
 
     root_processor->AddChild(rectify_processor);
     rectify_processor->AddChild(disparity_processor);
@@ -352,6 +354,10 @@ void Synthetic::InitProcessors() {
     depth_processor = std::make_shared<DepthProcessor>(
         rectify_processor_imp -> getCameraROSMsgInfoPair(),
         DEPTH_PROC_PERIOD);
+    disparity_processor =
+      std::make_shared<DisparityProcessor>(DisparityComputingMethod::BM,
+          rectify_processor_imp -> getCameraROSMsgInfoPair(),
+          DISPARITY_PROC_PERIOD);
 
     root_processor->AddChild(rectify_processor);
     rectify_processor->AddChild(disparity_processor);