feat(calib): add process impl code

include/mynteye/types.h

@@ -464,15 +464,8 @@ struct MYNTEYE_API IntrinsicsEquidistant : public IntrinsicsBase {
   std::uint16_t width;
   /** The height of the image in pixels */
   std::uint16_t height;
-  /** The distortion coefficients */
-  double k2;
-  double k3;
-  double k4;
-  double k5;
-  double mu;
-  double mv;
-  double u0;
-  double v0;
+  /** The distortion coefficients k2,k3,k4,k5,mu,mv,u0,v0*/
+  double coeffs[8];
 };
 
 MYNTEYE_API

src/mynteye/api/processor/rectify_processor.cc

@@ -43,10 +43,20 @@ std::string RectifyProcessor::Name() {
 void RectifyProcessor::NotifyImageParamsChanged() {
   auto in_left = device_->GetIntrinsics(Stream::LEFT);
   auto in_right = device_->GetIntrinsics(Stream::RIGHT);
-  InitParams(
+  if (in_left->calib_model == CalibrationModel::CALIB_MODEL_PINHOLE) {
+    InitParams(
       *std::dynamic_pointer_cast<IntrinsicsPinhole>(in_left),
       *std::dynamic_pointer_cast<IntrinsicsPinhole>(in_right),
       device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT));
+  } else if (in_left->calib_model ==
+             CalibrationModel::CALIB_MODEL_KANNALA_BRANDT) {
+    InitParams(
+      *std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_left),
+      *std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_right),
+      device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT));
+  } else {
+    // toido
+  }
 }
 
 Object *RectifyProcessor::OnCreateOutput() {
@@ -106,4 +116,78 @@ void RectifyProcessor::InitParams(
   cv::initUndistortRectifyMap(M2, D2, R2, P2, size, CV_16SC2, map21, map22);
 }
 
+// enum class CalibrationModel : std::uint8_t {
+//   /** Unknow */
+//   CALIB_MODEL_UNKNOW = 0,
+//   /** Pinhole */
+//   CALIB_MODEL_PINHOLE = 1,
+//   /** Equidistant: KANNALA_BRANDT */
+//   CALIB_MODEL_KANNALA_BRANDT = 2,
+//   // CALIB_MODEL_SCARAMUZZA,
+//   // CALIB_MODEL_MEI,
+// };
+
+// /**
+//  * @ingroup calibration
+//  * Stream intrinsics (Equidistant: KANNALA_BRANDT)
+//  */
+// struct MYNTEYE_API IntrinsicsEquidistant : public IntrinsicsBase {
+//   IntrinsicsEquidistant() {
+//     calib_model = CalibrationModel::CALIB_MODEL_KANNALA_BRANDT;
+//   }
+//   /** The width of the image in pixels */
+//   std::uint16_t width;
+//   /** The height of the image in pixels */
+//   std::uint16_t height;
+//   /** The distortion coefficients */
+//   double k2;
+//   double k3;
+//   double k4;
+//   double k5;
+//   double mu;
+//   double mv;
+//   double u0;
+//   double v0;
+// };
+
+void RectifyProcessor::InitParams(
+    IntrinsicsEquidistant in_left,
+    IntrinsicsEquidistant in_right,
+    Extrinsics ex_right_to_left) {
+  cv::Size size{in_left.width, in_left.height};
+
+  cv::Mat M1 =
+      (cv::Mat_<double>(3, 3) << 1.0, 0, 1.0, 0, 1.0,
+       1.0, 0, 0, 1);
+  cv::Mat M2 =
+      (cv::Mat_<double>(3, 3) << 1.0, 0, 1.0, 0, 1.0,
+       1.0, 0, 0, 1);
+  cv::Mat D1(1, 8, CV_64F, in_left.coeffs);
+  cv::Mat D2(1, 8, CV_64F, in_right.coeffs);
+  cv::Mat R =
+      (cv::Mat_<double>(3, 3) << ex_right_to_left.rotation[0][0],
+       ex_right_to_left.rotation[0][1], ex_right_to_left.rotation[0][2],
+       ex_right_to_left.rotation[1][0], ex_right_to_left.rotation[1][1],
+       ex_right_to_left.rotation[1][2], ex_right_to_left.rotation[2][0],
+       ex_right_to_left.rotation[2][1], ex_right_to_left.rotation[2][2]);
+  cv::Mat T(3, 1, CV_64F, ex_right_to_left.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/mynteye/api/processor/rectify_processor.h

@@ -50,6 +50,8 @@ class RectifyProcessor : public Processor {
  private:
   void InitParams(IntrinsicsPinhole in_left,
         IntrinsicsPinhole in_right, Extrinsics ex_right_to_left);
+  void InitParams(IntrinsicsEquidistant in_left,
+        IntrinsicsEquidistant in_right, Extrinsics ex_right_to_left);
 
   std::shared_ptr<Device> device_;
 };

src/mynteye/types.cc

@@ -198,14 +198,15 @@ std::ostream &operator<<(std::ostream &os, const IntrinsicsPinhole &in) {
     os << in.coeffs[i] << ", ";
   return os << in.coeffs[4] << "]";
 }
-
+/** The distortion coefficients k2,k3,k4,k5,mu,mv,u0,v0*/
+  double coeffs[8];
 std::ostream &operator<<(std::ostream &os, const IntrinsicsEquidistant &in) {
   os << "equidistant, " << FULL_PRECISION
      << "width: " << in.width << ", height: " << in.height
-     << ", k2: " << in.k2 << ", k3: " << in.k3
-     << ", k4: " << in.k4 << ", k5: " << in.k5
-     << ", mu: " << in.mu << ", mv: " << in.mv
-     << ", u0: " << in.u0 << ", v0: " << in.v0;
+     << ", k2: " << in.coeffs[0] << ", k3: " << in.coeffs[1]
+     << ", k4: " << in.coeffs[2] << ", k5: " << in.coeffs[3]
+     << ", mu: " << in.coeffs[4] << ", mv: " << in.coeffs[5]
+     << ", u0: " << in.coeffs[6] << ", v0: " << in.coeffs[7];
   return os;
 }