refactor(src/api/camera_models):add independent Matrix functions to camera_models

src/mynteye/api/camera_models/camera.cc

@@ -103,11 +103,37 @@ void Camera::estimateExtrinsics(
     cv::Mat &tvec) const {
   std::vector<cv::Point2f> Ms(imagePoints.size());
   for (size_t i = 0; i < Ms.size(); ++i) {
-    Eigen::Vector3d P;
-    liftProjective(
-        Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P);
+  Eigen::Vector3d P;
+  
+     liftProjective(
+         Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P);
+ 
+    P = P / P(2);
 
-    P /= P(2);
+    Ms.at(i).x = P(0);
+    Ms.at(i).y = P(1);
+  }
+
+  // assume unit focal length, zero principal point, and zero distortion
+  cv::solvePnP(
+      objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec);
+}
+
+//subEigen
+void Camera::estimateExtrinsics2(
+    const std::vector<cv::Point3f> &objectPoints,
+    const std::vector<cv::Point2f> &imagePoints, cv::Mat &rvec,
+    cv::Mat &tvec) const {
+  std::vector<cv::Point2f> Ms(imagePoints.size());
+  for (size_t i = 0; i < Ms.size(); ++i) {
+   // Eigen::Vector3d P;
+   Ctain::Vectord P(3, 1), p(2, 1);
+   p<< imagePoints.at(i).x << imagePoints.at(i).y;
+
+    // liftProjective(
+        // Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P);
+    liftProjective(p, P);
+    P = P / P(2);
 
     Ms.at(i).x = P(0);
     Ms.at(i).y = P(1);
@@ -128,6 +154,17 @@ double Camera::reprojectionDist(
   return (p1 - p2).norm();
 }
 
+//subEigen
+ double Camera::reprojectionDist(
+      const Ctain::Vectord &P1, const Ctain::Vectord &P2) const {
+  Ctain::Vectord p1(2, 1), p2(2, 1);
+
+  spaceToPlane(P1, p1);
+  spaceToPlane(P2, p2);
+
+  return (p1 - p2).norm();
+}
+
 double Camera::reprojectionError(
     const std::vector<std::vector<cv::Point3f> > &objectPoints,
     const std::vector<std::vector<cv::Point2f> > &imagePoints,
@@ -178,6 +215,19 @@ double Camera::reprojectionError(
   return (p - observed_p).norm();
 }
 
+//subEigen
+double Camera::reprojectionError(
+      const Ctain::Vector3d &P, const Ctain::Quaterniond &camera_q,
+      const Ctain::Vector3d &camera_t, const Ctain::Vector2d &observed_p) const {
+  Ctain::Vector3d P_cam;
+  P_cam = camera_q.toRotationMatrix() * P + camera_t;
+
+  Ctain::Vector2d p, res;
+  spaceToPlane(P_cam, p);
+  res = p - observed_p;
+  return res.norm();
+}
+
 void Camera::projectPoints(
     const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec,
     const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const {
@@ -212,6 +262,41 @@ void Camera::projectPoints(
   }
 }
 
+//subEigen
+void Camera::projectPoints2(
+    const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec,
+    const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const {
+  // project 3D object points to the image plane
+  imagePoints.reserve(objectPoints.size());
+
+  // double
+  cv::Mat R0;
+  cv::Rodrigues(rvec, R0);
+
+  Ctain::MatrixXd R(3, 3);
+  R << R0.at<double>(0, 0) << R0.at<double>(0, 1) << R0.at<double>(0, 2) <<
+       R0.at<double>(1, 0) << R0.at<double>(1, 1) << R0.at<double>(1, 2) <<
+       R0.at<double>(2, 0) << R0.at<double>(2, 1) << R0.at<double>(2, 2);
+
+  Ctain::Vectord t(3, 1);
+  t << tvec.at<double>(0) << tvec.at<double>(1) << tvec.at<double>(2);
+
+  for (size_t i = 0; i < objectPoints.size(); ++i) {
+    const cv::Point3f &objectPoint = objectPoints.at(i);
+
+    // Rotate and translate
+    Ctain::Vectord P(3, 1);
+    P << objectPoint.x << objectPoint.y << objectPoint.z;
+
+    P = R * P + t;
+
+    Ctain::Vector2d p;
+    spaceToPlane(P, p);
+
+    imagePoints.push_back(cv::Point2f(p(0), p(1)));
+  }
+}
+
 }  // namespace models
 
 MYNTEYE_END_NAMESPACE

src/mynteye/api/camera_models/camera.h

@@ -17,6 +17,7 @@
 #include <vector>
 #include <memory>
 #include "eigen3/Eigen/Dense"
+#include "Ctain/CtainBase.h"
 #include <opencv2/core/core.hpp>
 
 #include "mynteye/mynteye.h"
@@ -70,21 +71,45 @@ class Camera {
       const cv::Size &boardSize,
       const std::vector<std::vector<cv::Point3f> > &objectPoints,
       const std::vector<std::vector<cv::Point2f> > &imagePoints) = 0;
+
+// subEigen
+  virtual void estimateIntrinsics2(
+      const cv::Size &boardSize,
+      const std::vector<std::vector<cv::Point3f> > &objectPoints,
+      const std::vector<std::vector<cv::Point2f> > &imagePoints) = 0;
+
   virtual void estimateExtrinsics(
       const std::vector<cv::Point3f> &objectPoints,
       const std::vector<cv::Point2f> &imagePoints,
       cv::Mat &rvec, cv::Mat &tvec) const;  // NOLINT
 
+  virtual void estimateExtrinsics2(
+      const std::vector<cv::Point3f> &objectPoints,
+      const std::vector<cv::Point2f> &imagePoints,
+      cv::Mat &rvec, cv::Mat &tvec) const;  // NOLINT      
+
   // Lift points from the image plane to the projective space
   virtual void liftProjective(
       const Eigen::Vector2d &p, Eigen::Vector3d &P) const = 0;  // NOLINT
   // %output P
 
+//subEigen
+    // Lift points from the image plane to the projective space
+  virtual void liftProjective(
+      const Ctain::Vector2d &p, Ctain::Vector3d &P) const = 0;  // NOLINT
+  // %output P
+
   // Projects 3D points to the image plane (Pi function)
   virtual void spaceToPlane(
       const Eigen::Vector3d &P, Eigen::Vector2d &p) const = 0;  // NOLINT
   // %output p
 
+//subEigen
+  // Projects 3D points to the image plane (Pi function)
+  virtual void spaceToPlane(
+      const Ctain::Vector3d &P, Ctain::Vector2d &p) const = 0;  // NOLINT
+  // %output p
+
   // Projects 3D points to the image plane (Pi function)
   // and calculates jacobian
   // virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
@@ -116,20 +141,36 @@ class Camera {
   double reprojectionDist(
       const Eigen::Vector3d &P1, const Eigen::Vector3d &P2) const;
 
+//subEigen
+ double reprojectionDist(
+      const Ctain::Vector3d &P1, const Ctain::Vector2d &P2) const;
+
   double reprojectionError(
       const std::vector<std::vector<cv::Point3f> > &objectPoints,
       const std::vector<std::vector<cv::Point2f> > &imagePoints,
       const std::vector<cv::Mat> &rvecs, const std::vector<cv::Mat> &tvecs,
       cv::OutputArray perViewErrors = cv::noArray()) const;
 
+
   double reprojectionError(
       const Eigen::Vector3d &P, const Eigen::Quaterniond &camera_q,
       const Eigen::Vector3d &camera_t, const Eigen::Vector2d &observed_p) const;
 
+//subEigen
+double reprojectionError(
+      const Ctain::Vector3d &P, const Ctain::Quaterniond &camera_q,
+      const Ctain::Vector3d &camera_t, const Ctain::Vector2d &observed_p) const;
+
+
   void projectPoints(
       const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec,
       const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const;  // NOLINT
 
+//subEigen
+  void projectPoints2(
+      const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec,
+      const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const;  // NOLINT
+
  protected:
   cv::Mat m_mask;
 };

src/mynteye/api/camera_models/equidistant_camera.cc

@@ -342,6 +342,102 @@ void EquidistantCamera::estimateIntrinsics(
   setParameters(params);
 }
 
+// subEigen
+void EquidistantCamera::estimateIntrinsics2(
+    const cv::Size &boardSize,
+    const std::vector<std::vector<cv::Point3f> > &objectPoints,
+    const std::vector<std::vector<cv::Point2f> > &imagePoints) {
+  Parameters params = getParameters();
+
+  double u0 = params.imageWidth() / 2.0;
+  double v0 = params.imageHeight() / 2.0;
+
+  double minReprojErr = std::numeric_limits<double>::max();
+
+  std::vector<cv::Mat> rvecs, tvecs;
+  rvecs.assign(objectPoints.size(), cv::Mat());
+  tvecs.assign(objectPoints.size(), cv::Mat());
+
+  params.k2() = 0.0;
+  params.k3() = 0.0;
+  params.k4() = 0.0;
+  params.k5() = 0.0;
+  params.u0() = u0;
+  params.v0() = v0;
+
+  // Initialize focal length
+  // C. Hughes, P. Denny, M. Glavin, and E. Jones,
+  // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point
+  // Extraction, PAMI 2010
+  // Find circles from rows of chessboard corners, and for each pair
+  // of circles, find vanishing points: v1 and v2.
+  // f = ||v1 - v2|| / PI;
+  double f0 = 0.0;
+  for (size_t i = 0; i < imagePoints.size(); ++i) {
+    //std::vector<Eigen::Vector2d> center(boardSize.height);
+    std::vector<Ctain::Vector2d> center(boardSize.height);
+    int arrayLength = boardSize.height;
+    double *radius = new double[arrayLength];
+    for (int r = 0; r < boardSize.height; ++r) {
+      std::vector<cv::Point2d> circle;
+      for (int c = 0; c < boardSize.width; ++c) {
+        circle.push_back(imagePoints.at(i).at(r * boardSize.width + c));
+      }
+
+      fitCircle(circle, center[r](0), center[r](1), radius[r]);
+    }
+
+    for (int j = 0; j < boardSize.height; ++j) {
+      for (int k = j + 1; k < boardSize.height; ++k) {
+        // find distance between pair of vanishing points which
+        // correspond to intersection points of 2 circles
+        std::vector<cv::Point2d> ipts;
+        ipts = intersectCircles(
+            center[j](0), center[j](1), radius[j], center[k](0), center[k](1),
+            radius[k]);
+
+        if (ipts.size() < 2) {
+          continue;
+        }
+
+        double f = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI;
+
+        params.mu() = f;
+        params.mv() = f;
+
+        setParameters(params);
+
+        for (size_t l = 0; l < objectPoints.size(); ++l) {
+          estimateExtrinsics(
+              objectPoints.at(l), imagePoints.at(l), rvecs.at(l), tvecs.at(l));
+        }
+
+        double reprojErr = reprojectionError(
+            objectPoints, imagePoints, rvecs, tvecs, cv::noArray());
+
+        if (reprojErr < minReprojErr) {
+          minReprojErr = reprojErr;
+          f0 = f;
+        }
+      }
+    }
+    delete[] radius;
+  }
+
+  if (f0 <= 0.0 && minReprojErr >= std::numeric_limits<double>::max()) {
+    std::cout << "[" << params.cameraName() << "] "
+              << "# INFO: kannala-Brandt model fails with given data. "
+              << std::endl;
+
+    return;
+  }
+
+  params.mu() = f0;
+  params.mv() = f0;
+
+  setParameters(params);
+}
+
 /**
  * \brief Lifts a point from the image plane to its projective ray
  *
@@ -363,6 +459,22 @@ void EquidistantCamera::liftProjective(
   P(2) = cos(theta);
 }
 
+// subEigen
+void EquidistantCamera::liftProjective(
+    const Ctain::Vector2d &p, Ctain::Vector3d &P) const {
+  // Lift points to normalised plane
+  Ctain::Vector2d p_u(2, 1);
+  p_u << m_inv_K11 * p(0) + m_inv_K13 << m_inv_K22 * p(1) + m_inv_K23;
+
+  // Obtain a projective ray
+  double theta, phi;
+  backprojectSymmetric(p_u, theta, phi);
+
+  P(0) = sin(theta) * cos(phi);
+  P(1) = sin(theta) * sin(phi);
+  P(2) = cos(theta);
+}
+
 /**
  * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v)
  *
@@ -390,6 +502,31 @@ void EquidistantCamera::spaceToPlane(
       mParameters.mv() * p_u(1) + mParameters.v0();
 }
 
+// subEigen
+void EquidistantCamera::spaceToPlane(
+    const Ctain::Vector3d &P, Ctain::Vector2d &p) const {
+// double theta = acos(0.5);
+// double theta = 0.5;
+// double phi = 0.5;
+// Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(),
+//                         mParameters.k5(), theta) *
+// Eigen::Vector2d(cos(0.5), sin(0.5));
+
+
+  double theta = acos(P(2) / P.norm());
+  double phi = atan2(P(1), P(0));
+// double phi = ApproxAtan2(P(1), P(0));
+
+  double tmp[2] = {cos(phi), sin(phi)};
+  Ctain::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(),
+                          mParameters.k5(), theta) *
+                        Ctain::Vector2d(tmp, 2, 1);
+
+  // Apply generalised projection matrix
+  p << mParameters.mu() * p_u(0) + mParameters.u0()
+    << mParameters.mv() * p_u(1) + mParameters.v0();
+}
+
 /**
  * \brief Project a 3D point to the image plane and calculate Jacobian
  *
@@ -411,6 +548,22 @@ void EquidistantCamera::spaceToPlane(
       mParameters.mv() * p_u(1) + mParameters.v0();
 }
 
+// subEigen
+void EquidistantCamera::spaceToPlane(
+    const Ctain::Vector3d &P, Ctain::Vector2d &p,
+    Ctain::Matrix23d &J) const {
+  double theta = acos(P(2) / 3.0);
+  double phi = atan2(P(1), P(0));
+  double tmp[2] = {cos(phi), sin(phi)};
+  Ctain::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(),
+                          mParameters.k5(), theta) *
+                        Ctain::Vector2d(tmp, 2, 1);
+
+  // Apply generalised projection matrix
+  p << mParameters.mu() * p_u(0) + mParameters.u0()
+    << mParameters.mv() * p_u(1) + mParameters.v0();
+}
+ 
 void EquidistantCamera::initUndistortMap(
     cv::Mat &map1, cv::Mat &map2, double fScale) const {
   cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());
@@ -440,6 +593,36 @@ void EquidistantCamera::initUndistortMap(
   cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
 }
 
+// subEigen
+void EquidistantCamera::initUndistortMap2(
+    cv::Mat &map1, cv::Mat &map2, double fScale) const {
+  cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());
+
+  cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);
+  cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);
+
+  for (int v = 0; v < imageSize.height; ++v) {
+    for (int u = 0; u < imageSize.width; ++u) {
+      double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;
+      double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;
+
+      double theta, phi;
+      backprojectSymmetric(Eigen::Vector2d(mx_u, my_u), theta, phi);
+
+      Ctain::Vector3d P(3, 1);
+      P << sin(theta) * cos(phi) << sin(theta) * sin(phi) << cos(theta);
+
+      Ctain::Vector2d p(2, 1);
+      spaceToPlane(P, p);
+
+      mapX.at<float>(v, u) = p(0);
+      mapY.at<float>(v, u) = p(1);
+    }
+  }
+
+  cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
+}
+
 cv::Mat EquidistantCamera::initUndistortRectifyMap(
     cv::Mat &map1, cv::Mat &map2, float fx, float fy, cv::Size imageSize,
     float cx, float cy, cv::Mat rmat) const {
@@ -491,6 +674,68 @@ cv::Mat EquidistantCamera::initUndistortRectifyMap(
   return K_rect_cv;
 }
 
+
+// subEigen
+cv::Mat EquidistantCamera::initUndistortRectifyMap2(
+    cv::Mat &map1, cv::Mat &map2, float fx, float fy, cv::Size imageSize,
+    float cx, float cy, cv::Mat rmat) const {
+  if (imageSize == cv::Size(0, 0)) {
+    imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());
+  }
+
+  cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);
+  cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);
+
+  Ctain::Matrix3f K_rect(3);
+
+  if (cx == -1.0f && cy == -1.0f) {
+    K_rect << fx << 0 << imageSize.width / 2 << 0 << fy << imageSize.height / 2 << 0 << 0 << 1;
+  } else {
+    K_rect << fx << 0 << cx << 0 << fy << cy << 0 << 0 << 1;
+  }
+
+  if (fx == -1.0f || fy == -1.0f) {
+    K_rect(0, 0) = mParameters.mu();
+    K_rect(1, 1) = mParameters.mv();
+  }
+
+  Ctain::Matrix3f K_rect_inv = K_rect.inverse();
+
+  Ctain::Matrix3f R(3), R_inv(3);
+  
+  for(int i = 0; i < 3; ++i) {
+    for(int j = 0; j < 3; ++j) {
+      R(i, j) = rmat.at<float>(i, j);
+    }
+  }
+  R_inv = R.inverse();
+
+  for (int v = 0; v < imageSize.height; ++v) {
+    for (int u = 0; u < imageSize.width; ++u) {
+      Ctain::Vector3f xo(3, 1);
+      xo << u << v << 1;
+
+      Ctain::Vector3f uo = R_inv * K_rect_inv * xo;
+
+      Ctain::Vector2d p;
+      spaceToPlane(uo.cast<double>(), p);
+
+      mapX.at<float>(v, u) = p(0);
+      mapY.at<float>(v, u) = p(1);
+    }
+  }
+
+  cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
+
+  cv::Mat K_rect_cv(3, 3, CV_32FC1);
+  for(int i = 0; i < 3; ++i) {
+    for(int j = 0; j < 3; ++j) {
+      K_rect_cv.at<float>(i, j) = K_rect(i, j);
+    }
+  }
+  return K_rect_cv;
+}
+
 int EquidistantCamera::parameterCount(void) const {
   return 8;
 }
@@ -576,6 +821,33 @@ void EquidistantCamera::fitOddPoly(
   }
 }
 
+// subEigen
+void EquidistantCamera::fitOddPoly2(
+    const std::vector<double> &x, const std::vector<double> &y, int n,
+    std::vector<double> &coeffs) const {
+  std::vector<int> pows;
+  for (int i = 1; i <= n; i += 2) {
+    pows.push_back(i);
+  }
+
+  Ctain::MatrixXd X(x.size(), pows.size());
+  Ctain::MatrixXd Y(y.size(), 1);
+  for (size_t i = 0; i < x.size(); ++i) {
+    for (size_t j = 0; j < pows.size(); ++j) {
+      X(i, j) = pow(x.at(i), pows.at(j));
+    }
+    Y(i, 0) = y.at(i);
+  }
+  Ctain::SMatrix<double> Tmp;
+  Tmp = X.transpose() * X;
+  Ctain::MatrixXd A = Tmp.inverse() * X.transpose() * Y;
+
+  coeffs.resize(A.rows());
+  for (int i = 0; i < A.rows(); ++i) {
+    coeffs.at(i) = A(i, 0);
+  }
+}
+
 void EquidistantCamera::backprojectSymmetric(
     const Eigen::Vector2d &p_u, double &theta, double &phi) const {
   double tol = 1e-10;
@@ -657,6 +929,89 @@ void EquidistantCamera::backprojectSymmetric(
   }
 }
 
+// subEigen
+  void EquidistantCamera::backprojectSymmetric(
+      const Ctain::Vector2d &p_u, double &theta, double &phi) const {
+  double tol = 1e-10;
+  double p_u_norm = p_u.norm();
+
+  if (p_u_norm < 1e-10) {
+    phi = 0.0;
+  } else {
+    phi = atan2(p_u(1), p_u(0));
+  }
+
+  int npow = 9;
+  if (mParameters.k5() == 0.0) {
+    npow -= 2;
+  }
+  if (mParameters.k4() == 0.0) {
+    npow -= 2;
+  }
+  if (mParameters.k3() == 0.0) {
+    npow -= 2;
+  }
+  if (mParameters.k2() == 0.0) {
+    npow -= 2;
+  }
+
+  Ctain::MatrixXd coeffs(npow + 1, 1);
+  coeffs.setZero();
+  coeffs(0) = -p_u_norm;
+  coeffs(1) = 1.0;
+
+  if (npow >= 3) {
+    coeffs(3) = mParameters.k2();
+  }
+  if (npow >= 5) {
+    coeffs(5) = mParameters.k3();
+  }
+  if (npow >= 7) {
+    coeffs(7) = mParameters.k4();
+  }
+  if (npow >= 9) {
+    coeffs(9) = mParameters.k5();
+  }
+
+  if (npow == 1) {
+    theta = p_u_norm;
+  } else {
+    // Get eigenvalues of companion matrix corresponding to polynomial.
+    // Eigenvalues correspond to roots of polynomial.
+    Ctain::MatrixXd A(npow, npow);
+    A.setZero();
+    A.block(1, 0, npow - 1, npow - 1).setIdentity();
+    A.col(npow - 1) = -coeffs.block(0, 0, npow, 1) / coeffs(npow);
+
+    Ctain::EigenSolver es(A);
+    Ctain::MatrixXcd eigval = es.eigenvalues();
+
+    std::vector<double> thetas;
+    for (int i = 0; i < eigval.rows(); ++i) {
+      if (fabs(eigval(i, 1)) > tol) {   //imag
+        continue;
+      }
+
+      double t = eigval(i, 0);          //real
+
+      if (t < -tol) {
+        continue;
+      } else if (t < 0.0) {
+        t = 0.0;
+      }
+
+      thetas.push_back(t);
+    }
+
+    if (thetas.empty()) {
+      theta = p_u_norm;
+    } else {
+      theta = *std::min_element(thetas.begin(), thetas.end());
+    }
+  }
+
+}
+
 }  // namespace models
 
 MYNTEYE_END_NAMESPACE

src/mynteye/api/camera_models/equidistant_camera.h

@@ -115,20 +115,45 @@ class EquidistantCamera : public Camera {
       const std::vector<std::vector<cv::Point3f> > &objectPoints,
       const std::vector<std::vector<cv::Point2f> > &imagePoints);
 
+//subEigen
+  void estimateIntrinsics2(
+      const cv::Size &boardSize,
+      const std::vector<std::vector<cv::Point3f> > &objectPoints,
+      const std::vector<std::vector<cv::Point2f> > &imagePoints);
+
   // Lift points from the image plane to the projective space
   void liftProjective(const Eigen::Vector2d &p, Eigen::Vector3d &P) const;  // NOLINT
   // %output P
 
+  //subEigen
+  // Lift points from the image plane to the projective space
+  void liftProjective(const Ctain::Vector2d &p, Ctain::Vector3d &P) const;  // NOLINT
+  // %output P
+
   // Projects 3D points to the image plane (Pi function)
   void spaceToPlane(const Eigen::Vector3d &P, Eigen::Vector2d &p) const;  // NOLINT
   // %output p
 
+  //subEigen
+  // Projects 3D points to the image plane (Pi function)
+  void spaceToPlane(const Ctain::Vector3d &P, Ctain::Vector2d &p) const;  // NOLINT
+  // %output p
+
   // Projects 3D points to the image plane (Pi function)
   // and calculates jacobian
   void spaceToPlane(
       const Eigen::Vector3d &P, Eigen::Vector2d &p,  // NOLINT
       Eigen::Matrix<double, 2, 3> &J) const;  // NOLINT
   // %output p
+  // %output J
+
+    // subEigen
+  // Projects 3D points to the image plane (Pi function)
+  // and calculates jacobian
+void spaceToPlane(
+    const Ctain::Vector3d &P, Ctain::Vector2d &p,
+    Ctain::Matrix23d &J) const;  // NOLINT
+  // %output p
   // %output J
 
   template <typename T>
@@ -136,13 +161,29 @@ class EquidistantCamera : public Camera {
       const T *const params, const T *const q, const T *const t,
       const Eigen::Matrix<T, 3, 1> &P, Eigen::Matrix<T, 2, 1> &p);  // NOLINT
 
+// subEigen
+  template <typename T>
+  static void spaceToPlane(
+      const T *const params, const T *const q, const T *const t,
+      const Ctain::Matrix<T> &P, Ctain::Matrix<T> &p);  // NOLINT
+
   void initUndistortMap(
       cv::Mat &map1, cv::Mat &map2, double fScale = 1.0) const;  // NOLINT
+
   cv::Mat initUndistortRectifyMap(
       cv::Mat &map1, cv::Mat &map2, float fx = -1.0f, float fy = -1.0f,  // NOLINT
       cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f,
       cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;
 
+// subEigen
+  void initUndistortMap2(
+      cv::Mat &map1, cv::Mat &map2, double fScale = 1.0) const;  // NOLINT
+
+  cv::Mat initUndistortRectifyMap2(
+      cv::Mat &map1, cv::Mat &map2, float fx = -1.0f, float fy = -1.0f,  // NOLINT
+      cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f,
+      cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;
+
   int parameterCount(void) const;
 
   const Parameters &getParameters(void) const;
@@ -161,9 +202,18 @@ class EquidistantCamera : public Camera {
       const std::vector<double> &x, const std::vector<double> &y, int n,
       std::vector<double> &coeffs) const;  // NOLINT
 
+// subEigen
+  void fitOddPoly2(
+      const std::vector<double> &x, const std::vector<double> &y, int n,
+      std::vector<double> &coeffs) const; 
+
   void backprojectSymmetric(
       const Eigen::Vector2d &p_u, double &theta, double &phi) const;  // NOLINT
 
+// subEigen
+  void backprojectSymmetric(
+      const Ctain::Vector2d &p_u, double &theta, double &phi) const;  // NOLINT
+
   Parameters mParameters;
 
   double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
@@ -228,8 +278,56 @@ void EquidistantCamera::spaceToPlane(
   p(0) = mu * p_u(0) + u0;
   p(1) = mv * p_u(1) + v0;
 }
+
+// subEigen
+template <typename T>
+void spaceToPlane(
+      const T *const params, const T *const q, const T *const t,
+      const Ctain::Matrix<T> &P, Ctain::Matrix<T> &p) {
+  T P_w[3];
+  P_w[0] = T(P(0));
+  P_w[1] = T(P(1));
+  P_w[2] = T(P(2));
+
+  // Convert quaternion from Eigen convention (x, y, z, w)
+  // to Ceres convention (w, x, y, z)
+  T q_ceres[4] = {q[3], q[0], q[1], q[2]};
+
+  T P_c[3];
+  QuaternionRotatePoint(q_ceres, P_w, P_c);
+
+  P_c[0] += t[0];
+  P_c[1] += t[1];
+  P_c[2] += t[2];
+
+  // project 3D object point to the image plane;
+  T k2 = params[0];
+  T k3 = params[1];
+  T k4 = params[2];
+  T k5 = params[3];
+  T mu = params[4];
+  T mv = params[5];
+  T u0 = params[6];
+  T v0 = params[7];
+
+  T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]);
+  T theta = acos(P_c[2] / len);
+  T phi = atan2(P_c[1], P_c[0]);
+
+  Ctain::Matrix<T> p_u(2, 1), tmp(2, 1);
+  tmp(0) = cos(phi);
+  tmp(1) = sin(phi);
+  p_u = r(k2, k3, k4, k5, theta) * tmp;
+
+  p(0) = mu * p_u(0) + u0;
+  p(1) = mv * p_u(1) + v0;
 }
 
+}
+
+
+
+
 MYNTEYE_END_NAMESPACE
 
 #endif  // MYNTEYE_CAMERA_MODELS_EQUIDISTANT_CAMERA_H_