fix(*):fix Matrix-norm() & remove Eigen

include/Ctain/Matrix.h

@@ -190,7 +190,7 @@ namespace Ctain {
         Matrix<_Scalar> topRightCorner() const {
             Matrix<_Scalar> sub;
             sub = *this;
-            sub.setSub(_Rows-1-Rows, _Cols-1-Cols, Rows, Cols, data);
+            sub.setSub(0, _Cols-Cols, Rows, Cols, data);
             return sub;
         }
 
@@ -340,7 +340,7 @@ namespace Ctain {
         Matrix<_Scalar> res(_Rows, m._Cols);
         for(int i = 0; i < _Rows; i++) {
             for(int j = 0; j < m._Cols; j++) {
-                int sum = 0;
+                _Scalar sum = 0;
                 for(int k = 0; k < _Cols; k++) {
                     sum += cData(i, k) * m.cData(k, j);
                 }
@@ -383,7 +383,7 @@ namespace Ctain {
         double sum = 0;
         for(int i = 0; i < _Rows; i++) {
             for(int j = 0; j < _Cols; j++) {
-                sum += Matrix::cData(i, j);
+                sum += Matrix::cData(i, j) * Matrix::cData(i, j);
             }
         }
         sum = sqrt(sum);

src/mynteye/api/camera_models/camera.cc

@@ -97,34 +97,12 @@ const cv::Mat &Camera::mask(void) const {
   return m_mask;
 }
 
+
 void Camera::estimateExtrinsics(
     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;
-  
-     liftProjective(
-         Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P);
- 
-    P = 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);
@@ -134,7 +112,6 @@ void Camera::estimateExtrinsics2(
   // 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);
   }
@@ -145,19 +122,8 @@ void Camera::estimateExtrinsics2(
 }
 
 double Camera::reprojectionDist(
-    const Eigen::Vector3d &P1, const Eigen::Vector3d &P2) const {
+    const Ctain::Vector3d &P1, const Ctain::Vector3d &P2) const {
-  Eigen::Vector2d p1, p2;
+  Ctain::Vector2d p1(2, 1), p2(2, 1);
-
-  spaceToPlane(P1, p1);
-  spaceToPlane(P2, p2);
-
-  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);
@@ -204,25 +170,13 @@ double Camera::reprojectionError(
   return totalErr / pointsSoFar;
 }
 
-double Camera::reprojectionError(
-    const Eigen::Vector3d &P, const Eigen::Quaterniond &camera_q,
-    const Eigen::Vector3d &camera_t, const Eigen::Vector2d &observed_p) const {
-  Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t;
-
-  Eigen::Vector2d p;
-  spaceToPlane(P_cam, p);
-
-  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;
+  Ctain::Vector2d p(2, 1), res(2, 1);
   spaceToPlane(P_cam, p);
   res = p - observed_p;
   return res.norm();
@@ -238,41 +192,6 @@ void Camera::projectPoints(
   cv::Mat R0;
   cv::Rodrigues(rvec, R0);
 
-  Eigen::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);
-
-  Eigen::Vector3d t;
-  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
-    Eigen::Vector3d P;
-    P << objectPoint.x, objectPoint.y, objectPoint.z;
-
-    P = R * P + t;
-
-    Eigen::Vector2d p;
-    spaceToPlane(P, p);
-
-    imagePoints.push_back(cv::Point2f(p(0), p(1)));
-  }
-}
-
-// 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) <<
@@ -290,7 +209,7 @@ void Camera::projectPoints2(
 
     P = R * P + t;
 
-    Ctain::Vector2d p;
+    Ctain::Vector2d p(2, 1);
     spaceToPlane(P, p);
 
     imagePoints.push_back(cv::Point2f(p(0), p(1)));

src/mynteye/api/camera_models/camera.h

@@ -16,7 +16,6 @@
 
 #include <vector>
 #include <memory>
-#include "eigen3/Eigen/Dense"
 #include "Ctain/CtainBase.h"
 #include <opencv2/core/core.hpp>
 
@@ -28,12 +27,10 @@ namespace models {
 
 class Camera {
  public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   enum ModelType { KANNALA_BRANDT, MEI, PINHOLE, SCARAMUZZA };
 
   class Parameters {
    public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     explicit Parameters(ModelType modelType);
 
     Parameters(
@@ -72,39 +69,16 @@ class Camera {
       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
@@ -139,11 +113,7 @@ class Camera {
    * \return euclidean distance in the plane
    */
   double reprojectionDist(
-      const Eigen::Vector3d &P1, const Eigen::Vector3d &P2) const;
+      const Ctain::Vector3d &P1, const Ctain::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,
@@ -151,26 +121,13 @@ class Camera {
       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
+      const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const; 
-
-//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

@@ -270,102 +270,6 @@ void EquidistantCamera::estimateIntrinsics(
   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);
-    int arrayLength = boardSize.height;
-    double *radius = new double[arrayLength];
-    memset(radius, 0, arrayLength * sizeof(double));
-    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);
-}
-
-// 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
@@ -376,7 +280,7 @@ void EquidistantCamera::estimateIntrinsics2(
   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);
+    std::vector<Ctain::Vector2d> center(boardSize.height, Ctain::Vector2d(2, 1));
     int arrayLength = boardSize.height;
     double *radius = new double[arrayLength];
     memset(radius, 0, arrayLength * sizeof(double));
@@ -446,22 +350,7 @@ void EquidistantCamera::estimateIntrinsics2(
  * \param p image coordinates
  * \param P coordinates of the projective ray
  */
-void EquidistantCamera::liftProjective(
-    const Eigen::Vector2d &p, Eigen::Vector3d &P) const {
-  // Lift points to normalised plane
-  Eigen::Vector2d p_u;
-  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);
-}
-
-// subEigen
 void EquidistantCamera::liftProjective(
     const Ctain::Vector2d &p, Ctain::Vector3d &P) const {
   // Lift points to normalised plane
@@ -483,28 +372,7 @@ void EquidistantCamera::liftProjective(
  * \param P 3D point coordinates
  * \param p return value, contains the image point coordinates
  */
-void EquidistantCamera::spaceToPlane(
-    const Eigen::Vector3d &P, Eigen::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));
 
-  Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(),
-                          mParameters.k5(), theta) *
-                        Eigen::Vector2d(cos(phi), sin(phi));
-
-  // Apply generalised projection matrix
-  p << mParameters.mu() * p_u(0) + mParameters.u0(),
-      mParameters.mv() * p_u(1) + mParameters.v0();
-}
-
-// subEigen
 void EquidistantCamera::spaceToPlane(
     const Ctain::Vector3d &P, Ctain::Vector2d &p) const {
 // double theta = acos(0.5);
@@ -535,22 +403,6 @@ void EquidistantCamera::spaceToPlane(
  * \param P 3D point coordinates
  * \param p return value, contains the image point coordinates
  */
-void EquidistantCamera::spaceToPlane(
-    const Eigen::Vector3d &P, Eigen::Vector2d &p,
-    Eigen::Matrix<double, 2, 3> &J) const {
-  double theta = acos(P(2) / P.norm());
-  double phi = atan2(P(1), P(0));
-
-  Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(),
-                          mParameters.k5(), theta) *
-                        Eigen::Vector2d(cos(phi), sin(phi));
-
-  // Apply generalised projection matrix
-  p << mParameters.mu() * p_u(0) + mParameters.u0(),
-      mParameters.mv() * p_u(1) + mParameters.v0();
-}
-
-// subEigen
 void EquidistantCamera::spaceToPlane(
     const Ctain::Vector3d &P, Ctain::Vector2d &p,
     Ctain::Matrix23d &J) const {
@@ -569,7 +421,6 @@ void EquidistantCamera::spaceToPlane(
 void EquidistantCamera::initUndistortMap(
     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);
 
@@ -579,37 +430,9 @@ void EquidistantCamera::initUndistortMap(
       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::Vector2d tmp(2, 1);
-
+      tmp << mx_u << my_u;
-      Eigen::Vector3d P;
+      backprojectSymmetric(tmp, theta, phi);
-      P << sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta);
-
-      Eigen::Vector2d p;
-      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);
-}
-
-// 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);
@@ -625,86 +448,44 @@ void EquidistantCamera::initUndistortMap2(
   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 {
+      std::cout <<"w0";
   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);
+  std::cout << std::endl<<"w1";
-  cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);
-
-  Eigen::Matrix3f K_rect;
-
-  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();
-  }
-
-  Eigen::Matrix3f K_rect_inv = K_rect.inverse();
-
-  Eigen::Matrix3f R, R_inv;
-  cv::cv2eigen(rmat, R);
-  R_inv = R.inverse();
-
-  for (int v = 0; v < imageSize.height; ++v) {
-    for (int u = 0; u < imageSize.width; ++u) {
-      Eigen::Vector3f xo;
-      xo << u, v, 1;
-
-      Eigen::Vector3f uo = R_inv * K_rect_inv * xo;
-
-      Eigen::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;
-  cv::eigen2cv(K_rect, K_rect_cv);
-  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);
 
+  std::cout << std::endl <<"w2";
+  
   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;
   }
 
+  std::cout <<std::endl<<"w3";
+  
   if (fx == -1.0f || fy == -1.0f) {
     K_rect(0, 0) = mParameters.mu();
     K_rect(1, 1) = mParameters.mv();
   }
 
+
+  std::cout <<std::endl<<"w4";
   Ctain::Matrix3f K_rect_inv = K_rect.inverse();
 
   Ctain::Matrix3f R(3), R_inv(3);
   
+  std::cout <<std::endl<<"w5";
   for(int i = 0; i < 3; ++i) {
     for(int j = 0; j < 3; ++j) {
       R(i, j) = rmat.at<float>(i, j);
@@ -712,14 +493,15 @@ cv::Mat EquidistantCamera::initUndistortRectifyMap2(
   }
   R_inv = R.inverse();
 
+    std::cout<<"R:\n"<< R << std::endl;
+  std::cout <<"w6";
   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(2, 1);
-      Ctain::Vector2d p;
       spaceToPlane(uo.cast<double>(), p);
 
       mapX.at<float>(v, u) = p(0);
@@ -727,14 +509,17 @@ cv::Mat EquidistantCamera::initUndistortRectifyMap2(
     }
   }
 
+  std::cout <<"w7";
   cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
-
+  //std::cout <<"mapX:\n"<<mapY;
   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);
     }
   }
+
+  std::cout <<"K_rect_cv:\n"<<K_rect_cv;
   return K_rect_cv;
 }
 
@@ -806,32 +591,6 @@ void EquidistantCamera::fitOddPoly(
     pows.push_back(i);
   }
 
-  Eigen::MatrixXd X(x.size(), pows.size());
-  Eigen::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);
-  }
-
-  Eigen::MatrixXd A = (X.transpose() * X).inverse() * X.transpose() * Y;
-
-  coeffs.resize(A.rows());
-  for (int i = 0; i < A.rows(); ++i) {
-    coeffs.at(i) = A(i, 0);
-  }
-}
-
-// 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) {
@@ -850,88 +609,6 @@ void EquidistantCamera::fitOddPoly2(
   }
 }
 
-void EquidistantCamera::backprojectSymmetric(
-    const Eigen::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;
-  }
-
-  Eigen::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.
-    Eigen::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);
-
-    Eigen::EigenSolver<Eigen::MatrixXd> es(A);
-    Eigen::MatrixXcd eigval = es.eigenvalues();
-
-    std::vector<double> thetas;
-    for (int i = 0; i < eigval.rows(); ++i) {
-      if (fabs(eigval(i).imag()) > tol) {
-        continue;
-      }
-
-      double t = eigval(i).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());
-    }
-  }
-}
-
-// subEigen
 void EquidistantCamera::backprojectSymmetric(
     const Ctain::Vector2d &p_u, double &theta, double &phi) const {
   double tol = 1e-10;

src/mynteye/api/camera_models/equidistant_camera.h

@@ -115,53 +115,23 @@ 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
+      Ctain::Matrix23d &J) const;  
   // %output p
   // %output J
 
