Done rectify processor

samples/api/camera.cc

@@ -51,6 +51,8 @@ int main(int argc, char *argv[]) {
             << ", temperature: " << data.imu->temperature;
   });
 
+  api->EnableStreamData(Stream::LEFT_RECTIFIED);
+  api->EnableStreamData(Stream::RIGHT_RECTIFIED);
   // Enable this will cache the motion datas until you get them.
   api->EnableMotionDatas();
   api->Start(Source::ALL);
@@ -62,8 +64,13 @@ int main(int argc, char *argv[]) {
   while (true) {
     api->WaitForStreams();
 
-    auto &&left_data = api->GetStreamData(Stream::LEFT);
+    // auto &&left_data = api->GetStreamData(Stream::LEFT);
-    auto &&right_data = api->GetStreamData(Stream::RIGHT);
+    // auto &&right_data = api->GetStreamData(Stream::RIGHT);
+    auto &&left_data = api->GetStreamData(Stream::LEFT_RECTIFIED);
+    auto &&right_data = api->GetStreamData(Stream::RIGHT_RECTIFIED);
+    if (left_data.frame.empty() || right_data.frame.empty()) {
+      continue;
+    }
 
     auto &&motion_datas = api->GetMotionDatas();
     motion_count += motion_datas.size();

src/api/processor/rectify_processor.cc

@@ -1,11 +1,20 @@
 #include "api/processor/rectify_processor.h"
 
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
 #include <glog/logging.h>
 
+#include "device/device.h"
+
 MYNTEYE_BEGIN_NAMESPACE
 
-RectifyProcessor::RectifyProcessor() : Processor() {
+RectifyProcessor::RectifyProcessor(std::shared_ptr<Device> device)
+    : Processor() {
   VLOG(2) << __func__;
+  InitParams(
+      device->GetIntrinsics(Stream::LEFT), device->GetIntrinsics(Stream::RIGHT),
+      device->GetExtrinsics(Stream::LEFT, Stream::RIGHT));
 }
 
 RectifyProcessor::~RectifyProcessor() {
@@ -17,14 +26,53 @@ std::string RectifyProcessor::Name() {
 }
 
 Object *RectifyProcessor::OnCreateOutput() {
-  return nullptr;
+  return new ObjMat2();
 }
 
 void RectifyProcessor::OnProcess(
     Object *const in, Object *const out, Processor *const parent) {
-  UNUSED(in)
-  UNUSED(out)
   UNUSED(parent)
+  const ObjMat2 *input = Object::Cast<ObjMat2>(in);
+  ObjMat2 *output = Object::Cast<ObjMat2>(out);
+  cv::remap(input->first, output->first, map11, map12, cv::INTER_LINEAR);
+  cv::remap(input->second, output->second, map21, map22, cv::INTER_LINEAR);
+}
+
+void RectifyProcessor::InitParams(
+    Intrinsics in_left, Intrinsics in_right, Extrinsics ex_left_to_right) {
+  cv::Size size{in_left.width, in_left.height};
+
+  cv::Mat M1 =
+      (cv::Mat_<double>(3, 3) << in_left.fx, 0, in_left.cx, 0, in_left.fy,
+       in_left.cy, 0, 0, 1);
+  cv::Mat M2 =
+      (cv::Mat_<double>(3, 3) << in_right.fx, 0, in_right.cx, 0, in_right.fy,
+       in_right.cy, 0, 0, 1);
+  cv::Mat D1(1, 5, CV_64F, in_left.coeffs);
+  cv::Mat D2(1, 5, CV_64F, in_right.coeffs);
+  cv::Mat R =
+      (cv::Mat_<double>(3, 3) << ex_left_to_right.rotation[0][0],
+       ex_left_to_right.rotation[0][1], ex_left_to_right.rotation[0][2],
+       ex_left_to_right.rotation[1][0], ex_left_to_right.rotation[1][1],
+       ex_left_to_right.rotation[1][2], ex_left_to_right.rotation[2][0],
+       ex_left_to_right.rotation[2][1], ex_left_to_right.rotation[2][2]);
+  cv::Mat T(3, 1, CV_64F, ex_left_to_right.translation);
+
+  VLOG(2) << "InitParams size: " << size;
+  VLOG(2) << "M1: " << M1;
+  VLOG(2) << "M2: " << M2;
+  VLOG(2) << "D1: " << D1;
+  VLOG(2) << "D2: " << D2;
+  VLOG(2) << "R: " << R;
+  VLOG(2) << "T: " << T;
+
+  cv::Rect left_roi, right_roi;
+  cv::stereoRectify(
+      M1, D1, M2, D2, size, R, T, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY,
+      0, size, &left_roi, &right_roi);
+
+  cv::initUndistortRectifyMap(M1, D1, R1, P1, size, CV_16SC2, map11, map12);
+  cv::initUndistortRectifyMap(M2, D2, R2, P2, size, CV_16SC2, map21, map22);
 }
 
 MYNTEYE_END_NAMESPACE

src/api/processor/rectify_processor.h

@@ -2,17 +2,23 @@
 #define MYNTEYE_RECTIFY_PROCESSOR_H_
 #pragma once
 
+#include <opencv2/core/core.hpp>
+
+#include <memory>
 #include <string>
 
 #include "api/processor/processor.h"
+#include "mynteye/types.h"
 
 MYNTEYE_BEGIN_NAMESPACE
 
+class Device;
+
 class RectifyProcessor : public Processor {
  public:
   static constexpr auto &&NAME = "RectifyProcessor";
 
-  RectifyProcessor();
+  explicit RectifyProcessor(std::shared_ptr<Device> device);
   virtual ~RectifyProcessor();
 
   std::string Name() override;
@@ -21,6 +27,13 @@ class RectifyProcessor : public Processor {
   Object *OnCreateOutput() override;
   void OnProcess(
       Object *const in, Object *const out, Processor *const parent) override;
+
+ private:
+  void InitParams(
+      Intrinsics in_left, Intrinsics in_right, Extrinsics ex_left_to_right);
+
+  cv::Mat R1, P1, R2, P2, Q;
+  cv::Mat map11, map12, map21, map22;
 };
 
 MYNTEYE_END_NAMESPACE

src/api/synthetic.cc

@@ -363,7 +363,7 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) {
 }
 
 void Synthetic::InitProcessors() {
-  auto &&rectify_processor = std::make_shared<RectifyProcessor>();
+  auto &&rectify_processor = std::make_shared<RectifyProcessor>(api_->device());
   auto &&disparity_processor = std::make_shared<DisparityProcessor>();
   auto &&disparitynormalized_processor =
       std::make_shared<DisparityNormalizedProcessor>();