-  template <typename T>
-  static void spaceToPlane(
-      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,
@@ -175,15 +145,6 @@ void spaceToPlane(
       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;
@@ -199,18 +160,9 @@ void spaceToPlane(
   static T r(T k2, T k3, T k4, T k5, T theta);
 
   void fitOddPoly(
-      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
 
@@ -238,48 +190,6 @@ T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) {
 //             theta;
 }
 
-template <typename T>
-void EquidistantCamera::spaceToPlane(
-    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) {
-  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]);
-
-  Eigen::Matrix<T, 2, 1> p_u =
-      r(k2, k3, k4, k5, theta) * Eigen::Matrix<T, 2, 1>(cos(phi), sin(phi));
-
-  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,

src/mynteye/api/processor/rectify_processor.cc

@@ -242,6 +242,7 @@ Ctain::Matrix4d RectifyProcessor::loadT(const mynteye::Extrinsics &in) {
   Ctain::Quaterniond q(R);
   q.normalize();
   Ctain::Matrix4d T(4);
+  T(3, 3) = 1;
   T.topLeftCorner<3, 3>() = q.toRotationMatrix();
   Ctain::Vector3d t(3, 1);
   t << t_x << t_y << t_z;
@@ -290,6 +291,7 @@ std::shared_ptr<struct CameraROSMsgInfoPair> RectifyProcessor::stereoRectify(
   Ctain::Matrix4d T = loadT(ex_right_to_left);
   Ctain::Matrix3d R = T.topLeftCorner<3, 3>();
   Ctain::Vector3d t = T.topRightCorner<3, 1>();
+  std::cout<<"T:"<<T;
   // cv::Mat cv_R, cv_t;
   // cv::eigen2cv(R, cv_R);
   cv::Mat cv_R(3, 3, CV_64FC1);
@@ -326,16 +328,16 @@ std::shared_ptr<struct CameraROSMsgInfoPair> RectifyProcessor::stereoRectify(
       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;
+  std::cout << "K1: " << K1 << std::endl;
-  VLOG(2) << "D1: " << D1 << std::endl;
+  std::cout << "D1: " << D1 << std::endl;
-  VLOG(2) << "K2: " << K2 << std::endl;
+  std::cout << "K2: " << K2 << std::endl;
-  VLOG(2) << "D2: " << D2 << std::endl;
+  std::cout << "D2: " << D2 << std::endl;
-  VLOG(2) << "R: " << cv_R << std::endl;
+  std::cout << "R: " << cv_R << std::endl;
-  VLOG(2) << "t: " << cv_t << std::endl;
+  std::cout << "t: " << cv_t << std::endl;
-  VLOG(2) << "R1: " << R1 << std::endl;
+  std::cout << "R1: " << R1 << std::endl;
-  VLOG(2) << "R2: " << R2 << std::endl;
+  std::cout << "R2: " << R2 << std::endl;
-  VLOG(2) << "P1: " << P1 << std::endl;
+  std::cout << "P1: " << P1 << std::endl;
-  VLOG(2) << "P2: " << P2 << std::endl;
+  std::cout << "P2: " << P2 << std::endl;
 
   R1 = rectifyrad(R1);
   R2 = rectifyrad(R2);
@@ -397,21 +399,31 @@ void RectifyProcessor::InitParams(
     IntrinsicsEquidistant in_left,
     IntrinsicsEquidistant in_right,
     Extrinsics ex_right_to_left) {
+      LOG(INFO) <<"q1";
   calib_model = CalibrationModel::KANNALA_BRANDT;
   in_left.ResizeIntrinsics();
   in_right.ResizeIntrinsics();
   in_left_cur = in_left;
   in_right_cur = in_right;
   ex_right_to_left_cur = ex_right_to_left;
+    
+  LOG(INFO) <<"q2";
+  
   models::CameraPtr camera_odo_ptr_left =
       generateCameraFromIntrinsicsEquidistant(in_left);
   models::CameraPtr camera_odo_ptr_right =
       generateCameraFromIntrinsicsEquidistant(in_right);
+         
+  LOG(INFO) <<"q3";
+  
   auto calib_info_tmp = stereoRectify(camera_odo_ptr_left,
         camera_odo_ptr_right,
         in_left,
         in_right,
         ex_right_to_left);
+           
+  LOG(INFO) <<"q4";
+  
   *calib_infos = *calib_info_tmp;
   cv::Mat rect_R_l =
       cv::Mat::eye(3, 3, CV_32F), rect_R_r = cv::Mat::eye(3, 3, CV_32F);
@@ -421,6 +433,9 @@ void RectifyProcessor::InitParams(
       rect_R_r.at<float>(i, j) = calib_infos->right.R[i*3+j];
     }
   }
+     
+  LOG(INFO) <<"q5";
+  
   double left_f[] =
       {calib_infos->left.P[0], calib_infos->left.P[5]};
   double left_center[] =
@@ -429,6 +444,11 @@ void RectifyProcessor::InitParams(
       {calib_infos->right.P[0], calib_infos->right.P[5]};
   double right_center[] =
       {calib_infos->right.P[2], calib_infos->right.P[6]};
+         
+  LOG(INFO) <<"q6";
+  std::cout << "rectR:" << rect_R_l;
+  LOG(INFO) <<"q9\n";
+  std::cout << "left_center:"<<left_center[0]<<","<<left_center[1]<<std::endl;
   camera_odo_ptr_left->initUndistortRectifyMap(
       map11, map12, left_f[0], left_f[1],
       cv::Size(0, 0), left_center[0],
@@ -437,6 +457,18 @@ void RectifyProcessor::InitParams(
       map21, map22, right_f[0], right_f[1],
       cv::Size(0, 0), right_center[0],
       right_center[1], rect_R_r);
+//  cv::Mat a;
+//  cv::Mat b;
+
+  // camera_odo_ptr_left->initUndistortRectifyMap(
+  //     rect_R_l, rect_R_l,0, 0);
+  // camera_odo_ptr_right->initUndistortRectifyMap(
+  //     map21, map22, right_f[0], right_f[1],
+  //     cv::Size(0, 0), right_center[0],
+  //     right_center[1], rect_R_r);
+         
+  LOG(INFO) <<"q7";
+  
 }
 
 const char RectifyProcessor::NAME[] = "RectifyProcessor";
@@ -449,11 +481,20 @@ RectifyProcessor::RectifyProcessor(
     : Processor(std::move(proc_period)),
       calib_model(CalibrationModel::UNKNOW),
       _alpha(-1) {
+          
+  LOG(INFO) <<"Q";
+  
   calib_infos = std::make_shared<struct CameraROSMsgInfoPair>();
+    
+  LOG(INFO) <<"W";
+  
   InitParams(
     *std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_left),
     *std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_right),
     *extr);
+      
+  LOG(INFO) <<"E";
+  
 }
 
 RectifyProcessor::~RectifyProcessor() {

src/mynteye/api/processor/rectify_processor.h

@@ -27,7 +27,6 @@
 #include "equidistant_camera.h"
 
 #include <opencv2/core/core.hpp>
-#include <opencv2/core/eigen.hpp>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/highgui/highgui.hpp>