feat(camodoal): add camodoal to api layer

CMakeLists.txt

@@ -106,6 +106,7 @@ set_outdir(
 )
 
 ## main
+
 if(WITH_GLOG)
   add_executable(main src/main.cc)
   target_link_libraries(main glog::glog)
@@ -115,6 +116,42 @@ if(WITH_GLOG)
   )
 endif()
 
+## camodocal
+
+if(WITH_CAM_MODELS)
+  set(EIGEN_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/eigen3)
+
+  LIST(APPEND CMAKE_PREFIX_PATH ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/ceres/share/Ceres)
+  find_package(Ceres REQUIRED)
+
+  message(STATUS "CERES_INCLUDE_DIRS: ${CERES_INCLUDE_DIRS}")
+  message(STATUS "CERES_LIBRARIES: ${CERES_LIBRARIES}")
+  message(STATUS "EIGEN_INCLUDE_DIRS: ${EIGEN_INCLUDE_DIRS}")
+
+  include_directories(
+    ${catkin_INCLUDE_DIRS}
+    ${EIGEN_INCLUDE_DIRS}
+    src/mynteye/api/camodocal/include
+  )
+
+  add_library(camodocal STATIC
+    src/mynteye/api/camodocal/src/chessboard/Chessboard.cc
+    src/mynteye/api/camodocal/src/calib/CameraCalibration.cc
+    src/mynteye/api/camodocal/src/calib/StereoCameraCalibration.cc
+    src/mynteye/api/camodocal/src/camera_models/Camera.cc
+    src/mynteye/api/camodocal/src/camera_models/CameraFactory.cc
+    src/mynteye/api/camodocal/src/camera_models/CostFunctionFactory.cc
+    src/mynteye/api/camodocal/src/camera_models/PinholeCamera.cc
+    src/mynteye/api/camodocal/src/camera_models/CataCamera.cc
+    src/mynteye/api/camodocal/src/camera_models/EquidistantCamera.cc
+    src/mynteye/api/camodocal/src/camera_models/ScaramuzzaCamera.cc
+    src/mynteye/api/camodocal/src/sparse_graph/Transform.cc
+    src/mynteye/api/camodocal/src/gpl/gpl.cc
+    src/mynteye/api/camodocal/src/gpl/EigenQuaternionParameterization.cc
+  )
+  target_link_libraries(camodocal ${CERES_LIBRARIES})
+endif()
+
 ## libmynteye
 
 if(NOT WITH_GLOG AND NOT OS_WIN)
@@ -210,6 +247,7 @@ endif()
 if(WITH_GLOG)
   list(APPEND MYNTEYE_LINKLIBS glog::glog)
 endif()
+
 #message(STATUS "MYNTEYE_LINKLIBS: ${MYNTEYE_LINKLIBS}")
 
 add_library(${MYNTEYE_NAME} SHARED ${MYNTEYE_SRCS})

cmake/Option.cmake

@@ -23,7 +23,7 @@ include(${CMAKE_CURRENT_LIST_DIR}/Utils.cmake)
 option(WITH_API "Build with API layer, need OpenCV" ON)
 option(WITH_DEVICE_INFO_REQUIRED "Build with device info required" ON)
 
-option(WITH_CAM_MODELS "Build with more camera models" OFF)
+option(WITH_CAM_MODELS "Build with more camera models, WITH_API must be ON" OFF)
 
 # 3rdparty components
 
@@ -37,6 +37,9 @@ option(WITH_GLOG "Include glog support" OFF)
 
 if(WITH_API)
   include(${CMAKE_CURRENT_LIST_DIR}/DetectOpenCV.cmake)
+else()
+  # Disable WITH_CAM_MODELS if WITH_API is OFF
+  set(WITH_CAM_MODELS OFF)
 endif()
 
 if(WITH_DEVICE_INFO_REQUIRED)

src/mynteye/api/camodocal/include/camodocal/EigenUtils.h

@@ -0,0 +1,399 @@
+#ifndef EIGENUTILS_H
+#define EIGENUTILS_H
+
+#include "Eigen/Dense"
+#include <iostream>
+
+#include "ceres/rotation.h"
+
+namespace camodocal {
+
+template <typename T>
+T square(const T &m) {
+  return m * m;
+}
+
+// Returns the 3D cross product skew symmetric matrix of a given 3D vector
+template <typename T>
+Eigen::Matrix<T, 3, 3> skew(const Eigen::Matrix<T, 3, 1> &vec) {
+  return (Eigen::Matrix<T, 3, 3>() << T(0), -vec(2), vec(1), vec(2), T(0),
+          -vec(0), -vec(1), vec(0), T(0))
+      .finished();
+}
+
+template <typename Derived>
+typename Eigen::MatrixBase<Derived>::PlainObject sqrtm(
+    const Eigen::MatrixBase<Derived> &A) {
+  Eigen::SelfAdjointEigenSolver<typename Derived::PlainObject> es(A);
+
+  return es.operatorSqrt();
+}
+
+template <typename T>
+Eigen::Matrix<T, 3, 3> AngleAxisToRotationMatrix(
+    const Eigen::Matrix<T, 3, 1> &rvec) {
+  T angle = rvec.norm();
+  if (angle == T(0)) {
+    return Eigen::Matrix<T, 3, 3>::Identity();
+  }
+
+  Eigen::Matrix<T, 3, 1> axis;
+  axis = rvec.normalized();
+
+  Eigen::Matrix<T, 3, 3> rmat;
+  rmat = Eigen::AngleAxis<T>(angle, axis);
+
+  return rmat;
+}
+
+template <typename T>
+Eigen::Quaternion<T> AngleAxisToQuaternion(const Eigen::Matrix<T, 3, 1> &rvec) {
+  Eigen::Matrix<T, 3, 3> rmat = AngleAxisToRotationMatrix<T>(rvec);
+
+  return Eigen::Quaternion<T>(rmat);
+}
+
+template <typename T>
+void AngleAxisToQuaternion(const Eigen::Matrix<T, 3, 1> &rvec, T *q) {
+  Eigen::Quaternion<T> quat = AngleAxisToQuaternion<T>(rvec);
+
+  q[0] = quat.x();
+  q[1] = quat.y();
+  q[2] = quat.z();
+  q[3] = quat.w();
+}
+
+template <typename T>
+Eigen::Matrix<T, 3, 1> RotationToAngleAxis(const Eigen::Matrix<T, 3, 3> &rmat) {
+  Eigen::AngleAxis<T> angleaxis;
+  angleaxis.fromRotationMatrix(rmat);
+  return angleaxis.angle() * angleaxis.axis();
+}
+
+template <typename T>
+void QuaternionToAngleAxis(const T *const q, Eigen::Matrix<T, 3, 1> &rvec) {
+  Eigen::Quaternion<T> quat(q[3], q[0], q[1], q[2]);
+
+  Eigen::Matrix<T, 3, 3> rmat = quat.toRotationMatrix();
+
+  Eigen::AngleAxis<T> angleaxis;
+  angleaxis.fromRotationMatrix(rmat);
+
+  rvec = angleaxis.angle() * angleaxis.axis();
+}
+
+template <typename T>
+Eigen::Matrix<T, 3, 3> QuaternionToRotation(const T *const q) {
+  T R[9];
+  ceres::QuaternionToRotation(q, R);
+
+  Eigen::Matrix<T, 3, 3> rmat;
+  for (int i = 0; i < 3; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      rmat(i, j) = R[i * 3 + j];
+    }
+  }
+
+  return rmat;
+}
+
+template <typename T>
+void QuaternionToRotation(const T *const q, T *rot) {
+  ceres::QuaternionToRotation(q, rot);
+}
+
+template <typename T>
+Eigen::Matrix<T, 4, 4> QuaternionMultMatLeft(const Eigen::Quaternion<T> &q) {
+  return (Eigen::Matrix<T, 4, 4>() << q.w(), -q.z(), q.y(), q.x(), q.z(), q.w(),
+          -q.x(), q.y(), -q.y(), q.x(), q.w(), q.z(), -q.x(), -q.y(), -q.z(),
+          q.w())
+      .finished();
+}
+
+template <typename T>
+Eigen::Matrix<T, 4, 4> QuaternionMultMatRight(const Eigen::Quaternion<T> &q) {
+  return (Eigen::Matrix<T, 4, 4>() << q.w(), q.z(), -q.y(), q.x(), -q.z(),
+          q.w(), q.x(), q.y(), q.y(), -q.x(), q.w(), q.z(), -q.x(), -q.y(),
+          -q.z(), q.w())
+      .finished();
+}
+
+/// @param theta - rotation about screw axis
+/// @param d - projection of tvec on the rotation axis
+/// @param l - screw axis direction
+/// @param m - screw axis moment
+template <typename T>
+void AngleAxisAndTranslationToScrew(
+    const Eigen::Matrix<T, 3, 1> &rvec, const Eigen::Matrix<T, 3, 1> &tvec,
+    T &theta, T &d, Eigen::Matrix<T, 3, 1> &l, Eigen::Matrix<T, 3, 1> &m) {
+  theta = rvec.norm();
+  if (theta == 0) {
+    l.setZero();
+    m.setZero();
+    std::cout << "Warning: Undefined screw! Returned 0. " << std::endl;
+  }
+
+  l = rvec.normalized();
+
+  Eigen::Matrix<T, 3, 1> t = tvec;
+
+  d = t.transpose() * l;
+
+  // point on screw axis - projection of origin on screw axis
+  Eigen::Matrix<T, 3, 1> c;
+  c = 0.5 * (t - d * l + (1.0 / tan(theta / 2.0) * l).cross(t));
+
+  // c and hence the screw axis is not defined if theta is either 0 or M_PI
+  m = c.cross(l);
+}
+
+template <typename T>
+Eigen::Matrix<T, 3, 3> RPY2mat(T roll, T pitch, T yaw) {
+  Eigen::Matrix<T, 3, 3> m;
+
+  T cr = cos(roll);
+  T sr = sin(roll);
+  T cp = cos(pitch);
+  T sp = sin(pitch);
+  T cy = cos(yaw);
+  T sy = sin(yaw);
+
+  m(0, 0) = cy * cp;
+  m(0, 1) = cy * sp * sr - sy * cr;
+  m(0, 2) = cy * sp * cr + sy * sr;
+  m(1, 0) = sy * cp;
+  m(1, 1) = sy * sp * sr + cy * cr;
+  m(1, 2) = sy * sp * cr - cy * sr;
+  m(2, 0) = -sp;
+  m(2, 1) = cp * sr;
+  m(2, 2) = cp * cr;
+  return m;
+}
+
+template <typename T>
+void mat2RPY(const Eigen::Matrix<T, 3, 3> &m, T &roll, T &pitch, T &yaw) {
+  roll = atan2(m(2, 1), m(2, 2));
+  pitch = atan2(-m(2, 0), sqrt(m(2, 1) * m(2, 1) + m(2, 2) * m(2, 2)));
+  yaw = atan2(m(1, 0), m(0, 0));
+}
+
+template <typename T>
+Eigen::Matrix<T, 4, 4> homogeneousTransform(
+    const Eigen::Matrix<T, 3, 3> &R, const Eigen::Matrix<T, 3, 1> &t) {
+  Eigen::Matrix<T, 4, 4> H;
+  H.setIdentity();
+
+  H.block(0, 0, 3, 3) = R;
+  H.block(0, 3, 3, 1) = t;
+
+  return H;
+}
+
+template <typename T>
+Eigen::Matrix<T, 4, 4> poseWithCartesianTranslation(
+    const T *const q, const T *const p) {
+  Eigen::Matrix<T, 4, 4> pose = Eigen::Matrix<T, 4, 4>::Identity();
+
+  T rotation[9];
+  ceres::QuaternionToRotation(q, rotation);
+  for (int i = 0; i < 3; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      pose(i, j) = rotation[i * 3 + j];
+    }
+  }
+
+  pose(0, 3) = p[0];
+  pose(1, 3) = p[1];
+  pose(2, 3) = p[2];
+
+  return pose;
+}
+
+template <typename T>
+Eigen::Matrix<T, 4, 4> poseWithSphericalTranslation(
+    const T *const q, const T *const p, const T scale = T(1.0)) {
+  Eigen::Matrix<T, 4, 4> pose = Eigen::Matrix<T, 4, 4>::Identity();
+
+  T rotation[9];
+  ceres::QuaternionToRotation(q, rotation);
+  for (int i = 0; i < 3; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      pose(i, j) = rotation[i * 3 + j];
+    }
+  }
+
+  T theta = p[0];
+  T phi = p[1];
+  pose(0, 3) = sin(theta) * cos(phi) * scale;
+  pose(1, 3) = sin(theta) * sin(phi) * scale;
+  pose(2, 3) = cos(theta) * scale;
+
+  return pose;
+}
+
+// Returns the Sampson error of a given essential matrix and 2 image points
+template <typename T>
+T sampsonError(
+    const Eigen::Matrix<T, 3, 3> &E, const Eigen::Matrix<T, 3, 1> &p1,
+    const Eigen::Matrix<T, 3, 1> &p2) {
+  Eigen::Matrix<T, 3, 1> Ex1 = E * p1;
+  Eigen::Matrix<T, 3, 1> Etx2 = E.transpose() * p2;
+
+  T x2tEx1 = p2.dot(Ex1);
+
+  // compute Sampson error
+  T err = square(x2tEx1) / (square(Ex1(0, 0)) + square(Ex1(1, 0)) +
+                            square(Etx2(0, 0)) + square(Etx2(1, 0)));
+
+  return err;
+}
+
+// Returns the Sampson error of a given rotation/translation and 2 image points
+template <typename T>
+T sampsonError(
+    const Eigen::Matrix<T, 3, 3> &R, const Eigen::Matrix<T, 3, 1> &t,
+    const Eigen::Matrix<T, 3, 1> &p1, const Eigen::Matrix<T, 3, 1> &p2) {
+  // construct essential matrix
+  Eigen::Matrix<T, 3, 3> E = skew(t) * R;
+
+  Eigen::Matrix<T, 3, 1> Ex1 = E * p1;
+  Eigen::Matrix<T, 3, 1> Etx2 = E.transpose() * p2;
+
+  T x2tEx1 = p2.dot(Ex1);
+
+  // compute Sampson error
+  T err = square(x2tEx1) / (square(Ex1(0, 0)) + square(Ex1(1, 0)) +
+                            square(Etx2(0, 0)) + square(Etx2(1, 0)));
+
+  return err;
+}
+
+// Returns the Sampson error of a given rotation/translation and 2 image points
+template <typename T>
+T sampsonError(
+    const Eigen::Matrix<T, 4, 4> &H, const Eigen::Matrix<T, 3, 1> &p1,
+    const Eigen::Matrix<T, 3, 1> &p2) {
+  Eigen::Matrix<T, 3, 3> R = H.block(0, 0, 3, 3);
+  Eigen::Matrix<T, 3, 1> t = H.block(0, 3, 3, 1);
+
+  return sampsonError(R, t, p1, p2);
+}
+
+template <typename T>
+Eigen::Matrix<T, 3, 1> transformPoint(
+    const Eigen::Matrix<T, 4, 4> &H, const Eigen::Matrix<T, 3, 1> &P) {
+  Eigen::Matrix<T, 3, 1> P_trans =
+      H.block(0, 0, 3, 3) * P + H.block(0, 3, 3, 1);
+
+  return P_trans;
+}
+
+template <typename T>
+Eigen::Matrix<T, 4, 4> estimate3DRigidTransform(
+    const std::vector<Eigen::Matrix<T, 3, 1>,
+                      Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >
+        &points1,
+    const std::vector<Eigen::Matrix<T, 3, 1>,
+                      Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >
+        &points2) {
+  // compute centroids
+  Eigen::Matrix<T, 3, 1> c1, c2;
+  c1.setZero();
+  c2.setZero();
+
+  for (size_t i = 0; i < points1.size(); ++i) {
+    c1 += points1.at(i);
+    c2 += points2.at(i);
+  }
+
+  c1 /= points1.size();
+  c2 /= points1.size();
+
+  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> X(3, points1.size());
+  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Y(3, points1.size());
+  for (size_t i = 0; i < points1.size(); ++i) {
+    X.col(i) = points1.at(i) - c1;
+    Y.col(i) = points2.at(i) - c2;
+  }
+
+  Eigen::Matrix<T, 3, 3> H = X * Y.transpose();
+
+  Eigen::JacobiSVD<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> > svd(
+      H, Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+  Eigen::Matrix<T, 3, 3> U = svd.matrixU();
+  Eigen::Matrix<T, 3, 3> V = svd.matrixV();
+  if (U.determinant() * V.determinant() < 0.0) {
+    V.col(2) *= -1.0;
+  }
+
+  Eigen::Matrix<T, 3, 3> R = V * U.transpose();
+  Eigen::Matrix<T, 3, 1> t = c2 - R * c1;
+
+  return homogeneousTransform(R, t);
+}
+
+template <typename T>
+Eigen::Matrix<T, 4, 4> estimate3DRigidSimilarityTransform(
+    const std::vector<Eigen::Matrix<T, 3, 1>,
+                      Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >
+        &points1,
+    const std::vector<Eigen::Matrix<T, 3, 1>,
+                      Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >
+        &points2) {
+  // compute centroids
+  Eigen::Matrix<T, 3, 1> c1, c2;
+  c1.setZero();
+  c2.setZero();
+
+  for (size_t i = 0; i < points1.size(); ++i) {
+    c1 += points1.at(i);
+    c2 += points2.at(i);
+  }
+
+  c1 /= points1.size();
+  c2 /= points1.size();
+
+  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> X(3, points1.size());
+  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Y(3, points1.size());
+  for (size_t i = 0; i < points1.size(); ++i) {
+    X.col(i) = points1.at(i) - c1;
+    Y.col(i) = points2.at(i) - c2;
+  }
+
+  Eigen::Matrix<T, 3, 3> H = X * Y.transpose();
+
+  Eigen::JacobiSVD<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> > svd(
+      H, Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+  Eigen::Matrix<T, 3, 3> U = svd.matrixU();
+  Eigen::Matrix<T, 3, 3> V = svd.matrixV();
+  if (U.determinant() * V.determinant() < 0.0) {
+    V.col(2) *= -1.0;
+  }
+
+  Eigen::Matrix<T, 3, 3> R = V * U.transpose();
+
+  std::vector<Eigen::Matrix<T, 3, 1>,
+              Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >
+      rotatedPoints1(points1.size());
+  for (size_t i = 0; i < points1.size(); ++i) {
+    rotatedPoints1.at(i) = R * (points1.at(i) - c1);
+  }
+
+  double sum_ss = 0.0, sum_tt = 0.0;
+  for (size_t i = 0; i < points1.size(); ++i) {
+    sum_ss += (points1.at(i) - c1).squaredNorm();
+    sum_tt += (points2.at(i) - c2).dot(rotatedPoints1.at(i));
+  }
+
+  double scale = sum_tt / sum_ss;
+
+  Eigen::Matrix<T, 3, 3> sR = scale * R;
+  Eigen::Matrix<T, 3, 1> t = c2 - sR * c1;
+
+  return homogeneousTransform(sR, t);
+}
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/calib/CameraCalibration.h

@@ -0,0 +1,78 @@
+#ifndef CAMERACALIBRATION_H
+#define CAMERACALIBRATION_H
+
+#include <opencv2/core/core.hpp>
+
+#include "camodocal/camera_models/Camera.h"
+
+namespace camodocal {
+
+class CameraCalibration {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  CameraCalibration();
+
+  CameraCalibration(
+      Camera::ModelType modelType, const std::string &cameraName,
+      const cv::Size &imageSize, const cv::Size &boardSize, float squareSize);
+
+  void clear(void);
+
+  void addChessboardData(const std::vector<cv::Point2f> &corners);
+
+  bool calibrate(void);
+
+  int sampleCount(void) const;
+  std::vector<std::vector<cv::Point2f> > &imagePoints(void);
+  const std::vector<std::vector<cv::Point2f> > &imagePoints(void) const;
+  std::vector<std::vector<cv::Point3f> > &scenePoints(void);
+  const std::vector<std::vector<cv::Point3f> > &scenePoints(void) const;
+  CameraPtr &camera(void);
+  const CameraConstPtr camera(void) const;
+
+  Eigen::Matrix2d &measurementCovariance(void);
+  const Eigen::Matrix2d &measurementCovariance(void) const;
+
+  cv::Mat &cameraPoses(void);
+  const cv::Mat &cameraPoses(void) const;
+
+  void drawResults(std::vector<cv::Mat> &images) const;
+
+  void writeParams(const std::string &filename) const;
+
+  bool writeChessboardData(const std::string &filename) const;
+  bool readChessboardData(const std::string &filename);
+
+  void setVerbose(bool verbose);
+
+ private:
+  bool calibrateHelper(
+      CameraPtr &camera, std::vector<cv::Mat> &rvecs,
+      std::vector<cv::Mat> &tvecs) const;
+
+  void optimize(
+      CameraPtr &camera, std::vector<cv::Mat> &rvecs,
+      std::vector<cv::Mat> &tvecs) const;
+
+  template <typename T>
+  void readData(std::ifstream &ifs, T &data) const;
+
+  template <typename T>
+  void writeData(std::ofstream &ofs, T data) const;
+
+  cv::Size m_boardSize;
+  float m_squareSize;
+
+  CameraPtr m_camera;
+  cv::Mat m_cameraPoses;
+
+  std::vector<std::vector<cv::Point2f> > m_imagePoints;
+  std::vector<std::vector<cv::Point3f> > m_scenePoints;
+
+  Eigen::Matrix2d m_measurementCovariance;
+
+  bool m_verbose;
+};
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/calib/StereoCameraCalibration.h

@@ -0,0 +1,53 @@
+#ifndef STEREOCAMERACALIBRATION_H
+#define STEREOCAMERACALIBRATION_H
+
+#include "CameraCalibration.h"
+
+namespace camodocal {
+
+class StereoCameraCalibration {
+ public:
+  StereoCameraCalibration(
+      Camera::ModelType modelType, const std::string &cameraLeftName,
+      const std::string &cameraRightName, const cv::Size &imageSize,
+      const cv::Size &boardSize, float squareSize);
+
+  void clear(void);
+
+  void addChessboardData(
+      const std::vector<cv::Point2f> &cornersLeft,
+      const std::vector<cv::Point2f> &cornersRight);
+
+  bool calibrate(void);
+
+  int sampleCount(void) const;
+  const std::vector<std::vector<cv::Point2f> > &imagePointsLeft(void) const;
+  const std::vector<std::vector<cv::Point2f> > &imagePointsRight(void) const;
+  const std::vector<std::vector<cv::Point3f> > &scenePoints(void) const;
+
+  CameraPtr &cameraLeft(void);
+  const CameraConstPtr cameraLeft(void) const;
+
+  CameraPtr &cameraRight(void);
+  const CameraConstPtr cameraRight(void) const;
+
+  void drawResults(
+      std::vector<cv::Mat> &imagesLeft,
+      std::vector<cv::Mat> &imagesRight) const;
+
+  void writeParams(const std::string &directory) const;
+  void setVerbose(bool verbose);
+
+ private:
+  CameraCalibration m_calibLeft;
+  CameraCalibration m_calibRight;
+
+  Eigen::Quaterniond m_q;
+  Eigen::Vector3d m_t;
+
+  bool m_verbose;
+  std::vector<double> stereo_error;
+};
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/camera_models/Camera.h

@@ -0,0 +1,138 @@
+#ifndef CAMERA_H
+#define CAMERA_H
+
+#include <boost/shared_ptr.hpp>
+#include "eigen3/Eigen/Dense"
+#include <opencv2/core/core.hpp>
+#include <vector>
+
+namespace camodocal {
+
+class Camera {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  enum ModelType { KANNALA_BRANDT, MEI, PINHOLE, SCARAMUZZA };
+
+  class Parameters {
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    Parameters(ModelType modelType);
+
+    Parameters(
+        ModelType modelType, const std::string &cameraName, int w, int h);
+
+    ModelType &modelType(void);
+    std::string &cameraName(void);
+    int &imageWidth(void);
+    int &imageHeight(void);
+
+    ModelType modelType(void) const;
+    const std::string &cameraName(void) const;
+    int imageWidth(void) const;
+    int imageHeight(void) const;
+
+    int nIntrinsics(void) const;
+
+    virtual bool readFromYamlFile(const std::string &filename) = 0;
+    virtual void writeToYamlFile(const std::string &filename) const = 0;
+
+   protected:
+    ModelType m_modelType;
+    int m_nIntrinsics;
+    std::string m_cameraName;
+    int m_imageWidth;
+    int m_imageHeight;
+  };
+
+  virtual ModelType modelType(void) const = 0;
+  virtual const std::string &cameraName(void) const = 0;
+  virtual int imageWidth(void) const = 0;
+  virtual int imageHeight(void) const = 0;
+
+  virtual cv::Mat &mask(void);
+  virtual const cv::Mat &mask(void) const;
+
+  virtual void estimateIntrinsics(
+      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;
+
+  // Lift points from the image plane to the sphere
+  virtual void liftSphere(
+      const Eigen::Vector2d &p, Eigen::Vector3d &P) const = 0;
+  //%output P
+
+  // Lift points from the image plane to the projective space
+  virtual void liftProjective(
+      const Eigen::Vector2d &p, Eigen::Vector3d &P) const = 0;
+  //%output P
+
+  // Projects 3D points to the image plane (Pi function)
+  virtual void spaceToPlane(
+      const Eigen::Vector3d &P, Eigen::Vector2d &p) const = 0;
+  //%output p
+
+  // Projects 3D points to the image plane (Pi function)
+  // and calculates jacobian
+  // virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
+  //                          Eigen::Matrix<double,2,3>& J) const = 0;
+  //%output p
+  //%output J
+
+  virtual void undistToPlane(
+      const Eigen::Vector2d &p_u, Eigen::Vector2d &p) const = 0;
+  //%output p
+
+  // virtual void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale =
+  // 1.0) const = 0;
+  virtual cv::Mat initUndistortRectifyMap(
+      cv::Mat &map1, cv::Mat &map2, float fx = -1.0f, float fy = -1.0f,
+      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 = 0;
+
+  virtual int parameterCount(void) const = 0;
+
+  virtual void readParameters(const std::vector<double> &parameters) = 0;
+  virtual void writeParameters(std::vector<double> &parameters) const = 0;
+
+  virtual void writeParametersToYamlFile(const std::string &filename) const = 0;
+
+  virtual std::string parametersToString(void) const = 0;
+
+  /**
+   * \brief Calculates the reprojection distance between points
+   *
+   * \param P1 first 3D point coordinates
+   * \param P2 second 3D point coordinates
+   * \return euclidean distance in the plane
+   */
+  double reprojectionDist(
+      const Eigen::Vector3d &P1, const Eigen::Vector3d &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;
+
+  void projectPoints(
+      const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec,
+      const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const;
+
+ protected:
+  cv::Mat m_mask;
+};
+
+typedef boost::shared_ptr<Camera> CameraPtr;
+typedef boost::shared_ptr<const Camera> CameraConstPtr;
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/camera_models/CameraFactory.h

@@ -0,0 +1,29 @@
+#ifndef CAMERAFACTORY_H
+#define CAMERAFACTORY_H
+
+#include <boost/shared_ptr.hpp>
+#include <opencv2/core/core.hpp>
+
+#include "camodocal/camera_models/Camera.h"
+
+namespace camodocal {
+
+class CameraFactory {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  CameraFactory();
+
+  static boost::shared_ptr<CameraFactory> instance(void);
+
+  CameraPtr generateCamera(
+      Camera::ModelType modelType, const std::string &cameraName,
+      cv::Size imageSize) const;
+
+  CameraPtr generateCameraFromYamlFile(const std::string &filename);
+
+ private:
+  static boost::shared_ptr<CameraFactory> m_instance;
+};
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/camera_models/CataCamera.h

@@ -0,0 +1,205 @@
+#ifndef CATACAMERA_H
+#define CATACAMERA_H
+
+#include <opencv2/core/core.hpp>
+#include <string>
+
+#include "Camera.h"
+#include "ceres/rotation.h"
+
+namespace camodocal {
+
+/**
+ * C. Mei, and P. Rives, Single View Point Omnidirectional Camera Calibration
+ * from Planar Grids, ICRA 2007
+ */
+
+class CataCamera : public Camera {
+ public:
+  class Parameters : public Camera::Parameters {
+   public:
+    Parameters();
+    Parameters(
+        const std::string &cameraName, int w, int h, double xi, double k1,
+        double k2, double p1, double p2, double gamma1, double gamma2,
+        double u0, double v0);
+
+    double &xi(void);
+    double &k1(void);
+    double &k2(void);
+    double &p1(void);
+    double &p2(void);
+    double &gamma1(void);
+    double &gamma2(void);
+    double &u0(void);
+    double &v0(void);
+
+    double xi(void) const;
+    double k1(void) const;
+    double k2(void) const;
+    double p1(void) const;
+    double p2(void) const;
+    double gamma1(void) const;
+    double gamma2(void) const;
+    double u0(void) const;
+    double v0(void) const;
+
+    bool readFromYamlFile(const std::string &filename);
+    void writeToYamlFile(const std::string &filename) const;
+
+    Parameters &operator=(const Parameters &other);
+    friend std::ostream &operator<<(
+        std::ostream &out, const Parameters &params);
+
+   private:
+    double m_xi;
+    double m_k1;
+    double m_k2;
+    double m_p1;
+    double m_p2;
+    double m_gamma1;
+    double m_gamma2;
+    double m_u0;
+    double m_v0;
+  };
+
+  CataCamera();
+
+  /**
+  * \brief Constructor from the projection model parameters
+  */
+  CataCamera(
+      const std::string &cameraName, int imageWidth, int imageHeight, double xi,
+      double k1, double k2, double p1, double p2, double gamma1, double gamma2,
+      double u0, double v0);
+  /**
+  * \brief Constructor from the projection model parameters
+  */
+  CataCamera(const Parameters &params);
+
+  Camera::ModelType modelType(void) const;
+  const std::string &cameraName(void) const;
+  int imageWidth(void) const;
+  int imageHeight(void) const;
+
+  void estimateIntrinsics(
+      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 sphere
+  void liftSphere(const Eigen::Vector2d &p, Eigen::Vector3d &P) const;
+  //%output P
+
+  // Lift points from the image plane to the projective space
+  void liftProjective(const Eigen::Vector2d &p, Eigen::Vector3d &P) const;
+  //%output P
+
+  // Projects 3D points to the image plane (Pi function)
+  void spaceToPlane(const Eigen::Vector3d &P, Eigen::Vector2d &p) const;
+  //%output p
+
+  // Projects 3D points to the image plane (Pi function)
+  // and calculates jacobian
+  void spaceToPlane(
+      const Eigen::Vector3d &P, Eigen::Vector2d &p,
+      Eigen::Matrix<double, 2, 3> &J) const;
+  //%output p
+  //%output J
+
+  void undistToPlane(const Eigen::Vector2d &p_u, Eigen::Vector2d &p) const;
+  //%output p
+
+  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);
+
+  void distortion(const Eigen::Vector2d &p_u, Eigen::Vector2d &d_u) const;
+  void distortion(
+      const Eigen::Vector2d &p_u, Eigen::Vector2d &d_u,
+      Eigen::Matrix2d &J) const;
+
+  void initUndistortMap(
+      cv::Mat &map1, cv::Mat &map2, double fScale = 1.0) const;
+  cv::Mat initUndistortRectifyMap(
+      cv::Mat &map1, cv::Mat &map2, float fx = -1.0f, float fy = -1.0f,
+      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;
+  void setParameters(const Parameters &parameters);
+
+  void readParameters(const std::vector<double> &parameterVec);
+  void writeParameters(std::vector<double> &parameterVec) const;
+
+  void writeParametersToYamlFile(const std::string &filename) const;
+
+  std::string parametersToString(void) const;
+
+ private:
+  Parameters mParameters;
+
+  double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
+  bool m_noDistortion;
+};
+
+typedef boost::shared_ptr<CataCamera> CataCameraPtr;
+typedef boost::shared_ptr<const CataCamera> CataCameraConstPtr;
+
+template <typename T>
+void CataCamera::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];
+  ceres::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 xi = params[0];
+  T k1 = params[1];
+  T k2 = params[2];
+  T p1 = params[3];
+  T p2 = params[4];
+  T gamma1 = params[5];
+  T gamma2 = params[6];
+  T alpha = T(0);  // cameraParams.alpha();
+  T u0 = params[7];
+  T v0 = params[8];
+
+  // Transform to model plane
+  T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]);
+  P_c[0] /= len;
+  P_c[1] /= len;
+  P_c[2] /= len;
+
+  T u = P_c[0] / (P_c[2] + xi);
+  T v = P_c[1] / (P_c[2] + xi);
+
+  T rho_sqr = u * u + v * v;
+  T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr;
+  T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u);
+  T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v;
+
+  u = L * u + du;
+  v = L * v + dv;
+  p(0) = gamma1 * (u + alpha * v) + u0;
+  p(1) = gamma2 * v + v0;
+}
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/camera_models/CostFunctionFactory.h

@@ -0,0 +1,71 @@
+#ifndef COSTFUNCTIONFACTORY_H
+#define COSTFUNCTIONFACTORY_H
+
+#include <boost/shared_ptr.hpp>
+#include <opencv2/core/core.hpp>
+
+#include "camodocal/camera_models/Camera.h"
+
+namespace ceres {
+class CostFunction;
+}
+
+namespace camodocal {
+
+enum {
+  CAMERA_INTRINSICS = 1 << 0,
+  CAMERA_POSE = 1 << 1,
+  POINT_3D = 1 << 2,
+  ODOMETRY_INTRINSICS = 1 << 3,
+  ODOMETRY_3D_POSE = 1 << 4,
+  ODOMETRY_6D_POSE = 1 << 5,
+  CAMERA_ODOMETRY_TRANSFORM = 1 << 6
+};
+
+class CostFunctionFactory {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  CostFunctionFactory();
+
+  static boost::shared_ptr<CostFunctionFactory> instance(void);
+
+  ceres::CostFunction *generateCostFunction(
+      const CameraConstPtr &camera, const Eigen::Vector3d &observed_P,
+      const Eigen::Vector2d &observed_p, int flags) const;
+
+  ceres::CostFunction *generateCostFunction(
+      const CameraConstPtr &camera, const Eigen::Vector3d &observed_P,
+      const Eigen::Vector2d &observed_p,
+      const Eigen::Matrix2d &sqrtPrecisionMat, int flags) const;
+
+  ceres::CostFunction *generateCostFunction(
+      const CameraConstPtr &camera, const Eigen::Vector2d &observed_p,
+      int flags, bool optimize_cam_odo_z = true) const;
+
+  ceres::CostFunction *generateCostFunction(
+      const CameraConstPtr &camera, const Eigen::Vector2d &observed_p,
+      const Eigen::Matrix2d &sqrtPrecisionMat, int flags,
+      bool optimize_cam_odo_z = true) const;
+
+  ceres::CostFunction *generateCostFunction(
+      const CameraConstPtr &camera, const Eigen::Vector3d &odo_pos,
+      const Eigen::Vector3d &odo_att, const Eigen::Vector2d &observed_p,
+      int flags, bool optimize_cam_odo_z = true) const;
+
+  ceres::CostFunction *generateCostFunction(
+      const CameraConstPtr &camera, const Eigen::Quaterniond &cam_odo_q,
+      const Eigen::Vector3d &cam_odo_t, const Eigen::Vector3d &odo_pos,
+      const Eigen::Vector3d &odo_att, const Eigen::Vector2d &observed_p,
+      int flags) const;
+
+  ceres::CostFunction *generateCostFunction(
+      const CameraConstPtr &cameraLeft, const CameraConstPtr &cameraRight,
+      const Eigen::Vector3d &observed_P, const Eigen::Vector2d &observed_p_left,
+      const Eigen::Vector2d &observed_p_right) const;
+
+ private:
+  static boost::shared_ptr<CostFunctionFactory> m_instance;
+};
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/camera_models/EquidistantCamera.h

@@ -0,0 +1,212 @@
+#ifndef EQUIDISTANTCAMERA_H
+#define EQUIDISTANTCAMERA_H
+
+#include <opencv2/core/core.hpp>
+#include <string>
+
+#include "Camera.h"
+#include "ceres/rotation.h"
+
+namespace camodocal {
+
+/**
+ * J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method
+ * for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006
+ */
+
+class EquidistantCamera : public Camera {
+ public:
+  class Parameters : public Camera::Parameters {
+   public:
+    Parameters();
+    Parameters(
+        const std::string &cameraName, int w, int h, double k2, double k3,
+        double k4, double k5, double mu, double mv, double u0, double v0);
+
+    double &k2(void);
+    double &k3(void);
+    double &k4(void);
+    double &k5(void);
+    double &mu(void);
+    double &mv(void);
+    double &u0(void);
+    double &v0(void);
+
+    double k2(void) const;
+    double k3(void) const;
+    double k4(void) const;
+    double k5(void) const;
+    double mu(void) const;
+    double mv(void) const;
+    double u0(void) const;
+    double v0(void) const;
+
+    bool readFromYamlFile(const std::string &filename);
+    void writeToYamlFile(const std::string &filename) const;
+
+    Parameters &operator=(const Parameters &other);
+    friend std::ostream &operator<<(
+        std::ostream &out, const Parameters &params);
+
+   private:
+    // projection
+    double m_k2;
+    double m_k3;
+    double m_k4;
+    double m_k5;
+
+    double m_mu;
+    double m_mv;
+    double m_u0;
+    double m_v0;
+  };
+
+  EquidistantCamera();
+
+  /**
+  * \brief Constructor from the projection model parameters
+  */
+  EquidistantCamera(
+      const std::string &cameraName, int imageWidth, int imageHeight, double k2,
+      double k3, double k4, double k5, double mu, double mv, double u0,
+      double v0);
+  /**
+  * \brief Constructor from the projection model parameters
+  */
+  EquidistantCamera(const Parameters &params);
+
+  Camera::ModelType modelType(void) const;
+  const std::string &cameraName(void) const;
+  int imageWidth(void) const;
+  int imageHeight(void) const;
+
+  void estimateIntrinsics(
+      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 sphere
+  virtual void liftSphere(const Eigen::Vector2d &p, Eigen::Vector3d &P) const;
+  //%output P
+
+  // Lift points from the image plane to the projective space
+  void liftProjective(const Eigen::Vector2d &p, Eigen::Vector3d &P) const;
+  //%output P
+
+  // Projects 3D points to the image plane (Pi function)
+  void spaceToPlane(const Eigen::Vector3d &P, Eigen::Vector2d &p) const;
+  //%output p
+
+  // Projects 3D points to the image plane (Pi function)
+  // and calculates jacobian
+  void spaceToPlane(
+      const Eigen::Vector3d &P, Eigen::Vector2d &p,
+      Eigen::Matrix<double, 2, 3> &J) const;
+  //%output p
+  //%output J
+
+  void undistToPlane(const Eigen::Vector2d &p_u, Eigen::Vector2d &p) const;
+  //%output p
+
+  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);
+
+  void initUndistortMap(
+      cv::Mat &map1, cv::Mat &map2, double fScale = 1.0) const;
+  cv::Mat initUndistortRectifyMap(
+      cv::Mat &map1, cv::Mat &map2, float fx = -1.0f, float fy = -1.0f,
+      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;
+  void setParameters(const Parameters &parameters);
+
+  void readParameters(const std::vector<double> &parameterVec);
+  void writeParameters(std::vector<double> &parameterVec) const;
+
+  void writeParametersToYamlFile(const std::string &filename) const;
+
+  std::string parametersToString(void) const;
+
+ private:
+  template <typename T>
+  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;
+
+  void backprojectSymmetric(
+      const Eigen::Vector2d &p_u, double &theta, double &phi) const;
+
+  Parameters mParameters;
+
+  double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
+};
+
+typedef boost::shared_ptr<EquidistantCamera> EquidistantCameraPtr;
+typedef boost::shared_ptr<const EquidistantCamera> EquidistantCameraConstPtr;
+
+template <typename T>
+T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) {
+  // k1 = 1
+  T theta2 = theta*theta;
+  T theta3 = theta2*theta;
+  T theta5 = theta2 * theta3;
+  T theta7 = theta5 * theta2;
+  T theta9 = theta7 * theta2;
+  return theta + k2 * theta3 + k3 * theta5 + k4 * theta7 + k5 * theta9;
+//  return theta + k2 * theta * theta * theta +
+//         k3 * theta * theta * theta * theta * theta +
+//         k4 * theta * theta * theta * theta * theta * theta * theta +
+//         k5 * theta * theta * theta * theta * theta * theta * theta * 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];
+  ceres::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;
+}
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/camera_models/PinholeCamera.h

@@ -0,0 +1,191 @@
+#ifndef PINHOLECAMERA_H
+#define PINHOLECAMERA_H
+
+#include <opencv2/core/core.hpp>
+#include <string>
+
+#include "Camera.h"
+#include "ceres/rotation.h"
+
+namespace camodocal {
+
+class PinholeCamera : public Camera {
+ public:
+  class Parameters : public Camera::Parameters {
+   public:
+    Parameters();
+    Parameters(
+        const std::string &cameraName, int w, int h, double k1, double k2,
+        double p1, double p2, double fx, double fy, double cx, double cy);
+
+    double &k1(void);
+    double &k2(void);
+    double &p1(void);
+    double &p2(void);
+    double &fx(void);
+    double &fy(void);
+    double &cx(void);
+    double &cy(void);
+
+    double xi(void) const;
+    double k1(void) const;
+    double k2(void) const;
+    double p1(void) const;
+    double p2(void) const;
+    double fx(void) const;
+    double fy(void) const;
+    double cx(void) const;
+    double cy(void) const;
+
+    bool readFromYamlFile(const std::string &filename);
+    void writeToYamlFile(const std::string &filename) const;
+
+    Parameters &operator=(const Parameters &other);
+    friend std::ostream &operator<<(
+        std::ostream &out, const Parameters &params);
+
+   private:
+    double m_k1;
+    double m_k2;
+    double m_p1;
+    double m_p2;
+    double m_fx;
+    double m_fy;
+    double m_cx;
+    double m_cy;
+  };
+
+  PinholeCamera();
+
+  /**
+  * \brief Constructor from the projection model parameters
+  */
+  PinholeCamera(
+      const std::string &cameraName, int imageWidth, int imageHeight, double k1,
+      double k2, double p1, double p2, double fx, double fy, double cx,
+      double cy);
+  /**
+  * \brief Constructor from the projection model parameters
+  */
+  PinholeCamera(const Parameters &params);
+
+  Camera::ModelType modelType(void) const;
+  const std::string &cameraName(void) const;
+  int imageWidth(void) const;
+  int imageHeight(void) const;
+
+  void estimateIntrinsics(
+      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 sphere
+  virtual void liftSphere(const Eigen::Vector2d &p, Eigen::Vector3d &P) const;
+  //%output P
+
+  // Lift points from the image plane to the projective space
+  void liftProjective(const Eigen::Vector2d &p, Eigen::Vector3d &P) const;
+  //%output P
+
+  // Projects 3D points to the image plane (Pi function)
+  void spaceToPlane(const Eigen::Vector3d &P, Eigen::Vector2d &p) const;
+  //%output p
+
+  // Projects 3D points to the image plane (Pi function)
+  // and calculates jacobian
+  void spaceToPlane(
+      const Eigen::Vector3d &P, Eigen::Vector2d &p,
+      Eigen::Matrix<double, 2, 3> &J) const;
+  //%output p
+  //%output J
+
+  void undistToPlane(const Eigen::Vector2d &p_u, Eigen::Vector2d &p) const;
+  //%output p
+
+  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);
+
+  void distortion(const Eigen::Vector2d &p_u, Eigen::Vector2d &d_u) const;
+  void distortion(
+      const Eigen::Vector2d &p_u, Eigen::Vector2d &d_u,
+      Eigen::Matrix2d &J) const;
+
+  void initUndistortMap(
+      cv::Mat &map1, cv::Mat &map2, double fScale = 1.0) const;
+  cv::Mat initUndistortRectifyMap(
+      cv::Mat &map1, cv::Mat &map2, float fx = -1.0f, float fy = -1.0f,
+      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;
+  void setParameters(const Parameters &parameters);
+
+  void readParameters(const std::vector<double> &parameterVec);
+  void writeParameters(std::vector<double> &parameterVec) const;
+
+  void writeParametersToYamlFile(const std::string &filename) const;
+
+  std::string parametersToString(void) const;
+
+ private:
+  Parameters mParameters;
+
+  double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
+  bool m_noDistortion;
+};
+
+typedef boost::shared_ptr<PinholeCamera> PinholeCameraPtr;
+typedef boost::shared_ptr<const PinholeCamera> PinholeCameraConstPtr;
+
+template <typename T>
+void PinholeCamera::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];
+  ceres::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 k1 = params[0];
+  T k2 = params[1];
+  T p1 = params[2];
+  T p2 = params[3];
+  T fx = params[4];
+  T fy = params[5];
+  T alpha = T(0);  // cameraParams.alpha();
+  T cx = params[6];
+  T cy = params[7];
+
+  // Transform to model plane
+  T u = P_c[0] / P_c[2];
+  T v = P_c[1] / P_c[2];
+
+  T rho_sqr = u * u + v * v;
+  T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr;
+  T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u);
+  T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v;
+
+  u = L * u + du;
+  v = L * v + dv;
+  p(0) = fx * (u + alpha * v) + cx;
+  p(1) = fy * v + cy;
+}
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/camera_models/ScaramuzzaCamera.h

@@ -0,0 +1,364 @@
+#ifndef SCARAMUZZACAMERA_H
+#define SCARAMUZZACAMERA_H
+
+#include <opencv2/core/core.hpp>
+#include <string>
+
+#include "Camera.h"
+#include "ceres/rotation.h"
+
+namespace camodocal {
+
+#define SCARAMUZZA_POLY_SIZE 5
+#define SCARAMUZZA_INV_POLY_SIZE 20
+
+#define SCARAMUZZA_CAMERA_NUM_PARAMS                                \
+  (SCARAMUZZA_POLY_SIZE + SCARAMUZZA_INV_POLY_SIZE + 2 /*center*/ + \
+   3 /*affine*/)
+
+/**
+ * Scaramuzza Camera (Omnidirectional)
+ * https://sites.google.com/site/scarabotix/ocamcalib-toolbox
+ */
+
+class OCAMCamera : public Camera {
+ public:
+  class Parameters : public Camera::Parameters {
+   public:
+    Parameters();
+
+    double &C(void) {
+      return m_C;
+    }
+    double &D(void) {
+      return m_D;
+    }
+    double &E(void) {
+      return m_E;
+    }
+
+    double &center_x(void) {
+      return m_center_x;
+    }
+    double &center_y(void) {
+      return m_center_y;
+    }
+
+    double &poly(int idx) {
+      return m_poly[idx];
+    }
+    double &inv_poly(int idx) {
+      return m_inv_poly[idx];
+    }
+
+    double C(void) const {
+      return m_C;
+    }
+    double D(void) const {
+      return m_D;
+    }
+    double E(void) const {
+      return m_E;
+    }
+
+    double center_x(void) const {
+      return m_center_x;
+    }
+    double center_y(void) const {
+      return m_center_y;
+    }
+
+    double poly(int idx) const {
+      return m_poly[idx];
+    }
+    double inv_poly(int idx) const {
+      return m_inv_poly[idx];
+    }
+
+    bool readFromYamlFile(const std::string &filename);
+    void writeToYamlFile(const std::string &filename) const;
+
+    Parameters &operator=(const Parameters &other);
+    friend std::ostream &operator<<(
+        std::ostream &out, const Parameters &params);
+
+   private:
+    double m_poly[SCARAMUZZA_POLY_SIZE];
+    double m_inv_poly[SCARAMUZZA_INV_POLY_SIZE];
+    double m_C;
+    double m_D;
+    double m_E;
+    double m_center_x;
+    double m_center_y;
+  };
+
+  OCAMCamera();
+
+  /**
+  * \brief Constructor from the projection model parameters
+  */
+  OCAMCamera(const Parameters &params);
+
+  Camera::ModelType modelType(void) const;
+  const std::string &cameraName(void) const;
+  int imageWidth(void) const;
+  int imageHeight(void) const;
+
+  void estimateIntrinsics(
+      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 sphere
+  void liftSphere(const Eigen::Vector2d &p, Eigen::Vector3d &P) const;
+  //%output P
+
+  // Lift points from the image plane to the projective space
+  void liftProjective(const Eigen::Vector2d &p, Eigen::Vector3d &P) const;
+  //%output P
+
+  // Projects 3D points to the image plane (Pi function)
+  void spaceToPlane(const Eigen::Vector3d &P, Eigen::Vector2d &p) const;
+  //%output p
+
+  // Projects 3D points to the image plane (Pi function)
+  // and calculates jacobian
+  // void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
+  //                  Eigen::Matrix<double,2,3>& J) const;
+  //%output p
+  //%output J
+
+  void undistToPlane(const Eigen::Vector2d &p_u, Eigen::Vector2d &p) const;
+  //%output p
+
+  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);
+  template <typename T>
+  static void spaceToSphere(
+      const T *const params, const T *const q, const T *const t,
+      const Eigen::Matrix<T, 3, 1> &P, Eigen::Matrix<T, 3, 1> &P_s);
+  template <typename T>
+  static void LiftToSphere(
+      const T *const params, const Eigen::Matrix<T, 2, 1> &p,
+      Eigen::Matrix<T, 3, 1> &P);
+
+  template <typename T>
+  static void SphereToPlane(
+      const T *const params, const Eigen::Matrix<T, 3, 1> &P,
+      Eigen::Matrix<T, 2, 1> &p);
+
+  void initUndistortMap(
+      cv::Mat &map1, cv::Mat &map2, double fScale = 1.0) const;
+  cv::Mat initUndistortRectifyMap(
+      cv::Mat &map1, cv::Mat &map2, float fx = -1.0f, float fy = -1.0f,
+      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;
+  void setParameters(const Parameters &parameters);
+
+  void readParameters(const std::vector<double> &parameterVec);
+  void writeParameters(std::vector<double> &parameterVec) const;
+
+  void writeParametersToYamlFile(const std::string &filename) const;
+
+  std::string parametersToString(void) const;
+
+ private:
+  Parameters mParameters;
+
+  double m_inv_scale;
+};
+
+typedef boost::shared_ptr<OCAMCamera> OCAMCameraPtr;
+typedef boost::shared_ptr<const OCAMCamera> OCAMCameraConstPtr;
+
+template <typename T>
+void OCAMCamera::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_c[3];
+  {
+    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]};
+
+    ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);
+
+    P_c[0] += t[0];
+    P_c[1] += t[1];
+    P_c[2] += t[2];
+  }
+
+  T c = params[0];
+  T d = params[1];
+  T e = params[2];
+  T xc[2] = {params[3], params[4]};
+
+  // T poly[SCARAMUZZA_POLY_SIZE];
+  // for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)
+  //    poly[i] = params[5+i];
+
+  T inv_poly[SCARAMUZZA_INV_POLY_SIZE];
+  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
+    inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i];
+
+  T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1];
+  T norm = T(0.0);
+  if (norm_sqr > T(0.0))
+    norm = sqrt(norm_sqr);
+
+  T theta = atan2(-P_c[2], norm);
+  T rho = T(0.0);
+  T theta_i = T(1.0);
+
+  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) {
+    rho += theta_i * inv_poly[i];
+    theta_i *= theta;
+  }
+
+  T invNorm = T(1.0) / norm;
+  T xn[2] = {P_c[0] * invNorm * rho, P_c[1] * invNorm * rho};
+
+  p(0) = xn[0] * c + xn[1] * d + xc[0];
+  p(1) = xn[0] * e + xn[1] + xc[1];
+}
+
+template <typename T>
+void OCAMCamera::spaceToSphere(
+    const T *const params, const T *const q, const T *const t,
+    const Eigen::Matrix<T, 3, 1> &P, Eigen::Matrix<T, 3, 1> &P_s) {
+  T P_c[3];
+  {
+    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]};
+
+    ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);
+
+    P_c[0] += t[0];
+    P_c[1] += t[1];
+    P_c[2] += t[2];
+  }
+
+  // T poly[SCARAMUZZA_POLY_SIZE];
+  // for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)
+  //    poly[i] = params[5+i];
+
+  T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2];
+  T norm = T(0.0);
+  if (norm_sqr > T(0.0))
+    norm = sqrt(norm_sqr);
+
+  P_s(0) = P_c[0] / norm;
+  P_s(1) = P_c[1] / norm;
+  P_s(2) = P_c[2] / norm;
+}
+
+template <typename T>
+void OCAMCamera::LiftToSphere(
+    const T *const params, const Eigen::Matrix<T, 2, 1> &p,
+    Eigen::Matrix<T, 3, 1> &P) {
+  T c = params[0];
+  T d = params[1];
+  T e = params[2];
+  T cc[2] = {params[3], params[4]};
+  T poly[SCARAMUZZA_POLY_SIZE];
+  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
+    poly[i] = params[5 + i];
+
+  // Relative to Center
+  T p_2d[2];
+  p_2d[0] = T(p(0));
+  p_2d[1] = T(p(1));
+
+  T xc[2] = {p_2d[0] - cc[0], p_2d[1] - cc[1]};
+
+  T inv_scale = T(1.0) / (c - d * e);
+
+  // Affine Transformation
+  T xc_a[2];
+
+  xc_a[0] = inv_scale * (xc[0] - d * xc[1]);
+  xc_a[1] = inv_scale * (-e * xc[0] + c * xc[1]);
+
+  T norm_sqr = xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1];
+  T phi = sqrt(norm_sqr);
+  T phi_i = T(1.0);
+  T z = T(0.0);
+
+  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) {
+    if (i != 1) {
+      z += phi_i * poly[i];
+    }
+    phi_i *= phi;
+  }
+
+  T p_3d[3];
+  p_3d[0] = xc[0];
+  p_3d[1] = xc[1];
+  p_3d[2] = -z;
+
+  T p_3d_norm_sqr = p_3d[0] * p_3d[0] + p_3d[1] * p_3d[1] + p_3d[2] * p_3d[2];
+  T p_3d_norm = sqrt(p_3d_norm_sqr);
+
+  P << p_3d[0] / p_3d_norm, p_3d[1] / p_3d_norm, p_3d[2] / p_3d_norm;
+}
+
+template <typename T>
+void OCAMCamera::SphereToPlane(
+    const T *const params, const Eigen::Matrix<T, 3, 1> &P,
+    Eigen::Matrix<T, 2, 1> &p) {
+  T P_c[3];
+  {
+    P_c[0] = T(P(0));
+    P_c[1] = T(P(1));
+    P_c[2] = T(P(2));
+  }
+
+  T c = params[0];
+  T d = params[1];
+  T e = params[2];
+  T xc[2] = {params[3], params[4]};
+
+  T inv_poly[SCARAMUZZA_INV_POLY_SIZE];
+  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
+    inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i];
+
+  T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1];
+  T norm = T(0.0);
+  if (norm_sqr > T(0.0))
+    norm = sqrt(norm_sqr);
+
+  T theta = atan2(-P_c[2], norm);
+  T rho = T(0.0);
+  T theta_i = T(1.0);
+
+  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) {
+    rho += theta_i * inv_poly[i];
+    theta_i *= theta;
+  }
+
+  T invNorm = T(1.0) / norm;
+  T xn[2] = {P_c[0] * invNorm * rho, P_c[1] * invNorm * rho};
+
+  p(0) = xn[0] * c + xn[1] * d + xc[0];
+  p(1) = xn[0] * e + xn[1] + xc[1];
+}
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/chessboard/Chessboard.h

@@ -0,0 +1,85 @@
+#ifndef CHESSBOARD_H
+#define CHESSBOARD_H
+
+#include <boost/shared_ptr.hpp>
+#include <opencv2/core/core.hpp>
+
+namespace camodocal {
+
+// forward declarations
+class ChessboardCorner;
+typedef boost::shared_ptr<ChessboardCorner> ChessboardCornerPtr;
+class ChessboardQuad;
+typedef boost::shared_ptr<ChessboardQuad> ChessboardQuadPtr;
+
+class Chessboard {
+ public:
+  Chessboard(cv::Size boardSize, cv::Mat &image);
+
+  void findCorners(bool useOpenCV = false);
+  const std::vector<cv::Point2f> &getCorners(void) const;
+  bool cornersFound(void) const;
+
+  const cv::Mat &getImage(void) const;
+  const cv::Mat &getSketch(void) const;
+
+ private:
+  bool findChessboardCorners(
+      const cv::Mat &image, const cv::Size &patternSize,
+      std::vector<cv::Point2f> &corners, int flags, bool useOpenCV);
+
+  bool findChessboardCornersImproved(
+      const cv::Mat &image, const cv::Size &patternSize,
+      std::vector<cv::Point2f> &corners, int flags);
+
+  void cleanFoundConnectedQuads(
+      std::vector<ChessboardQuadPtr> &quadGroup, cv::Size patternSize);
+
+  void findConnectedQuads(
+      std::vector<ChessboardQuadPtr> &quads,
+      std::vector<ChessboardQuadPtr> &group, int group_idx, int dilation);
+
+  //    int checkQuadGroup(std::vector<ChessboardQuadPtr>& quadGroup,
+  //                       std::vector<ChessboardCornerPtr>& outCorners,
+  //                       cv::Size patternSize);
+
+  void labelQuadGroup(
+      std::vector<ChessboardQuadPtr> &quad_group, cv::Size patternSize,
+      bool firstRun);
+
+  void findQuadNeighbors(std::vector<ChessboardQuadPtr> &quads, int dilation);
+
+  int augmentBestRun(
+      std::vector<ChessboardQuadPtr> &candidateQuads, int candidateDilation,
+      std::vector<ChessboardQuadPtr> &existingQuads, int existingDilation);
+
+  void generateQuads(
+      std::vector<ChessboardQuadPtr> &quads, cv::Mat &image, int flags,
+      int dilation, bool firstRun);
+
+  bool checkQuadGroup(
+      std::vector<ChessboardQuadPtr> &quads,
+      std::vector<ChessboardCornerPtr> &corners, cv::Size patternSize);
+
+  void getQuadrangleHypotheses(
+      const std::vector<std::vector<cv::Point> > &contours,
+      std::vector<std::pair<float, int> > &quads, int classId) const;
+
+  bool checkChessboard(const cv::Mat &image, cv::Size patternSize) const;
+
+  bool checkBoardMonotony(
+      std::vector<ChessboardCornerPtr> &corners, cv::Size patternSize);
+
+  bool matchCorners(
+      ChessboardQuadPtr &quad1, int corner1, ChessboardQuadPtr &quad2,
+      int corner2) const;
+
+  cv::Mat mImage;
+  cv::Mat mSketch;
+  std::vector<cv::Point2f> mCorners;
+  cv::Size mBoardSize;
+  bool mCornersFound;
+};
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/chessboard/ChessboardCorner.h

@@ -0,0 +1,39 @@
+#ifndef CHESSBOARDCORNER_H
+#define CHESSBOARDCORNER_H
+
+#include <boost/shared_ptr.hpp>
+#include <opencv2/core/core.hpp>
+
+namespace camodocal {
+
+class ChessboardCorner;
+typedef boost::shared_ptr<ChessboardCorner> ChessboardCornerPtr;
+
+class ChessboardCorner {
+ public:
+  ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {}
+
+  float meanDist(int &n) const {
+    float sum = 0;
+    n = 0;
+    for (int i = 0; i < 4; ++i) {
+      if (neighbors[i].get()) {
+        float dx = neighbors[i]->pt.x - pt.x;
+        float dy = neighbors[i]->pt.y - pt.y;
+        sum += sqrt(dx * dx + dy * dy);
+        n++;
+      }
+    }
+    return sum / std::max(n, 1);
+  }
+
+  cv::Point2f pt;                    // X and y coordinates
+  int row;                           // Row and column of the corner
+  int column;                        // in the found pattern
+  bool needsNeighbor;                // Does the corner require a neighbor?
+  int count;                         // number of corner neighbors
+  ChessboardCornerPtr neighbors[4];  // pointer to all corner neighbors
+};
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/chessboard/ChessboardQuad.h

@@ -0,0 +1,27 @@
+#ifndef CHESSBOARDQUAD_H
+#define CHESSBOARDQUAD_H
+
+#include <boost/shared_ptr.hpp>
+
+#include "camodocal/chessboard/ChessboardCorner.h"
+
+namespace camodocal {
+
+class ChessboardQuad;
+typedef boost::shared_ptr<ChessboardQuad> ChessboardQuadPtr;
+
+class ChessboardQuad {
+ public:
+  ChessboardQuad()
+      : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {}
+
+  int count;                       // Number of quad neighbors
+  int group_idx;                   // Quad group ID
+  float edge_len;                  // Smallest side length^2
+  ChessboardCornerPtr corners[4];  // Coordinates of quad corners
+  ChessboardQuadPtr neighbors[4];  // Pointers of quad neighbors
+  bool labeled;                    // Has this corner been labeled?
+};
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/chessboard/Spline.h

@@ -0,0 +1,336 @@
+/*  dynamo:- Event driven molecular dynamics simulator
+    http://www.marcusbannerman.co.uk/dynamo
+    Copyright (C) 2011  Marcus N Campbell Bannerman <m.bannerman@gmail.com>
+
+    This program is free software: you can redistribute it and/or
+    modify it under the terms of the GNU General Public License
+    version 3 as published by the Free Software Foundation.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#pragma once
+
+#include <boost/numeric/ublas/lu.hpp>
+#include <boost/numeric/ublas/matrix.hpp>
+#include <boost/numeric/ublas/triangular.hpp>
+#include <boost/numeric/ublas/vector.hpp>
+#include <boost/numeric/ublas/vector_proxy.hpp>
+#include <exception>
+
+namespace ublas = boost::numeric::ublas;
+
+class Spline : private std::vector<std::pair<double, double> > {
+ public:
+  // The boundary conditions available
+  enum BC_type { FIXED_1ST_DERIV_BC, FIXED_2ND_DERIV_BC, PARABOLIC_RUNOUT_BC };
+
+  enum Spline_type { LINEAR, CUBIC };
+
+  // Constructor takes the boundary conditions as arguments, this
+  // sets the first derivative (gradient) at the lower and upper
+  // end points
+  Spline()
+      : _valid(false),
+        _BCLow(FIXED_2ND_DERIV_BC),
+        _BCHigh(FIXED_2ND_DERIV_BC),
+        _BCLowVal(0),
+        _BCHighVal(0),
+        _type(CUBIC) {}
+
+  typedef std::vector<std::pair<double, double> > base;
+  typedef base::const_iterator const_iterator;
+
+  // Standard STL read-only container stuff
+  const_iterator begin() const {
+    return base::begin();
+  }
+  const_iterator end() const {
+    return base::end();
+  }
+  void clear() {
+    _valid = false;
+    base::clear();
+    _data.clear();
+  }
+  size_t size() const {
+    return base::size();
+  }
+  size_t max_size() const {
+    return base::max_size();
+  }
+  size_t capacity() const {
+    return base::capacity();
+  }
+  bool empty() const {
+    return base::empty();
+  }
+
+  // Add a point to the spline, and invalidate it so its
+  // recalculated on the next access
+  inline void addPoint(double x, double y) {
+    _valid = false;
+    base::push_back(std::pair<double, double>(x, y));
+  }
+
+  // Reset the boundary conditions
+  inline void setLowBC(BC_type BC, double val = 0) {
+    _BCLow = BC;
+    _BCLowVal = val;
+    _valid = false;
+  }
+
+  inline void setHighBC(BC_type BC, double val = 0) {
+    _BCHigh = BC;
+    _BCHighVal = val;
+    _valid = false;
+  }
+
+  void setType(Spline_type type) {
+    _type = type;
+    _valid = false;
+  }
+
+  // Check if the spline has been calculated, then generate the
+  // spline interpolated value
+  double operator()(double xval) {
+    if (!_valid)
+      generate();
+
+    // Special cases when we're outside the range of the spline points
+    if (xval <= x(0))
+      return lowCalc(xval);
+    if (xval >= x(size() - 1))
+      return highCalc(xval);
+
+    // Check all intervals except the last one
+    for (std::vector<SplineData>::const_iterator iPtr = _data.begin();
+         iPtr != _data.end() - 1; ++iPtr)
+      if ((xval >= iPtr->x) && (xval <= (iPtr + 1)->x))
+        return splineCalc(iPtr, xval);
+
+    return splineCalc(_data.end() - 1, xval);
+  }
+
+ private:
+  ///////PRIVATE DATA MEMBERS
+  struct SplineData {
+    double x, a, b, c, d;
+  };
+  // vector of calculated spline data
+  std::vector<SplineData> _data;
+  // Second derivative at each point
+  ublas::vector<double> _ddy;
+  // Tracks whether the spline parameters have been calculated for
+  // the current set of points
+  bool _valid;
+  // The boundary conditions
+  BC_type _BCLow, _BCHigh;
+  // The values of the boundary conditions
+  double _BCLowVal, _BCHighVal;
+
+  Spline_type _type;
+
+  ///////PRIVATE FUNCTIONS
+  // Function to calculate the value of a given spline at a point xval
+  inline double splineCalc(
+      std::vector<SplineData>::const_iterator i, double xval) {
+    const double lx = xval - i->x;
+    return ((i->a * lx + i->b) * lx + i->c) * lx + i->d;
+  }
+
+  inline double lowCalc(double xval) {
+    const double lx = xval - x(0);
+
+    if (_type == LINEAR)
+      return lx * _BCHighVal + y(0);
+
+    const double firstDeriv =
+        (y(1) - y(0)) / h(0) - 2 * h(0) * (_data[0].b + 2 * _data[1].b) / 6;
+
+    switch (_BCLow) {
+      case FIXED_1ST_DERIV_BC:
+        return lx * _BCLowVal + y(0);
+      case FIXED_2ND_DERIV_BC:
+        return lx * lx * _BCLowVal + firstDeriv * lx + y(0);
+      case PARABOLIC_RUNOUT_BC:
+        return lx * lx * _ddy[0] + lx * firstDeriv + y(0);
+    }
+    throw std::runtime_error("Unknown BC");
+  }
+
+  inline double highCalc(double xval) {
+    const double lx = xval - x(size() - 1);
+
+    if (_type == LINEAR)
+      return lx * _BCHighVal + y(size() - 1);
+
+    const double firstDeriv =
+        2 * h(size() - 2) * (_ddy[size() - 2] + 2 * _ddy[size() - 1]) / 6 +
+        (y(size() - 1) - y(size() - 2)) / h(size() - 2);
+
+    switch (_BCHigh) {
+      case FIXED_1ST_DERIV_BC:
+        return lx * _BCHighVal + y(size() - 1);
+      case FIXED_2ND_DERIV_BC:
+        return lx * lx * _BCHighVal + firstDeriv * lx + y(size() - 1);
+      case PARABOLIC_RUNOUT_BC:
+        return lx * lx * _ddy[size() - 1] + lx * firstDeriv + y(size() - 1);
+    }
+    throw std::runtime_error("Unknown BC");
+  }
+
+  // These just provide access to the point data in a clean way
+  inline double x(size_t i) const {
+    return operator[](i).first;
+  }
+  inline double y(size_t i) const {
+    return operator[](i).second;
+  }
+  inline double h(size_t i) const {
+    return x(i + 1) - x(i);
+  }
+
+  // Invert a arbitrary matrix using the boost ublas library
+  template <class T>
+  bool InvertMatrix(ublas::matrix<T> A, ublas::matrix<T> &inverse) {
+    using namespace ublas;
+
+    // create a permutation matrix for the LU-factorization
+    permutation_matrix<std::size_t> pm(A.size1());
+
+    // perform LU-factorization
+    int res = lu_factorize(A, pm);
+    if (res != 0)
+      return false;
+
+    // create identity matrix of "inverse"
+    inverse.assign(ublas::identity_matrix<T>(A.size1()));
+
+    // backsubstitute to get the inverse
+    lu_substitute(A, pm, inverse);
+
+    return true;
+  }
+
+  // This function will recalculate the spline parameters and store
+  // them in _data, ready for spline interpolation
+  void generate() {
+    if (size() < 2)
+      throw std::runtime_error("Spline requires at least 2 points");
+
+    // If any spline points are at the same x location, we have to
+    // just slightly seperate them
+    {
+      bool testPassed(false);
+      while (!testPassed) {
+        testPassed = true;
+        std::sort(base::begin(), base::end());
+
+        for (base::iterator iPtr = base::begin(); iPtr != base::end() - 1;
+             ++iPtr)
+          if (iPtr->first == (iPtr + 1)->first) {
+            if ((iPtr + 1)->first != 0)
+              (iPtr + 1)->first += (iPtr + 1)->first *
+                                   std::numeric_limits<double>::epsilon() * 10;
+            else
+              (iPtr + 1)->first = std::numeric_limits<double>::epsilon() * 10;
+            testPassed = false;
+            break;
+          }
+      }
+    }
+
+    const size_t e = size() - 1;
+
+    switch (_type) {
+      case LINEAR: {
+        _data.resize(e);
+        for (size_t i(0); i < e; ++i) {
+          _data[i].x = x(i);
+          _data[i].a = 0;
+          _data[i].b = 0;
+          _data[i].c = (y(i + 1) - y(i)) / (x(i + 1) - x(i));
+          _data[i].d = y(i);
+        }
+        break;
+      }
+      case CUBIC: {
+        ublas::matrix<double> A(size(), size());
+        for (size_t yv(0); yv <= e; ++yv)
+          for (size_t xv(0); xv <= e; ++xv)
+            A(xv, yv) = 0;
+
+        for (size_t i(1); i < e; ++i) {
+          A(i - 1, i) = h(i - 1);
+          A(i, i) = 2 * (h(i - 1) + h(i));
+          A(i + 1, i) = h(i);
+        }
+
+        ublas::vector<double> C(size());
+        for (size_t xv(0); xv <= e; ++xv)
+          C(xv) = 0;
+
+        for (size_t i(1); i < e; ++i)
+          C(i) = 6 * ((y(i + 1) - y(i)) / h(i) - (y(i) - y(i - 1)) / h(i - 1));
+
+        // Boundary conditions
+        switch (_BCLow) {
+          case FIXED_1ST_DERIV_BC:
+            C(0) = 6 * ((y(1) - y(0)) / h(0) - _BCLowVal);
+            A(0, 0) = 2 * h(0);
+            A(1, 0) = h(0);
+            break;
+          case FIXED_2ND_DERIV_BC:
+            C(0) = _BCLowVal;
+            A(0, 0) = 1;
+            break;
+          case PARABOLIC_RUNOUT_BC:
+            C(0) = 0;
+            A(0, 0) = 1;
+            A(1, 0) = -1;
+            break;
+        }
+
+        switch (_BCHigh) {
+          case FIXED_1ST_DERIV_BC:
+            C(e) = 6 * (_BCHighVal - (y(e) - y(e - 1)) / h(e - 1));
+            A(e, e) = 2 * h(e - 1);
+            A(e - 1, e) = h(e - 1);
+            break;
+          case FIXED_2ND_DERIV_BC:
+            C(e) = _BCHighVal;
+            A(e, e) = 1;
+            break;
+          case PARABOLIC_RUNOUT_BC:
+            C(e) = 0;
+            A(e, e) = 1;
+            A(e - 1, e) = -1;
+            break;
+        }
+
+        ublas::matrix<double> AInv(size(), size());
+        InvertMatrix(A, AInv);
+
+        _ddy = ublas::prod(C, AInv);
+
+        _data.resize(size() - 1);
+        for (size_t i(0); i < e; ++i) {
+          _data[i].x = x(i);
+          _data[i].a = (_ddy(i + 1) - _ddy(i)) / (6 * h(i));
+          _data[i].b = _ddy(i) / 2;
+          _data[i].c = (y(i + 1) - y(i)) / h(i) - _ddy(i + 1) * h(i) / 6 -
+                       _ddy(i) * h(i) / 3;
+          _data[i].d = y(i);
+        }
+      }
+    }
+    _valid = true;
+  }
+};

src/mynteye/api/camodocal/include/camodocal/gpl/EigenQuaternionParameterization.h

@@ -0,0 +1,36 @@
+#ifndef EIGENQUATERNIONPARAMETERIZATION_H
+#define EIGENQUATERNIONPARAMETERIZATION_H
+
+#include "ceres/local_parameterization.h"
+
+namespace camodocal {
+
+class EigenQuaternionParameterization : public ceres::LocalParameterization {
+ public:
+  virtual ~EigenQuaternionParameterization() {}
+  virtual bool Plus(
+      const double *x, const double *delta, double *x_plus_delta) const;
+  virtual bool ComputeJacobian(const double *x, double *jacobian) const;
+  virtual int GlobalSize() const {
+    return 4;
+  }
+  virtual int LocalSize() const {
+    return 3;
+  }
+
+ private:
+  template <typename T>
+  void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const;
+};
+
+template <typename T>
+void EigenQuaternionParameterization::EigenQuaternionProduct(
+    const T z[4], const T w[4], T zw[4]) const {
+  zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1];
+  zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0];
+  zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3];
+  zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2];
+}
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/gpl/EigenUtils.h

@@ -0,0 +1,394 @@
+#ifndef EIGENUTILS_H
+#define EIGENUTILS_H
+
+#include "eigen3/Eigen/Dense"
+
+#include "camodocal/gpl/gpl.h"
+#include "ceres/rotation.h"
+
+namespace camodocal {
+
+// Returns the 3D cross product skew symmetric matrix of a given 3D vector
+template <typename T>
+Eigen::Matrix<T, 3, 3> skew(const Eigen::Matrix<T, 3, 1> &vec) {
+  return (Eigen::Matrix<T, 3, 3>() << T(0), -vec(2), vec(1), vec(2), T(0),
+          -vec(0), -vec(1), vec(0), T(0))
+      .finished();
+}
+
+template <typename Derived>
+typename Eigen::MatrixBase<Derived>::PlainObject sqrtm(
+    const Eigen::MatrixBase<Derived> &A) {
+  Eigen::SelfAdjointEigenSolver<typename Derived::PlainObject> es(A);
+
+  return es.operatorSqrt();
+}
+
+template <typename T>
+Eigen::Matrix<T, 3, 3> AngleAxisToRotationMatrix(
+    const Eigen::Matrix<T, 3, 1> &rvec) {
+  T angle = rvec.norm();
+  if (angle == T(0)) {
+    return Eigen::Matrix<T, 3, 3>::Identity();
+  }
+
+  Eigen::Matrix<T, 3, 1> axis;
+  axis = rvec.normalized();
+
+  Eigen::Matrix<T, 3, 3> rmat;
+  rmat = Eigen::AngleAxis<T>(angle, axis);
+
+  return rmat;
+}
+
+template <typename T>
+Eigen::Quaternion<T> AngleAxisToQuaternion(const Eigen::Matrix<T, 3, 1> &rvec) {
+  Eigen::Matrix<T, 3, 3> rmat = AngleAxisToRotationMatrix<T>(rvec);
+
+  return Eigen::Quaternion<T>(rmat);
+}
+
+template <typename T>
+void AngleAxisToQuaternion(const Eigen::Matrix<T, 3, 1> &rvec, T *q) {
+  Eigen::Quaternion<T> quat = AngleAxisToQuaternion<T>(rvec);
+
+  q[0] = quat.x();
+  q[1] = quat.y();
+  q[2] = quat.z();
+  q[3] = quat.w();
+}
+
+template <typename T>
+Eigen::Matrix<T, 3, 1> RotationToAngleAxis(const Eigen::Matrix<T, 3, 3> &rmat) {
+  Eigen::AngleAxis<T> angleaxis;
+  angleaxis.fromRotationMatrix(rmat);
+  return angleaxis.angle() * angleaxis.axis();
+}
+
+template <typename T>
+void QuaternionToAngleAxis(const T *const q, Eigen::Matrix<T, 3, 1> &rvec) {
+  Eigen::Quaternion<T> quat(q[3], q[0], q[1], q[2]);
+
+  Eigen::Matrix<T, 3, 3> rmat = quat.toRotationMatrix();
+
+  Eigen::AngleAxis<T> angleaxis;
+  angleaxis.fromRotationMatrix(rmat);
+
+  rvec = angleaxis.angle() * angleaxis.axis();
+}
+
+template <typename T>
+Eigen::Matrix<T, 3, 3> QuaternionToRotation(const T *const q) {
+  T R[9];
+  ceres::QuaternionToRotation(q, R);
+
+  Eigen::Matrix<T, 3, 3> rmat;
+  for (int i = 0; i < 3; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      rmat(i, j) = R[i * 3 + j];
+    }
+  }
+
+  return rmat;
+}
+
+template <typename T>
+void QuaternionToRotation(const T *const q, T *rot) {
+  ceres::QuaternionToRotation(q, rot);
+}
+
+template <typename T>
+Eigen::Matrix<T, 4, 4> QuaternionMultMatLeft(const Eigen::Quaternion<T> &q) {
+  return (Eigen::Matrix<T, 4, 4>() << q.w(), -q.z(), q.y(), q.x(), q.z(), q.w(),
+          -q.x(), q.y(), -q.y(), q.x(), q.w(), q.z(), -q.x(), -q.y(), -q.z(),
+          q.w())
+      .finished();
+}
+
+template <typename T>
+Eigen::Matrix<T, 4, 4> QuaternionMultMatRight(const Eigen::Quaternion<T> &q) {
+  return (Eigen::Matrix<T, 4, 4>() << q.w(), q.z(), -q.y(), q.x(), -q.z(),
+          q.w(), q.x(), q.y(), q.y(), -q.x(), q.w(), q.z(), -q.x(), -q.y(),
+          -q.z(), q.w())
+      .finished();
+}
+
+/// @param theta - rotation about screw axis
+/// @param d - projection of tvec on the rotation axis
+/// @param l - screw axis direction
+/// @param m - screw axis moment
+template <typename T>
+void AngleAxisAndTranslationToScrew(
+    const Eigen::Matrix<T, 3, 1> &rvec, const Eigen::Matrix<T, 3, 1> &tvec,
+    T &theta, T &d, Eigen::Matrix<T, 3, 1> &l, Eigen::Matrix<T, 3, 1> &m) {
+  theta = rvec.norm();
+  if (theta == 0) {
+    l.setZero();
+    m.setZero();
+    std::cout << "Warning: Undefined screw! Returned 0. " << std::endl;
+  }
+
+  l = rvec.normalized();
+
+  Eigen::Matrix<T, 3, 1> t = tvec;
+
+  d = t.transpose() * l;
+
+  // point on screw axis - projection of origin on screw axis
+  Eigen::Matrix<T, 3, 1> c;
+  c = 0.5 * (t - d * l + (1.0 / tan(theta / 2.0) * l).cross(t));
+
+  // c and hence the screw axis is not defined if theta is either 0 or M_PI
+  m = c.cross(l);
+}
+
+template <typename T>
+Eigen::Matrix<T, 3, 3> RPY2mat(T roll, T pitch, T yaw) {
+  Eigen::Matrix<T, 3, 3> m;
+
+  T cr = cos(roll);
+  T sr = sin(roll);
+  T cp = cos(pitch);
+  T sp = sin(pitch);
+  T cy = cos(yaw);
+  T sy = sin(yaw);
+
+  m(0, 0) = cy * cp;
+  m(0, 1) = cy * sp * sr - sy * cr;
+  m(0, 2) = cy * sp * cr + sy * sr;
+  m(1, 0) = sy * cp;
+  m(1, 1) = sy * sp * sr + cy * cr;
+  m(1, 2) = sy * sp * cr - cy * sr;
+  m(2, 0) = -sp;
+  m(2, 1) = cp * sr;
+  m(2, 2) = cp * cr;
+  return m;
+}
+
+template <typename T>
+void mat2RPY(const Eigen::Matrix<T, 3, 3> &m, T &roll, T &pitch, T &yaw) {
+  roll = atan2(m(2, 1), m(2, 2));
+  pitch = atan2(-m(2, 0), sqrt(m(2, 1) * m(2, 1) + m(2, 2) * m(2, 2)));
+  yaw = atan2(m(1, 0), m(0, 0));
+}
+
+template <typename T>
+Eigen::Matrix<T, 4, 4> homogeneousTransform(
+    const Eigen::Matrix<T, 3, 3> &R, const Eigen::Matrix<T, 3, 1> &t) {
+  Eigen::Matrix<T, 4, 4> H;
+  H.setIdentity();
+
+  H.block(0, 0, 3, 3) = R;
+  H.block(0, 3, 3, 1) = t;
+
+  return H;
+}
+
+template <typename T>
+Eigen::Matrix<T, 4, 4> poseWithCartesianTranslation(
+    const T *const q, const T *const p) {
+  Eigen::Matrix<T, 4, 4> pose = Eigen::Matrix<T, 4, 4>::Identity();
+
+  T rotation[9];
+  ceres::QuaternionToRotation(q, rotation);
+  for (int i = 0; i < 3; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      pose(i, j) = rotation[i * 3 + j];
+    }
+  }
+
+  pose(0, 3) = p[0];
+  pose(1, 3) = p[1];
+  pose(2, 3) = p[2];
+
+  return pose;
+}
+
+template <typename T>
+Eigen::Matrix<T, 4, 4> poseWithSphericalTranslation(
+    const T *const q, const T *const p, const T scale = T(1.0)) {
+  Eigen::Matrix<T, 4, 4> pose = Eigen::Matrix<T, 4, 4>::Identity();
+
+  T rotation[9];
+  ceres::QuaternionToRotation(q, rotation);
+  for (int i = 0; i < 3; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      pose(i, j) = rotation[i * 3 + j];
+    }
+  }
+
+  T theta = p[0];
+  T phi = p[1];
+  pose(0, 3) = sin(theta) * cos(phi) * scale;
+  pose(1, 3) = sin(theta) * sin(phi) * scale;
+  pose(2, 3) = cos(theta) * scale;
+
+  return pose;
+}
+
+// Returns the Sampson error of a given essential matrix and 2 image points
+template <typename T>
+T sampsonError(
+    const Eigen::Matrix<T, 3, 3> &E, const Eigen::Matrix<T, 3, 1> &p1,
+    const Eigen::Matrix<T, 3, 1> &p2) {
+  Eigen::Matrix<T, 3, 1> Ex1 = E * p1;
+  Eigen::Matrix<T, 3, 1> Etx2 = E.transpose() * p2;
+
+  T x2tEx1 = p2.dot(Ex1);
+
+  // compute Sampson error
+  T err = square(x2tEx1) / (square(Ex1(0, 0)) + square(Ex1(1, 0)) +
+                            square(Etx2(0, 0)) + square(Etx2(1, 0)));
+
+  return err;
+}
+
+// Returns the Sampson error of a given rotation/translation and 2 image points
+template <typename T>
+T sampsonError(
+    const Eigen::Matrix<T, 3, 3> &R, const Eigen::Matrix<T, 3, 1> &t,
+    const Eigen::Matrix<T, 3, 1> &p1, const Eigen::Matrix<T, 3, 1> &p2) {
+  // construct essential matrix
+  Eigen::Matrix<T, 3, 3> E = skew(t) * R;
+
+  Eigen::Matrix<T, 3, 1> Ex1 = E * p1;
+  Eigen::Matrix<T, 3, 1> Etx2 = E.transpose() * p2;
+
+  T x2tEx1 = p2.dot(Ex1);
+
+  // compute Sampson error
+  T err = square(x2tEx1) / (square(Ex1(0, 0)) + square(Ex1(1, 0)) +
+                            square(Etx2(0, 0)) + square(Etx2(1, 0)));
+
+  return err;
+}
+
+// Returns the Sampson error of a given rotation/translation and 2 image points
+template <typename T>
+T sampsonError(
+    const Eigen::Matrix<T, 4, 4> &H, const Eigen::Matrix<T, 3, 1> &p1,
+    const Eigen::Matrix<T, 3, 1> &p2) {
+  Eigen::Matrix<T, 3, 3> R = H.block(0, 0, 3, 3);
+  Eigen::Matrix<T, 3, 1> t = H.block(0, 3, 3, 1);
+
+  return sampsonError(R, t, p1, p2);
+}
+
+template <typename T>
+Eigen::Matrix<T, 3, 1> transformPoint(
+    const Eigen::Matrix<T, 4, 4> &H, const Eigen::Matrix<T, 3, 1> &P) {
+  Eigen::Matrix<T, 3, 1> P_trans =
+      H.block(0, 0, 3, 3) * P + H.block(0, 3, 3, 1);
+
+  return P_trans;
+}
+
+template <typename T>
+Eigen::Matrix<T, 4, 4> estimate3DRigidTransform(
+    const std::vector<Eigen::Matrix<T, 3, 1>,
+                      Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >
+        &points1,
+    const std::vector<Eigen::Matrix<T, 3, 1>,
+                      Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >
+        &points2) {
+  // compute centroids
+  Eigen::Matrix<T, 3, 1> c1, c2;
+  c1.setZero();
+  c2.setZero();
+
+  for (size_t i = 0; i < points1.size(); ++i) {
+    c1 += points1.at(i);
+    c2 += points2.at(i);
+  }
+
+  c1 /= points1.size();
+  c2 /= points1.size();
+
+  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> X(3, points1.size());
+  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Y(3, points1.size());
+  for (size_t i = 0; i < points1.size(); ++i) {
+    X.col(i) = points1.at(i) - c1;
+    Y.col(i) = points2.at(i) - c2;
+  }
+
+  Eigen::Matrix<T, 3, 3> H = X * Y.transpose();
+
+  Eigen::JacobiSVD<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> > svd(
+      H, Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+  Eigen::Matrix<T, 3, 3> U = svd.matrixU();
+  Eigen::Matrix<T, 3, 3> V = svd.matrixV();
+  if (U.determinant() * V.determinant() < 0.0) {
+    V.col(2) *= -1.0;
+  }
+
+  Eigen::Matrix<T, 3, 3> R = V * U.transpose();
+  Eigen::Matrix<T, 3, 1> t = c2 - R * c1;
+
+  return homogeneousTransform(R, t);
+}
+
+template <typename T>
+Eigen::Matrix<T, 4, 4> estimate3DRigidSimilarityTransform(
+    const std::vector<Eigen::Matrix<T, 3, 1>,
+                      Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >
+        &points1,
+    const std::vector<Eigen::Matrix<T, 3, 1>,
+                      Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >
+        &points2) {
+  // compute centroids
+  Eigen::Matrix<T, 3, 1> c1, c2;
+  c1.setZero();
+  c2.setZero();
+
+  for (size_t i = 0; i < points1.size(); ++i) {
+    c1 += points1.at(i);
+    c2 += points2.at(i);
+  }
+
+  c1 /= points1.size();
+  c2 /= points1.size();
+
+  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> X(3, points1.size());
+  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Y(3, points1.size());
+  for (size_t i = 0; i < points1.size(); ++i) {
+    X.col(i) = points1.at(i) - c1;
+    Y.col(i) = points2.at(i) - c2;
+  }
+
+  Eigen::Matrix<T, 3, 3> H = X * Y.transpose();
+
+  Eigen::JacobiSVD<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> > svd(
+      H, Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+  Eigen::Matrix<T, 3, 3> U = svd.matrixU();
+  Eigen::Matrix<T, 3, 3> V = svd.matrixV();
+  if (U.determinant() * V.determinant() < 0.0) {
+    V.col(2) *= -1.0;
+  }
+
+  Eigen::Matrix<T, 3, 3> R = V * U.transpose();
+
+  std::vector<Eigen::Matrix<T, 3, 1>,
+              Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >
+      rotatedPoints1(points1.size());
+  for (size_t i = 0; i < points1.size(); ++i) {
+    rotatedPoints1.at(i) = R * (points1.at(i) - c1);
+  }
+
+  double sum_ss = 0.0, sum_tt = 0.0;
+  for (size_t i = 0; i < points1.size(); ++i) {
+    sum_ss += (points1.at(i) - c1).squaredNorm();
+    sum_tt += (points2.at(i) - c2).dot(rotatedPoints1.at(i));
+  }
+
+  double scale = sum_tt / sum_ss;
+
+  Eigen::Matrix<T, 3, 3> sR = scale * R;
+  Eigen::Matrix<T, 3, 1> t = c2 - sR * c1;
+
+  return homogeneousTransform(sR, t);
+}
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/gpl/gpl.h

@@ -0,0 +1,100 @@
+#ifndef GPL_H
+#define GPL_H
+
+#include <algorithm>
+#include <cmath>
+#include <opencv2/core/core.hpp>
+
+namespace camodocal {
+
+template <class T>
+const T clamp(const T &v, const T &a, const T &b) {
+  return std::min(b, std::max(a, v));
+}
+
+double hypot3(double x, double y, double z);
+float hypot3f(float x, float y, float z);
+
+template <class T>
+const T normalizeTheta(const T &theta) {
+  T normTheta = theta;
+
+  while (normTheta < -M_PI) {
+    normTheta += 2.0 * M_PI;
+  }
+  while (normTheta > M_PI) {
+    normTheta -= 2.0 * M_PI;
+  }
+
+  return normTheta;
+}
+
+double d2r(double deg);
+float d2r(float deg);
+double r2d(double rad);
+float r2d(float rad);
+
+double sinc(double theta);
+
+template <class T>
+const T square(const T &x) {
+  return x * x;
+}
+
+template <class T>
+const T cube(const T &x) {
+  return x * x * x;
+}
+
+template <class T>
+const T random(const T &a, const T &b) {
+  return static_cast<double>(rand()) / RAND_MAX * (b - a) + a;
+}
+
+template <class T>
+const T randomNormal(const T &sigma) {
+  T x1, x2, w;
+
+  do {
+    x1 = 2.0 * random(0.0, 1.0) - 1.0;
+    x2 = 2.0 * random(0.0, 1.0) - 1.0;
+    w = x1 * x1 + x2 * x2;
+  } while (w >= 1.0 || w == 0.0);
+
+  w = sqrt((-2.0 * log(w)) / w);
+
+  return x1 * w * sigma;
+}
+
+unsigned long long timeInMicroseconds(void);
+
+double timeInSeconds(void);
+
+void colorDepthImage(
+    cv::Mat &imgDepth, cv::Mat &imgColoredDepth, float minRange,
+    float maxRange);
+
+bool colormap(
+    const std::string &name, unsigned char idx, float &r, float &g, float &b);
+
+std::vector<cv::Point2i> bresLine(int x0, int y0, int x1, int y1);
+std::vector<cv::Point2i> bresCircle(int x0, int y0, int r);
+
+void fitCircle(
+    const std::vector<cv::Point2d> &points, double &centerX, double &centerY,
+    double &radius);
+
+std::vector<cv::Point2d> intersectCircles(
+    double x1, double y1, double r1, double x2, double y2, double r2);
+
+void LLtoUTM(
+    double latitude, double longitude, double &utmNorthing, double &utmEasting,
+    std::string &utmZone);
+void UTMtoLL(
+    double utmNorthing, double utmEasting, const std::string &utmZone,
+    double &latitude, double &longitude);
+
+long int timestampDiff(uint64_t t1, uint64_t t2);
+}
+
+#endif

src/mynteye/api/camodocal/include/camodocal/sparse_graph/Transform.h

@@ -0,0 +1,35 @@
+#ifndef TRANSFORM_H
+#define TRANSFORM_H
+
+#include <boost/shared_ptr.hpp>
+#include "eigen3/Eigen/Dense"
+#include <stdint.h>
+
+namespace camodocal {
+
+class Transform {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  Transform();
+  Transform(const Eigen::Matrix4d &H);
+
+  Eigen::Quaterniond &rotation(void);
+  const Eigen::Quaterniond &rotation(void) const;
+  double *rotationData(void);
+  const double *const rotationData(void) const;
+
+  Eigen::Vector3d &translation(void);
+  const Eigen::Vector3d &translation(void) const;
+  double *translationData(void);
+  const double *const translationData(void) const;
+
+  Eigen::Matrix4d toMatrix(void) const;
+
+ private:
+  Eigen::Quaterniond m_q;
+  Eigen::Vector3d m_t;
+};
+}
+
+#endif

src/mynteye/api/camodocal/src/calib/CameraCalibration.cc

@@ -0,0 +1,493 @@
+#include "camodocal/calib/CameraCalibration.h"
+
+#include <algorithm>
+#include <cstdio>
+#include "eigen3/Eigen/Dense"
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/core/eigen.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include "camodocal/camera_models/CameraFactory.h"
+#include "camodocal/camera_models/CostFunctionFactory.h"
+#include "camodocal/gpl/EigenQuaternionParameterization.h"
+#include "camodocal/gpl/EigenUtils.h"
+#include "camodocal/sparse_graph/Transform.h"
+
+#include "ceres/ceres.h"
+namespace camodocal {
+
+CameraCalibration::CameraCalibration()
+    : m_boardSize(cv::Size(0, 0)), m_squareSize(0.0f), m_verbose(false) {}
+
+CameraCalibration::CameraCalibration(
+    const Camera::ModelType modelType, const std::string &cameraName,
+    const cv::Size &imageSize, const cv::Size &boardSize, float squareSize)
+    : m_boardSize(boardSize), m_squareSize(squareSize), m_verbose(false) {
+  m_camera = CameraFactory::instance()->generateCamera(
+      modelType, cameraName, imageSize);
+}
+
+void CameraCalibration::clear(void) {
+  m_imagePoints.clear();
+  m_scenePoints.clear();
+}
+
+void CameraCalibration::addChessboardData(
+    const std::vector<cv::Point2f> &corners) {
+  m_imagePoints.push_back(corners);
+
+  std::vector<cv::Point3f> scenePointsInView;
+  for (int i = 0; i < m_boardSize.height; ++i) {
+    for (int j = 0; j < m_boardSize.width; ++j) {
+      scenePointsInView.push_back(
+          cv::Point3f(i * m_squareSize, j * m_squareSize, 0.0));
+    }
+  }
+  m_scenePoints.push_back(scenePointsInView);
+}
+
+bool CameraCalibration::calibrate(void) {
+  int imageCount = m_imagePoints.size();
+
+  // compute intrinsic camera parameters and extrinsic parameters for each of
+  // the views
+  std::vector<cv::Mat> rvecs;
+  std::vector<cv::Mat> tvecs;
+  bool ret = calibrateHelper(m_camera, rvecs, tvecs);
+
+  m_cameraPoses = cv::Mat(imageCount, 6, CV_64F);
+  for (int i = 0; i < imageCount; ++i) {
+    m_cameraPoses.at<double>(i, 0) = rvecs.at(i).at<double>(0);
+    m_cameraPoses.at<double>(i, 1) = rvecs.at(i).at<double>(1);
+    m_cameraPoses.at<double>(i, 2) = rvecs.at(i).at<double>(2);
+    m_cameraPoses.at<double>(i, 3) = tvecs.at(i).at<double>(0);
+    m_cameraPoses.at<double>(i, 4) = tvecs.at(i).at<double>(1);
+    m_cameraPoses.at<double>(i, 5) = tvecs.at(i).at<double>(2);
+  }
+
+  // Compute measurement covariance.
+  std::vector<std::vector<cv::Point2f> > errVec(m_imagePoints.size());
+  Eigen::Vector2d errSum = Eigen::Vector2d::Zero();
+  size_t errCount = 0;
+  for (size_t i = 0; i < m_imagePoints.size(); ++i) {
+    std::vector<cv::Point2f> estImagePoints;
+    m_camera->projectPoints(
+        m_scenePoints.at(i), rvecs.at(i), tvecs.at(i), estImagePoints);
+
+    for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) {
+      cv::Point2f pObs = m_imagePoints.at(i).at(j);
+      cv::Point2f pEst = estImagePoints.at(j);
+
+      cv::Point2f err = pObs - pEst;
+
+      errVec.at(i).push_back(err);
+
+      errSum += Eigen::Vector2d(err.x, err.y);
+    }
+
+    errCount += m_imagePoints.at(i).size();
+  }
+
+  Eigen::Vector2d errMean = errSum / static_cast<double>(errCount);
+
+  Eigen::Matrix2d measurementCovariance = Eigen::Matrix2d::Zero();
+  for (size_t i = 0; i < errVec.size(); ++i) {
+    for (size_t j = 0; j < errVec.at(i).size(); ++j) {
+      cv::Point2f err = errVec.at(i).at(j);
+      double d0 = err.x - errMean(0);
+      double d1 = err.y - errMean(1);
+
+      measurementCovariance(0, 0) += d0 * d0;
+      measurementCovariance(0, 1) += d0 * d1;
+      measurementCovariance(1, 1) += d1 * d1;
+    }
+  }
+  measurementCovariance /= static_cast<double>(errCount);
+  measurementCovariance(1, 0) = measurementCovariance(0, 1);
+
+  m_measurementCovariance = measurementCovariance;
+
+  return ret;
+}
+
+int CameraCalibration::sampleCount(void) const {
+  return m_imagePoints.size();
+}
+
+std::vector<std::vector<cv::Point2f> > &CameraCalibration::imagePoints(void) {
+  return m_imagePoints;
+}
+
+const std::vector<std::vector<cv::Point2f> > &CameraCalibration::imagePoints(
+    void) const {
+  return m_imagePoints;
+}
+
+std::vector<std::vector<cv::Point3f> > &CameraCalibration::scenePoints(void) {
+  return m_scenePoints;
+}
+
+const std::vector<std::vector<cv::Point3f> > &CameraCalibration::scenePoints(
+    void) const {
+  return m_scenePoints;
+}
+
+CameraPtr &CameraCalibration::camera(void) {
+  return m_camera;
+}
+
+const CameraConstPtr CameraCalibration::camera(void) const {
+  return m_camera;
+}
+
+Eigen::Matrix2d &CameraCalibration::measurementCovariance(void) {
+  return m_measurementCovariance;
+}
+
+const Eigen::Matrix2d &CameraCalibration::measurementCovariance(void) const {
+  return m_measurementCovariance;
+}
+
+cv::Mat &CameraCalibration::cameraPoses(void) {
+  return m_cameraPoses;
+}
+
+const cv::Mat &CameraCalibration::cameraPoses(void) const {
+  return m_cameraPoses;
+}
+
+void CameraCalibration::drawResults(std::vector<cv::Mat> &images) const {
+  std::vector<cv::Mat> rvecs, tvecs;
+
+  for (size_t i = 0; i < images.size(); ++i) {
+    cv::Mat rvec(3, 1, CV_64F);
+    rvec.at<double>(0) = m_cameraPoses.at<double>(i, 0);
+    rvec.at<double>(1) = m_cameraPoses.at<double>(i, 1);
+    rvec.at<double>(2) = m_cameraPoses.at<double>(i, 2);
+
+    cv::Mat tvec(3, 1, CV_64F);
+    tvec.at<double>(0) = m_cameraPoses.at<double>(i, 3);
+    tvec.at<double>(1) = m_cameraPoses.at<double>(i, 4);
+    tvec.at<double>(2) = m_cameraPoses.at<double>(i, 5);
+
+    rvecs.push_back(rvec);
+    tvecs.push_back(tvec);
+  }
+
+  int drawShiftBits = 4;
+  int drawMultiplier = 1 << drawShiftBits;
+
+  cv::Scalar green(0, 255, 0);
+  cv::Scalar red(0, 0, 255);
+
+  for (size_t i = 0; i < images.size(); ++i) {
+    cv::Mat &image = images.at(i);
+    if (image.channels() == 1) {
+      cv::cvtColor(image, image, CV_GRAY2RGB);
+    }
+
+    std::vector<cv::Point2f> estImagePoints;
+    m_camera->projectPoints(
+        m_scenePoints.at(i), rvecs.at(i), tvecs.at(i), estImagePoints);
+
+    float errorSum = 0.0f;
+    float errorMax = std::numeric_limits<float>::min();
+
+    for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) {
+      cv::Point2f pObs = m_imagePoints.at(i).at(j);
+      cv::Point2f pEst = estImagePoints.at(j);
+
+      cv::circle(
+          image, cv::Point(
+                     cvRound(pObs.x * drawMultiplier),
+                     cvRound(pObs.y * drawMultiplier)),
+          5, green, 2, CV_AA, drawShiftBits);
+
+      cv::circle(
+          image, cv::Point(
+                     cvRound(pEst.x * drawMultiplier),
+                     cvRound(pEst.y * drawMultiplier)),
+          5, red, 2, CV_AA, drawShiftBits);
+
+      float error = cv::norm(pObs - pEst);
+
+      errorSum += error;
+      if (error > errorMax) {
+        errorMax = error;
+      }
+    }
+
+    std::ostringstream oss;
+    oss << "Reprojection error: avg = " << errorSum / m_imagePoints.at(i).size()
+        << "   max = " << errorMax;
+
+    cv::putText(
+        image, oss.str(), cv::Point(10, image.rows - 10),
+        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, CV_AA);
+  }
+}
+
+void CameraCalibration::writeParams(const std::string &filename) const {
+  m_camera->writeParametersToYamlFile(filename);
+}
+
+bool CameraCalibration::writeChessboardData(const std::string &filename) const {
+  std::ofstream ofs(filename.c_str(), std::ios::out | std::ios::binary);
+  if (!ofs.is_open()) {
+    return false;
+  }
+
+  writeData(ofs, m_boardSize.width);
+  writeData(ofs, m_boardSize.height);
+  writeData(ofs, m_squareSize);
+
+  writeData(ofs, m_measurementCovariance(0, 0));
+  writeData(ofs, m_measurementCovariance(0, 1));
+  writeData(ofs, m_measurementCovariance(1, 0));
+  writeData(ofs, m_measurementCovariance(1, 1));
+
+  writeData(ofs, m_cameraPoses.rows);
+  writeData(ofs, m_cameraPoses.cols);
+  writeData(ofs, m_cameraPoses.type());
+  for (int i = 0; i < m_cameraPoses.rows; ++i) {
+    for (int j = 0; j < m_cameraPoses.cols; ++j) {
+      writeData(ofs, m_cameraPoses.at<double>(i, j));
+    }
+  }
+
+  writeData(ofs, m_imagePoints.size());
+  for (size_t i = 0; i < m_imagePoints.size(); ++i) {
+    writeData(ofs, m_imagePoints.at(i).size());
+    for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) {
+      const cv::Point2f &ipt = m_imagePoints.at(i).at(j);
+
+      writeData(ofs, ipt.x);
+      writeData(ofs, ipt.y);
+    }
+  }
+
+  writeData(ofs, m_scenePoints.size());
+  for (size_t i = 0; i < m_scenePoints.size(); ++i) {
+    writeData(ofs, m_scenePoints.at(i).size());
+    for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j) {
+      const cv::Point3f &spt = m_scenePoints.at(i).at(j);
+
+      writeData(ofs, spt.x);
+      writeData(ofs, spt.y);
+      writeData(ofs, spt.z);
+    }
+  }
+
+  return true;
+}
+
+bool CameraCalibration::readChessboardData(const std::string &filename) {
+  std::ifstream ifs(filename.c_str(), std::ios::in | std::ios::binary);
+  if (!ifs.is_open()) {
+    return false;
+  }
+
+  readData(ifs, m_boardSize.width);
+  readData(ifs, m_boardSize.height);
+  readData(ifs, m_squareSize);
+
+  readData(ifs, m_measurementCovariance(0, 0));
+  readData(ifs, m_measurementCovariance(0, 1));
+  readData(ifs, m_measurementCovariance(1, 0));
+  readData(ifs, m_measurementCovariance(1, 1));
+
+  int rows, cols, type;
+  readData(ifs, rows);
+  readData(ifs, cols);
+  readData(ifs, type);
+  m_cameraPoses = cv::Mat(rows, cols, type);
+
+  for (int i = 0; i < m_cameraPoses.rows; ++i) {
+    for (int j = 0; j < m_cameraPoses.cols; ++j) {
+      readData(ifs, m_cameraPoses.at<double>(i, j));
+    }
+  }
+
+  size_t nImagePointSets;
+  readData(ifs, nImagePointSets);
+
+  m_imagePoints.clear();
+  m_imagePoints.resize(nImagePointSets);
+  for (size_t i = 0; i < m_imagePoints.size(); ++i) {
+    size_t nImagePoints;
+    readData(ifs, nImagePoints);
+    m_imagePoints.at(i).resize(nImagePoints);
+
+    for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) {
+      cv::Point2f &ipt = m_imagePoints.at(i).at(j);
+      readData(ifs, ipt.x);
+      readData(ifs, ipt.y);
+    }
+  }
+
+  size_t nScenePointSets;
+  readData(ifs, nScenePointSets);
+
+  m_scenePoints.clear();
+  m_scenePoints.resize(nScenePointSets);
+  for (size_t i = 0; i < m_scenePoints.size(); ++i) {
+    size_t nScenePoints;
+    readData(ifs, nScenePoints);
+    m_scenePoints.at(i).resize(nScenePoints);
+
+    for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j) {
+      cv::Point3f &spt = m_scenePoints.at(i).at(j);
+      readData(ifs, spt.x);
+      readData(ifs, spt.y);
+      readData(ifs, spt.z);
+    }
+  }
+
+  return true;
+}
+
+void CameraCalibration::setVerbose(bool verbose) {
+  m_verbose = verbose;
+}
+
+bool CameraCalibration::calibrateHelper(
+    CameraPtr &camera, std::vector<cv::Mat> &rvecs,
+    std::vector<cv::Mat> &tvecs) const {
+  rvecs.assign(m_scenePoints.size(), cv::Mat());
+  tvecs.assign(m_scenePoints.size(), cv::Mat());
+
+  // STEP 1: Estimate intrinsics
+  camera->estimateIntrinsics(m_boardSize, m_scenePoints, m_imagePoints);
+
+  // STEP 2: Estimate extrinsics
+  for (size_t i = 0; i < m_scenePoints.size(); ++i) {
+    camera->estimateExtrinsics(
+        m_scenePoints.at(i), m_imagePoints.at(i), rvecs.at(i), tvecs.at(i));
+  }
+
+  if (m_verbose) {
+    std::cout << "[" << camera->cameraName() << "] "
+              << "# INFO: "
+              << "Initial reprojection error: " << std::fixed
+              << std::setprecision(3)
+              << camera->reprojectionError(
+                     m_scenePoints, m_imagePoints, rvecs, tvecs)
+              << " pixels" << std::endl;
+  }
+
+  // STEP 3: optimization using ceres
+  optimize(camera, rvecs, tvecs);
+
+  if (m_verbose) {
+    double err =
+        camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs);
+    std::cout << "[" << camera->cameraName() << "] "
+              << "# INFO: Final reprojection error: " << err << " pixels"
+              << std::endl;
+    std::cout << "[" << camera->cameraName() << "] "
+              << "# INFO: " << camera->parametersToString() << std::endl;
+  }
+
+  return true;
+}
+
+void CameraCalibration::optimize(
+    CameraPtr &camera, std::vector<cv::Mat> &rvecs,
+    std::vector<cv::Mat> &tvecs) const {
+  // Use ceres to do optimization
+  ceres::Problem problem;
+
+  std::vector<Transform, Eigen::aligned_allocator<Transform> > transformVec(
+      rvecs.size());
+  for (size_t i = 0; i < rvecs.size(); ++i) {
+    Eigen::Vector3d rvec;
+    cv::cv2eigen(rvecs.at(i), rvec);
+
+    transformVec.at(i).rotation() =
+        Eigen::AngleAxisd(rvec.norm(), rvec.normalized());
+    transformVec.at(i).translation() << tvecs[i].at<double>(0),
+        tvecs[i].at<double>(1), tvecs[i].at<double>(2);
+  }
+
+  std::vector<double> intrinsicCameraParams;
+  m_camera->writeParameters(intrinsicCameraParams);
+
+  // create residuals for each observation
+  for (size_t i = 0; i < m_imagePoints.size(); ++i) {
+    for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) {
+      const cv::Point3f &spt = m_scenePoints.at(i).at(j);
+      const cv::Point2f &ipt = m_imagePoints.at(i).at(j);
+
+      ceres::CostFunction *costFunction =
+          CostFunctionFactory::instance()->generateCostFunction(
+              camera, Eigen::Vector3d(spt.x, spt.y, spt.z),
+              Eigen::Vector2d(ipt.x, ipt.y), CAMERA_INTRINSICS | CAMERA_POSE);
+
+      ceres::LossFunction *lossFunction = new ceres::CauchyLoss(1.0);
+      problem.AddResidualBlock(
+          costFunction, lossFunction, intrinsicCameraParams.data(),
+          transformVec.at(i).rotationData(),
+          transformVec.at(i).translationData());
+    }
+
+    ceres::LocalParameterization *quaternionParameterization =
+        new EigenQuaternionParameterization;
+
+    problem.SetParameterization(
+        transformVec.at(i).rotationData(), quaternionParameterization);
+  }
+
+  std::cout << "begin ceres" << std::endl;
+  ceres::Solver::Options options;
+  options.max_num_iterations = 1000;
+  options.num_threads = 1;
+
+  if (m_verbose) {
+    options.minimizer_progress_to_stdout = true;
+  }
+
+  ceres::Solver::Summary summary;
+  ceres::Solve(options, &problem, &summary);
+  std::cout << "end ceres" << std::endl;
+
+  if (m_verbose) {
+    std::cout << summary.FullReport() << std::endl;
+  }
+
+  camera->readParameters(intrinsicCameraParams);
+
+  for (size_t i = 0; i < rvecs.size(); ++i) {
+    Eigen::AngleAxisd aa(transformVec.at(i).rotation());
+
+    Eigen::Vector3d rvec = aa.angle() * aa.axis();
+    cv::eigen2cv(rvec, rvecs.at(i));
+
+    cv::Mat &tvec = tvecs.at(i);
+    tvec.at<double>(0) = transformVec.at(i).translation()(0);
+    tvec.at<double>(1) = transformVec.at(i).translation()(1);
+    tvec.at<double>(2) = transformVec.at(i).translation()(2);
+  }
+}
+
+template <typename T>
+void CameraCalibration::readData(std::ifstream &ifs, T &data) const {
+  char *buffer = new char[sizeof(T)];
+
+  ifs.read(buffer, sizeof(T));
+
+  data = *(reinterpret_cast<T *>(buffer));
+
+  delete[] buffer;
+}
+
+template <typename T>
+void CameraCalibration::writeData(std::ofstream &ofs, T data) const {
+  char *pData = reinterpret_cast<char *>(&data);
+
+  ofs.write(pData, sizeof(T));
+}
+}

src/mynteye/api/camodocal/src/calib/StereoCameraCalibration.cc

@@ -0,0 +1,370 @@
+#include "camodocal/calib/StereoCameraCalibration.h"
+
+#include <boost/filesystem.hpp>
+#include <opencv2/core/eigen.hpp>
+
+#include "camodocal/EigenUtils.h"
+#include "camodocal/camera_models/CameraFactory.h"
+#include "camodocal/camera_models/CostFunctionFactory.h"
+#include "camodocal/gpl/EigenQuaternionParameterization.h"
+#include "ceres/ceres.h"
+
+namespace camodocal {
+
+StereoCameraCalibration::StereoCameraCalibration(
+    Camera::ModelType modelType, const std::string &cameraLeftName,
+    const std::string &cameraRightName, const cv::Size &imageSize,
+    const cv::Size &boardSize, float squareSize)
+    : m_calibLeft(modelType, cameraLeftName, imageSize, boardSize, squareSize),
+      m_calibRight(
+          modelType, cameraRightName, imageSize, boardSize, squareSize),
+      m_verbose(false) {
+       stereo_error.resize(2, 0.0);
+    }
+
+void StereoCameraCalibration::clear(void) {
+  m_calibLeft.clear();
+  m_calibRight.clear();
+}
+
+void StereoCameraCalibration::addChessboardData(
+    const std::vector<cv::Point2f> &cornersLeft,
+    const std::vector<cv::Point2f> &cornersRight) {
+  m_calibLeft.addChessboardData(cornersLeft);
+  m_calibRight.addChessboardData(cornersRight);
+}
+
+bool StereoCameraCalibration::calibrate(void) {
+  // calibrate cameras individually
+  if (!m_calibLeft.calibrate()) {
+    return false;
+  }
+  std::cout << "left_calibrate complete." << std::endl;
+  if (!m_calibRight.calibrate()) {
+    return false;
+  }
+  std::cout << "right_calibrate complete." << std::endl;
+
+  // perform stereo calibration
+  int imageCount = imagePointsLeft().size();
+
+  // find best estimate for initial transform from left camera frame to right
+  // camera frame
+  double minReprojErr = std::numeric_limits<double>::max();
+  for (int i = 0; i < imageCount; ++i) {
+    Eigen::Vector3d rvec;
+    rvec << m_calibLeft.cameraPoses().at<double>(i, 0),
+        m_calibLeft.cameraPoses().at<double>(i, 1),
+        m_calibLeft.cameraPoses().at<double>(i, 2);
+
+    Eigen::Quaterniond q_l = AngleAxisToQuaternion(rvec);
+
+    Eigen::Vector3d t_l;
+    t_l << m_calibLeft.cameraPoses().at<double>(i, 3),
+        m_calibLeft.cameraPoses().at<double>(i, 4),
+        m_calibLeft.cameraPoses().at<double>(i, 5);
+
+    rvec << m_calibRight.cameraPoses().at<double>(i, 0),
+        m_calibRight.cameraPoses().at<double>(i, 1),
+        m_calibRight.cameraPoses().at<double>(i, 2);
+
+    Eigen::Quaterniond q_r = AngleAxisToQuaternion(rvec);
+
+    Eigen::Vector3d t_r;
+    t_r << m_calibRight.cameraPoses().at<double>(i, 3),
+        m_calibRight.cameraPoses().at<double>(i, 4),
+        m_calibRight.cameraPoses().at<double>(i, 5);
+
+    Eigen::Quaterniond q_l_r = q_r * q_l.conjugate();
+    Eigen::Vector3d t_l_r = -q_l_r.toRotationMatrix() * t_l + t_r;
+
+    std::vector<cv::Mat> rvecs(imageCount);
+    std::vector<cv::Mat> tvecs(imageCount);
+
+    for (int j = 0; j < imageCount; ++j) {
+      rvec << m_calibLeft.cameraPoses().at<double>(j, 0),
+          m_calibLeft.cameraPoses().at<double>(j, 1),
+          m_calibLeft.cameraPoses().at<double>(j, 2);
+
+      q_l = AngleAxisToQuaternion(rvec);
+
+      t_l << m_calibLeft.cameraPoses().at<double>(j, 3),
+          m_calibLeft.cameraPoses().at<double>(j, 4),
+          m_calibLeft.cameraPoses().at<double>(j, 5);
+
+      Eigen::Quaterniond q_r = q_l_r * q_l;
+      Eigen::Vector3d t_r = q_l_r.toRotationMatrix() * t_l + t_l_r;
+
+      QuaternionToAngleAxis(q_r.coeffs().data(), rvec);
+      cv::eigen2cv(rvec, rvecs.at(j));
+
+      cv::eigen2cv(t_r, tvecs.at(j));
+    }
+
+    double reprojErr = cameraRight()->reprojectionError(
+        scenePoints(), imagePointsRight(), rvecs, tvecs);
+
+    if (reprojErr < minReprojErr) {
+      minReprojErr = reprojErr;
+      m_q = q_l_r;
+      m_t = t_l_r;
+    }
+  }
+
+  std::vector<cv::Mat> rvecsL(imageCount);
+  std::vector<cv::Mat> tvecsL(imageCount);
+  std::vector<cv::Mat> rvecsR(imageCount);
+  std::vector<cv::Mat> tvecsR(imageCount);
+
+  double *extrinsicCameraLParams[scenePoints().size()];
+  for (int i = 0; i < imageCount; ++i) {
+    extrinsicCameraLParams[i] = new double[7];
+
+    Eigen::Vector3d rvecL(
+        m_calibLeft.cameraPoses().at<double>(i, 0),
+        m_calibLeft.cameraPoses().at<double>(i, 1),
+        m_calibLeft.cameraPoses().at<double>(i, 2));
+
+    AngleAxisToQuaternion(rvecL, extrinsicCameraLParams[i]);
+
+    extrinsicCameraLParams[i][4] = m_calibLeft.cameraPoses().at<double>(i, 3);
+    extrinsicCameraLParams[i][5] = m_calibLeft.cameraPoses().at<double>(i, 4);
+    extrinsicCameraLParams[i][6] = m_calibLeft.cameraPoses().at<double>(i, 5);
+
+    cv::eigen2cv(rvecL, rvecsL.at(i));
+
+    Eigen::Vector3d tvecL;
+    tvecL << m_calibLeft.cameraPoses().at<double>(i, 3),
+        m_calibLeft.cameraPoses().at<double>(i, 4),
+        m_calibLeft.cameraPoses().at<double>(i, 5);
+
+    cv::eigen2cv(tvecL, tvecsL.at(i));
+
+    Eigen::Quaterniond q_r = m_q * AngleAxisToQuaternion(rvecL);
+    Eigen::Vector3d t_r = m_q.toRotationMatrix() * tvecL + m_t;
+
+    Eigen::Vector3d rvecR;
+    QuaternionToAngleAxis(q_r.coeffs().data(), rvecR);
+
+    cv::eigen2cv(rvecR, rvecsR.at(i));
+    cv::eigen2cv(t_r, tvecsR.at(i));
+  }
+
+  if (m_verbose) {
+    double roll, pitch, yaw;
+    mat2RPY(m_q.toRotationMatrix(), roll, pitch, yaw);
+
+    std::cout << "[stereo]"
+              << "# INFO: Initial extrinsics: " << std::endl
+              << "r: " << roll << "  p: " << pitch << "  yaw: " << yaw
+              << std::endl
+              << "x: " << m_t(0) << "  y: " << m_t(1) << "  z: " << m_t(2)
+              << std::endl;
+
+    double error = cameraLeft()->reprojectionError(
+        scenePoints(), imagePointsLeft(), rvecsL, tvecsL);
+    std::cout << "[" << cameraLeft()->cameraName() << "] "
+              << "# INFO: Initial reprojection error: " << error << " pixels"
+              << std::endl;
+
+    error = cameraRight()->reprojectionError(
+        scenePoints(), imagePointsRight(), rvecsR, tvecsR);
+    std::cout << "[" << cameraRight()->cameraName() << "] "
+              << "# INFO: Initial reprojection error: " << error << " pixels"
+              << std::endl;
+  }
+
+  std::vector<double> intrinsicCameraLParams;
+  cameraLeft()->writeParameters(intrinsicCameraLParams);
+
+  std::vector<double> intrinsicCameraRParams;
+  cameraRight()->writeParameters(intrinsicCameraRParams);
+
+  ceres::Problem problem;
+
+  for (int i = 0; i < imageCount; ++i) {
+    for (size_t j = 0; j < scenePoints().at(i).size(); ++j) {
+      const cv::Point3f &spt = scenePoints().at(i).at(j);
+      const cv::Point2f &iptL = imagePointsLeft().at(i).at(j);
+      const cv::Point2f &iptR = imagePointsRight().at(i).at(j);
+
+      ceres::CostFunction *costFunction =
+          CostFunctionFactory::instance()->generateCostFunction(
+              cameraLeft(), cameraRight(), Eigen::Vector3d(spt.x, spt.y, spt.z),
+              Eigen::Vector2d(iptL.x, iptL.y), Eigen::Vector2d(iptR.x, iptR.y));
+
+      ceres::LossFunction *lossFunction = new ceres::CauchyLoss(1.0);
+      problem.AddResidualBlock(
+          costFunction, lossFunction, intrinsicCameraLParams.data(),
+          intrinsicCameraRParams.data(), extrinsicCameraLParams[i],
+          extrinsicCameraLParams[i] + 4, m_q.coeffs().data(), m_t.data());
+    }
+  }
+
+  for (int i = 0; i < imageCount; ++i) {
+    ceres::LocalParameterization *quaternionParameterization =
+        new EigenQuaternionParameterization;
+
+    problem.SetParameterization(
+        extrinsicCameraLParams[i], quaternionParameterization);
+  }
+
+  ceres::LocalParameterization *quaternionParameterization =
+      new EigenQuaternionParameterization;
+
+  problem.SetParameterization(m_q.coeffs().data(), quaternionParameterization);
+
+  ceres::Solver::Options options;
+  options.max_num_iterations = 1000;
+  options.num_threads = 8;
+
+  if (m_verbose) {
+    options.minimizer_progress_to_stdout = true;
+  }
+
+  ceres::Solver::Summary summary;
+  ceres::Solve(options, &problem, &summary);
+
+  if (m_verbose) {
+    std::cout << summary.FullReport() << "\n";
+  }
+
+  cameraLeft()->readParameters(intrinsicCameraLParams);
+  cameraRight()->readParameters(intrinsicCameraRParams);
+
+  for (int i = 0; i < imageCount; ++i) {
+    Eigen::Vector3d rvecL;
+    QuaternionToAngleAxis(extrinsicCameraLParams[i], rvecL);
+
+    m_calibLeft.cameraPoses().at<double>(i, 0) = rvecL(0);
+    m_calibLeft.cameraPoses().at<double>(i, 1) = rvecL(1);
+    m_calibLeft.cameraPoses().at<double>(i, 2) = rvecL(2);
+    m_calibLeft.cameraPoses().at<double>(i, 3) = extrinsicCameraLParams[i][4];
+    m_calibLeft.cameraPoses().at<double>(i, 4) = extrinsicCameraLParams[i][5];
+    m_calibLeft.cameraPoses().at<double>(i, 5) = extrinsicCameraLParams[i][6];
+
+    cv::eigen2cv(rvecL, rvecsL.at(i));
+
+    Eigen::Vector3d tvecL;
+    tvecL << extrinsicCameraLParams[i][4], extrinsicCameraLParams[i][5],
+        extrinsicCameraLParams[i][6];
+
+    cv::eigen2cv(tvecL, tvecsL.at(i));
+
+    Eigen::Quaterniond q_r = m_q * AngleAxisToQuaternion(rvecL);
+    Eigen::Vector3d t_r = m_q.toRotationMatrix() * tvecL + m_t;
+
+    Eigen::Vector3d rvecR;
+    QuaternionToAngleAxis(q_r.coeffs().data(), rvecR);
+
+    m_calibRight.cameraPoses().at<double>(i, 0) = rvecR(0);
+    m_calibRight.cameraPoses().at<double>(i, 1) = rvecR(1);
+    m_calibRight.cameraPoses().at<double>(i, 2) = rvecR(2);
+    m_calibRight.cameraPoses().at<double>(i, 3) = t_r(0);
+    m_calibRight.cameraPoses().at<double>(i, 4) = t_r(1);
+    m_calibRight.cameraPoses().at<double>(i, 5) = t_r(2);
+
+    cv::eigen2cv(rvecR, rvecsR.at(i));
+    cv::eigen2cv(t_r, tvecsR.at(i));
+  }
+
+  if (m_verbose) {
+    double roll, pitch, yaw;
+    mat2RPY(m_q.toRotationMatrix(), roll, pitch, yaw);
+
+    std::cout << "[stereo]"
+              << "# INFO: Final extrinsics: " << std::endl
+              << "r: " << roll << "  p: " << pitch << "  yaw: " << yaw
+              << std::endl
+              << "x: " << m_t(0) << "  y: " << m_t(1) << "  z: " << m_t(2)
+              << std::endl;
+
+    stereo_error[0] = cameraLeft()->reprojectionError(
+        scenePoints(), imagePointsLeft(), rvecsL, tvecsL);
+    std::cout << "[" << cameraLeft()->cameraName() << "] "
+              << "# INFO: Final reprojection error: " << stereo_error[0] << " pixels"
+              << std::endl;
+    std::cout << "[" << cameraLeft()->cameraName() << "] "
+              << "# INFO: " << cameraLeft()->parametersToString() << std::endl;
+
+    stereo_error[1] = cameraRight()->reprojectionError(
+        scenePoints(), imagePointsRight(), rvecsR, tvecsR);
+    std::cout << "[" << cameraRight()->cameraName() << "] "
+              << "# INFO: Final reprojection error: " << stereo_error[1] << " pixels"
+              << std::endl;
+    std::cout << "[" << cameraRight()->cameraName() << "] "
+              << "# INFO: " << cameraRight()->parametersToString() << std::endl;
+  }
+
+  return true;
+}
+
+int StereoCameraCalibration::sampleCount(void) const {
+  return m_calibLeft.sampleCount();
+}
+
+const std::vector<std::vector<cv::Point2f> >
+    &StereoCameraCalibration::imagePointsLeft(void) const {
+  return m_calibLeft.imagePoints();
+}
+
+const std::vector<std::vector<cv::Point2f> >
+    &StereoCameraCalibration::imagePointsRight(void) const {
+  return m_calibRight.imagePoints();
+}
+
+const std::vector<std::vector<cv::Point3f> >
+    &StereoCameraCalibration::scenePoints(void) const {
+  return m_calibLeft.scenePoints();
+}
+
+CameraPtr &StereoCameraCalibration::cameraLeft(void) {
+  return m_calibLeft.camera();
+}
+
+const CameraConstPtr StereoCameraCalibration::cameraLeft(void) const {
+  return m_calibLeft.camera();
+}
+
+CameraPtr &StereoCameraCalibration::cameraRight(void) {
+  return m_calibRight.camera();
+}
+
+const CameraConstPtr StereoCameraCalibration::cameraRight(void) const {
+  return m_calibRight.camera();
+}
+
+void StereoCameraCalibration::drawResults(
+    std::vector<cv::Mat> &imagesLeft, std::vector<cv::Mat> &imagesRight) const {
+  m_calibLeft.drawResults(imagesLeft);
+  m_calibRight.drawResults(imagesRight);
+}
+
+void StereoCameraCalibration::writeParams(const std::string &directory) const {
+  if (!boost::filesystem::exists(directory)) {
+    boost::filesystem::create_directory(directory);
+  }
+
+  cameraLeft()->writeParametersToYamlFile(directory + "/camera_left.yaml");
+  cameraRight()->writeParametersToYamlFile(directory + "/camera_right.yaml");
+
+  cv::FileStorage fs(directory + "/extrinsics.yaml", cv::FileStorage::WRITE);
+
+  fs << "transform";
+  fs << "{"
+     << "q_x" << m_q.x() << "q_y" << m_q.y() << "q_z" << m_q.z() << "q_w"
+     << m_q.w() << "t_x" << m_t(0) << "t_y" << m_t(1) << "t_z" << m_t(2) << "}";
+
+  fs.release();
+  cv::FileStorage error_file(directory + "/stereo_reprojection_error.yaml", cv::FileStorage::WRITE);
+  error_file << "left_reprojection_error" << stereo_error[0];
+  error_file << "right_reprojection_error" << stereo_error[1];
+  error_file.release();
+}
+
+void StereoCameraCalibration::setVerbose(bool verbose) {
+  m_verbose = verbose;
+  m_calibLeft.setVerbose(verbose);
+  m_calibRight.setVerbose(verbose);
+}
+}

src/mynteye/api/camodocal/src/camera_models/Camera.cc

@@ -0,0 +1,206 @@
+#include "camodocal/camera_models/Camera.h"
+#include "camodocal/camera_models/ScaramuzzaCamera.h"
+
+#include <opencv2/calib3d/calib3d.hpp>
+
+namespace camodocal {
+
+Camera::Parameters::Parameters(ModelType modelType)
+    : m_modelType(modelType), m_imageWidth(0), m_imageHeight(0) {
+  switch (modelType) {
+    case KANNALA_BRANDT:
+      m_nIntrinsics = 8;
+      break;
+    case PINHOLE:
+      m_nIntrinsics = 8;
+      break;
+    case SCARAMUZZA:
+      m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS;
+      break;
+    case MEI:
+    default:
+      m_nIntrinsics = 9;
+  }
+}
+
+Camera::Parameters::Parameters(
+    ModelType modelType, const std::string &cameraName, int w, int h)
+    : m_modelType(modelType),
+      m_cameraName(cameraName),
+      m_imageWidth(w),
+      m_imageHeight(h) {
+  switch (modelType) {
+    case KANNALA_BRANDT:
+      m_nIntrinsics = 8;
+      break;
+    case PINHOLE:
+      m_nIntrinsics = 8;
+      break;
+    case SCARAMUZZA:
+      m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS;
+      break;
+    case MEI:
+    default:
+      m_nIntrinsics = 9;
+  }
+}
+
+Camera::ModelType &Camera::Parameters::modelType(void) {
+  return m_modelType;
+}
+
+std::string &Camera::Parameters::cameraName(void) {
+  return m_cameraName;
+}
+
+int &Camera::Parameters::imageWidth(void) {
+  return m_imageWidth;
+}
+
+int &Camera::Parameters::imageHeight(void) {
+  return m_imageHeight;
+}
+
+Camera::ModelType Camera::Parameters::modelType(void) const {
+  return m_modelType;
+}
+
+const std::string &Camera::Parameters::cameraName(void) const {
+  return m_cameraName;
+}
+
+int Camera::Parameters::imageWidth(void) const {
+  return m_imageWidth;
+}
+
+int Camera::Parameters::imageHeight(void) const {
+  return m_imageHeight;
+}
+
+int Camera::Parameters::nIntrinsics(void) const {
+  return m_nIntrinsics;
+}
+
+cv::Mat &Camera::mask(void) {
+  return m_mask;
+}
+
+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(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);
+}
+
+double Camera::reprojectionDist(
+    const Eigen::Vector3d &P1, const Eigen::Vector3d &P2) const {
+  Eigen::Vector2d p1, p2;
+
+  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,
+    const std::vector<cv::Mat> &rvecs, const std::vector<cv::Mat> &tvecs,
+    cv::OutputArray _perViewErrors) const {
+  int imageCount = objectPoints.size();
+  size_t pointsSoFar = 0;
+  double totalErr = 0.0;
+
+  bool computePerViewErrors = _perViewErrors.needed();
+  cv::Mat perViewErrors;
+  if (computePerViewErrors) {
+    _perViewErrors.create(imageCount, 1, CV_64F);
+    perViewErrors = _perViewErrors.getMat();
+  }
+
+  for (int i = 0; i < imageCount; ++i) {
+    size_t pointCount = imagePoints.at(i).size();
+
+    pointsSoFar += pointCount;
+
+    std::vector<cv::Point2f> estImagePoints;
+    projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i), estImagePoints);
+
+    double err = 0.0;
+    for (size_t j = 0; j < imagePoints.at(i).size(); ++j) {
+      err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j));
+    }
+
+    if (computePerViewErrors) {
+      perViewErrors.at<double>(i) = err / pointCount;
+    }
+
+    totalErr += err;
+  }
+
+  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();
+}
+
+void Camera::projectPoints(
+    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);
+
+  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)));
+  }
+}
+}

src/mynteye/api/camodocal/src/camera_models/CameraFactory.cc

@@ -0,0 +1,140 @@
+#include "camodocal/camera_models/CameraFactory.h"
+
+#include <boost/algorithm/string.hpp>
+
+#include "camodocal/camera_models/CataCamera.h"
+#include "camodocal/camera_models/EquidistantCamera.h"
+#include "camodocal/camera_models/PinholeCamera.h"
+#include "camodocal/camera_models/ScaramuzzaCamera.h"
+
+#include "ceres/ceres.h"
+
+namespace camodocal {
+
+boost::shared_ptr<CameraFactory> CameraFactory::m_instance;
+
+CameraFactory::CameraFactory() {}
+
+boost::shared_ptr<CameraFactory> CameraFactory::instance(void) {
+  if (m_instance.get() == 0) {
+    m_instance.reset(new CameraFactory);
+  }
+
+  return m_instance;
+}
+
+CameraPtr CameraFactory::generateCamera(
+    Camera::ModelType modelType, const std::string &cameraName,
+    cv::Size imageSize) const {
+  switch (modelType) {
+    case Camera::KANNALA_BRANDT: {
+      EquidistantCameraPtr camera(new EquidistantCamera);
+
+      EquidistantCamera::Parameters params = camera->getParameters();
+      params.cameraName() = cameraName;
+      params.imageWidth() = imageSize.width;
+      params.imageHeight() = imageSize.height;
+      camera->setParameters(params);
+      return camera;
+    }
+    case Camera::PINHOLE: {
+      PinholeCameraPtr camera(new PinholeCamera);
+
+      PinholeCamera::Parameters params = camera->getParameters();
+      params.cameraName() = cameraName;
+      params.imageWidth() = imageSize.width;
+      params.imageHeight() = imageSize.height;
+      camera->setParameters(params);
+      return camera;
+    }
+    case Camera::SCARAMUZZA: {
+      OCAMCameraPtr camera(new OCAMCamera);
+
+      OCAMCamera::Parameters params = camera->getParameters();
+      params.cameraName() = cameraName;
+      params.imageWidth() = imageSize.width;
+      params.imageHeight() = imageSize.height;
+      camera->setParameters(params);
+      return camera;
+    }
+    case Camera::MEI:
+    default: {
+      CataCameraPtr camera(new CataCamera);
+
+      CataCamera::Parameters params = camera->getParameters();
+      params.cameraName() = cameraName;
+      params.imageWidth() = imageSize.width;
+      params.imageHeight() = imageSize.height;
+      camera->setParameters(params);
+      return camera;
+    }
+  }
+}
+
+CameraPtr CameraFactory::generateCameraFromYamlFile(
+    const std::string &filename) {
+  cv::FileStorage fs(filename, cv::FileStorage::READ);
+
+  if (!fs.isOpened()) {
+    std::cout << "# ERROR: can not open " << filename << std::endl;
+    return CameraPtr();
+  }
+
+  Camera::ModelType modelType = Camera::MEI;
+  if (!fs["model_type"].isNone()) {
+    std::string sModelType;
+    fs["model_type"] >> sModelType;
+
+    if (boost::iequals(sModelType, "kannala_brandt")) {
+      modelType = Camera::KANNALA_BRANDT;
+    } else if (boost::iequals(sModelType, "mei")) {
+      modelType = Camera::MEI;
+    } else if (boost::iequals(sModelType, "scaramuzza")) {
+      modelType = Camera::SCARAMUZZA;
+    } else if (boost::iequals(sModelType, "pinhole")) {
+      modelType = Camera::PINHOLE;
+    } else {
+      std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl;
+      return CameraPtr();
+    }
+  }
+
+  switch (modelType) {
+    case Camera::KANNALA_BRANDT: {
+      EquidistantCameraPtr camera(new EquidistantCamera);
+
+      EquidistantCamera::Parameters params = camera->getParameters();
+      params.readFromYamlFile(filename);
+      camera->setParameters(params);
+      return camera;
+    }
+    case Camera::PINHOLE: {
+      PinholeCameraPtr camera(new PinholeCamera);
+
+      PinholeCamera::Parameters params = camera->getParameters();
+      params.readFromYamlFile(filename);
+      camera->setParameters(params);
+      return camera;
+    }
+    case Camera::SCARAMUZZA: {
+      OCAMCameraPtr camera(new OCAMCamera);
+
+      OCAMCamera::Parameters params = camera->getParameters();
+      params.readFromYamlFile(filename);
+      camera->setParameters(params);
+      return camera;
+    }
+    case Camera::MEI:
+    default: {
+      CataCameraPtr camera(new CataCamera);
+
+      CataCamera::Parameters params = camera->getParameters();
+      params.readFromYamlFile(filename);
+      camera->setParameters(params);
+      return camera;
+    }
+  }
+
+  return CameraPtr();
+}
+}

src/mynteye/api/camodocal/src/camera_models/CataCamera.cc

@@ -0,0 +1,863 @@
+#include "camodocal/camera_models/CataCamera.h"
+
+#include <cmath>
+#include <cstdio>
+#include "eigen3/Eigen/Dense"
+#include <iomanip>
+#include <iostream>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/core/eigen.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include "camodocal/gpl/gpl.h"
+
+namespace camodocal {
+
+CataCamera::Parameters::Parameters()
+    : Camera::Parameters(MEI),
+      m_xi(0.0),
+      m_k1(0.0),
+      m_k2(0.0),
+      m_p1(0.0),
+      m_p2(0.0),
+      m_gamma1(0.0),
+      m_gamma2(0.0),
+      m_u0(0.0),
+      m_v0(0.0) {}
+
+CataCamera::Parameters::Parameters(
+    const std::string &cameraName, int w, int h, double xi, double k1,
+    double k2, double p1, double p2, double gamma1, double gamma2, double u0,
+    double v0)
+    : Camera::Parameters(MEI, cameraName, w, h),
+      m_xi(xi),
+      m_k1(k1),
+      m_k2(k2),
+      m_p1(p1),
+      m_p2(p2),
+      m_gamma1(gamma1),
+      m_gamma2(gamma2),
+      m_u0(u0),
+      m_v0(v0) {}
+
+double &CataCamera::Parameters::xi(void) {
+  return m_xi;
+}
+
+double &CataCamera::Parameters::k1(void) {
+  return m_k1;
+}
+
+double &CataCamera::Parameters::k2(void) {
+  return m_k2;
+}
+
+double &CataCamera::Parameters::p1(void) {
+  return m_p1;
+}
+
+double &CataCamera::Parameters::p2(void) {
+  return m_p2;
+}
+
+double &CataCamera::Parameters::gamma1(void) {
+  return m_gamma1;
+}
+
+double &CataCamera::Parameters::gamma2(void) {
+  return m_gamma2;
+}
+
+double &CataCamera::Parameters::u0(void) {
+  return m_u0;
+}
+
+double &CataCamera::Parameters::v0(void) {
+  return m_v0;
+}
+
+double CataCamera::Parameters::xi(void) const {
+  return m_xi;
+}
+
+double CataCamera::Parameters::k1(void) const {
+  return m_k1;
+}
+
+double CataCamera::Parameters::k2(void) const {
+  return m_k2;
+}
+
+double CataCamera::Parameters::p1(void) const {
+  return m_p1;
+}
+
+double CataCamera::Parameters::p2(void) const {
+  return m_p2;
+}
+
+double CataCamera::Parameters::gamma1(void) const {
+  return m_gamma1;
+}
+
+double CataCamera::Parameters::gamma2(void) const {
+  return m_gamma2;
+}
+
+double CataCamera::Parameters::u0(void) const {
+  return m_u0;
+}
+
+double CataCamera::Parameters::v0(void) const {
+  return m_v0;
+}
+
+bool CataCamera::Parameters::readFromYamlFile(const std::string &filename) {
+  cv::FileStorage fs(filename, cv::FileStorage::READ);
+
+  if (!fs.isOpened()) {
+    return false;
+  }
+
+  if (!fs["model_type"].isNone()) {
+    std::string sModelType;
+    fs["model_type"] >> sModelType;
+
+    if (sModelType.compare("MEI") != 0) {
+      return false;
+    }
+  }
+
+  m_modelType = MEI;
+  fs["camera_name"] >> m_cameraName;
+  m_imageWidth = static_cast<int>(fs["image_width"]);
+  m_imageHeight = static_cast<int>(fs["image_height"]);
+
+  cv::FileNode n = fs["mirror_parameters"];
+  m_xi = static_cast<double>(n["xi"]);
+
+  n = fs["distortion_parameters"];
+  m_k1 = static_cast<double>(n["k1"]);
+  m_k2 = static_cast<double>(n["k2"]);
+  m_p1 = static_cast<double>(n["p1"]);
+  m_p2 = static_cast<double>(n["p2"]);
+
+  n = fs["projection_parameters"];
+  m_gamma1 = static_cast<double>(n["gamma1"]);
+  m_gamma2 = static_cast<double>(n["gamma2"]);
+  m_u0 = static_cast<double>(n["u0"]);
+  m_v0 = static_cast<double>(n["v0"]);
+
+  return true;
+}
+
+void CataCamera::Parameters::writeToYamlFile(
+    const std::string &filename) const {
+  cv::FileStorage fs(filename, cv::FileStorage::WRITE);
+
+  fs << "model_type"
+     << "MEI";
+  fs << "camera_name" << m_cameraName;
+  fs << "image_width" << m_imageWidth;
+  fs << "image_height" << m_imageHeight;
+
+  // mirror: xi
+  fs << "mirror_parameters";
+  fs << "{"
+     << "xi" << m_xi << "}";
+
+  // radial distortion: k1, k2
+  // tangential distortion: p1, p2
+  fs << "distortion_parameters";
+  fs << "{"
+     << "k1" << m_k1 << "k2" << m_k2 << "p1" << m_p1 << "p2" << m_p2 << "}";
+
+  // projection: gamma1, gamma2, u0, v0
+  fs << "projection_parameters";
+  fs << "{"
+     << "gamma1" << m_gamma1 << "gamma2" << m_gamma2 << "u0" << m_u0 << "v0"
+     << m_v0 << "}";
+
+  fs.release();
+}
+
+CataCamera::Parameters &CataCamera::Parameters::operator=(
+    const CataCamera::Parameters &other) {
+  if (this != &other) {
+    m_modelType = other.m_modelType;
+    m_cameraName = other.m_cameraName;
+    m_imageWidth = other.m_imageWidth;
+    m_imageHeight = other.m_imageHeight;
+    m_xi = other.m_xi;
+    m_k1 = other.m_k1;
+    m_k2 = other.m_k2;
+    m_p1 = other.m_p1;
+    m_p2 = other.m_p2;
+    m_gamma1 = other.m_gamma1;
+    m_gamma2 = other.m_gamma2;
+    m_u0 = other.m_u0;
+    m_v0 = other.m_v0;
+  }
+
+  return *this;
+}
+
+std::ostream &operator<<(
+    std::ostream &out, const CataCamera::Parameters &params) {
+  out << "Camera Parameters:" << std::endl;
+  out << "    model_type "
+      << "MEI" << std::endl;
+  out << "   camera_name " << params.m_cameraName << std::endl;
+  out << "   image_width " << params.m_imageWidth << std::endl;
+  out << "  image_height " << params.m_imageHeight << std::endl;
+
+  out << "Mirror Parameters" << std::endl;
+  out << std::fixed << std::setprecision(10);
+  out << "            xi " << params.m_xi << std::endl;
+
+  // radial distortion: k1, k2
+  // tangential distortion: p1, p2
+  out << "Distortion Parameters" << std::endl;
+  out << "            k1 " << params.m_k1 << std::endl
+      << "            k2 " << params.m_k2 << std::endl
+      << "            p1 " << params.m_p1 << std::endl
+      << "            p2 " << params.m_p2 << std::endl;
+
+  // projection: gamma1, gamma2, u0, v0
+  out << "Projection Parameters" << std::endl;
+  out << "        gamma1 " << params.m_gamma1 << std::endl
+      << "        gamma2 " << params.m_gamma2 << std::endl
+      << "            u0 " << params.m_u0 << std::endl
+      << "            v0 " << params.m_v0 << std::endl;
+
+  return out;
+}
+
+CataCamera::CataCamera()
+    : m_inv_K11(1.0),
+      m_inv_K13(0.0),
+      m_inv_K22(1.0),
+      m_inv_K23(0.0),
+      m_noDistortion(true) {}
+
+CataCamera::CataCamera(
+    const std::string &cameraName, int imageWidth, int imageHeight, double xi,
+    double k1, double k2, double p1, double p2, double gamma1, double gamma2,
+    double u0, double v0)
+    : mParameters(
+          cameraName, imageWidth, imageHeight, xi, k1, k2, p1, p2, gamma1,
+          gamma2, u0, v0) {
+  if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) &&
+      (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) {
+    m_noDistortion = true;
+  } else {
+    m_noDistortion = false;
+  }
+
+  // Inverse camera projection matrix parameters
+  m_inv_K11 = 1.0 / mParameters.gamma1();
+  m_inv_K13 = -mParameters.u0() / mParameters.gamma1();
+  m_inv_K22 = 1.0 / mParameters.gamma2();
+  m_inv_K23 = -mParameters.v0() / mParameters.gamma2();
+}
+
+CataCamera::CataCamera(const CataCamera::Parameters &params)
+    : mParameters(params) {
+  if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) &&
+      (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) {
+    m_noDistortion = true;
+  } else {
+    m_noDistortion = false;
+  }
+
+  // Inverse camera projection matrix parameters
+  m_inv_K11 = 1.0 / mParameters.gamma1();
+  m_inv_K13 = -mParameters.u0() / mParameters.gamma1();
+  m_inv_K22 = 1.0 / mParameters.gamma2();
+  m_inv_K23 = -mParameters.v0() / mParameters.gamma2();
+}
+
+Camera::ModelType CataCamera::modelType(void) const {
+  return mParameters.modelType();
+}
+
+const std::string &CataCamera::cameraName(void) const {
+  return mParameters.cameraName();
+}
+
+int CataCamera::imageWidth(void) const {
+  return mParameters.imageWidth();
+}
+
+int CataCamera::imageHeight(void) const {
+  return mParameters.imageHeight();
+}
+
+void CataCamera::estimateIntrinsics(
+    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 gamma0 = 0.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.xi() = 1.0;
+  params.k1() = 0.0;
+  params.k2() = 0.0;
+  params.p1() = 0.0;
+  params.p2() = 0.0;
+  params.u0() = u0;
+  params.v0() = v0;
+
+  // Initialize gamma (focal length)
+  // Use non-radial line image and xi = 1
+  for (size_t i = 0; i < imagePoints.size(); ++i) {
+    for (int r = 0; r < boardSize.height; ++r) {
+      cv::Mat P(boardSize.width, 4, CV_64F);
+      for (int c = 0; c < boardSize.width; ++c) {
+        const cv::Point2f &imagePoint =
+            imagePoints.at(i).at(r * boardSize.width + c);
+
+        double u = imagePoint.x - u0;
+        double v = imagePoint.y - v0;
+
+        P.at<double>(c, 0) = u;
+        P.at<double>(c, 1) = v;
+        P.at<double>(c, 2) = 0.5;
+        P.at<double>(c, 3) = -0.5 * (square(u) + square(v));
+      }
+
+      cv::Mat C;
+      cv::SVD::solveZ(P, C);
+
+      double t = square(C.at<double>(0)) + square(C.at<double>(1)) +
+                 C.at<double>(2) * C.at<double>(3);
+      if (t < 0.0) {
+        continue;
+      }
+
+      // check that line image is not radial
+      double d = sqrt(1.0 / t);
+      double nx = C.at<double>(0) * d;
+      double ny = C.at<double>(1) * d;
+      if (hypot(nx, ny) > 0.95) {
+        continue;
+      }
+
+      double gamma = sqrt(C.at<double>(2) / C.at<double>(3));
+
+      params.gamma1() = gamma;
+      params.gamma2() = gamma;
+      setParameters(params);
+
+      for (size_t j = 0; j < objectPoints.size(); ++j) {
+        estimateExtrinsics(
+            objectPoints.at(j), imagePoints.at(j), rvecs.at(j), tvecs.at(j));
+      }
+
+      double reprojErr = reprojectionError(
+          objectPoints, imagePoints, rvecs, tvecs, cv::noArray());
+
+      if (reprojErr < minReprojErr) {
+        minReprojErr = reprojErr;
+        gamma0 = gamma;
+      }
+    }
+  }
+
+  if (gamma0 <= 0.0 && minReprojErr >= std::numeric_limits<double>::max()) {
+    std::cout << "[" << params.cameraName() << "] "
+              << "# INFO: CataCamera model fails with given data. "
+              << std::endl;
+
+    return;
+  }
+
+  params.gamma1() = gamma0;
+  params.gamma2() = gamma0;
+  setParameters(params);
+}
+
+/**
+ * \brief Lifts a point from the image plane to the unit sphere
+ *
+ * \param p image coordinates
+ * \param P coordinates of the point on the sphere
+ */
+void CataCamera::liftSphere(
+    const Eigen::Vector2d &p, Eigen::Vector3d &P) const {
+  double mx_d, my_d, mx2_d, mxy_d, my2_d, mx_u, my_u;
+  double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;
+  double lambda;
+
+  // Lift points to normalised plane
+  mx_d = m_inv_K11 * p(0) + m_inv_K13;
+  my_d = m_inv_K22 * p(1) + m_inv_K23;
+
+  if (m_noDistortion) {
+    mx_u = mx_d;
+    my_u = my_d;
+  } else {
+    // Apply inverse distortion model
+    if (0) {
+      double k1 = mParameters.k1();
+      double k2 = mParameters.k2();
+      double p1 = mParameters.p1();
+      double p2 = mParameters.p2();
+
+      // Inverse distortion model
+      // proposed by Heikkila
+      mx2_d = mx_d * mx_d;
+      my2_d = my_d * my_d;
+      mxy_d = mx_d * my_d;
+      rho2_d = mx2_d + my2_d;
+      rho4_d = rho2_d * rho2_d;
+      radDist_d = k1 * rho2_d + k2 * rho4_d;
+      Dx_d = mx_d * radDist_d + p2 * (rho2_d + 2 * mx2_d) + 2 * p1 * mxy_d;
+      Dy_d = my_d * radDist_d + p1 * (rho2_d + 2 * my2_d) + 2 * p2 * mxy_d;
+      inv_denom_d = 1 / (1 + 4 * k1 * rho2_d + 6 * k2 * rho4_d + 8 * p1 * my_d +
+                         8 * p2 * mx_d);
+
+      mx_u = mx_d - inv_denom_d * Dx_d;
+      my_u = my_d - inv_denom_d * Dy_d;
+    } else {
+      // Recursive distortion model
+      int n = 6;
+      Eigen::Vector2d d_u;
+      distortion(Eigen::Vector2d(mx_d, my_d), d_u);
+      // Approximate value
+      mx_u = mx_d - d_u(0);
+      my_u = my_d - d_u(1);
+
+      for (int i = 1; i < n; ++i) {
+        distortion(Eigen::Vector2d(mx_u, my_u), d_u);
+        mx_u = mx_d - d_u(0);
+        my_u = my_d - d_u(1);
+      }
+    }
+  }
+
+  // Lift normalised points to the sphere (inv_hslash)
+  double xi = mParameters.xi();
+  if (xi == 1.0) {
+    lambda = 2.0 / (mx_u * mx_u + my_u * my_u + 1.0);
+    P << lambda * mx_u, lambda * my_u, lambda - 1.0;
+  } else {
+    lambda = (xi + sqrt(1.0 + (1.0 - xi * xi) * (mx_u * mx_u + my_u * my_u))) /
+             (1.0 + mx_u * mx_u + my_u * my_u);
+    P << lambda * mx_u, lambda * my_u, lambda - xi;
+  }
+}
+
+/**
+ * \brief Lifts a point from the image plane to its projective ray
+ *
+ * \param p image coordinates
+ * \param P coordinates of the projective ray
+ */
+void CataCamera::liftProjective(
+    const Eigen::Vector2d &p, Eigen::Vector3d &P) const {
+  double mx_d, my_d, mx2_d, mxy_d, my2_d, mx_u, my_u;
+  double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;
+  // double lambda;
+
+  // Lift points to normalised plane
+  mx_d = m_inv_K11 * p(0) + m_inv_K13;
+  my_d = m_inv_K22 * p(1) + m_inv_K23;
+
+  if (m_noDistortion) {
+    mx_u = mx_d;
+    my_u = my_d;
+  } else {
+    if (0) {
+      double k1 = mParameters.k1();
+      double k2 = mParameters.k2();
+      double p1 = mParameters.p1();
+      double p2 = mParameters.p2();
+
+      // Apply inverse distortion model
+      // proposed by Heikkila
+      mx2_d = mx_d * mx_d;
+      my2_d = my_d * my_d;
+      mxy_d = mx_d * my_d;
+      rho2_d = mx2_d + my2_d;
+      rho4_d = rho2_d * rho2_d;
+      radDist_d = k1 * rho2_d + k2 * rho4_d;
+      Dx_d = mx_d * radDist_d + p2 * (rho2_d + 2 * mx2_d) + 2 * p1 * mxy_d;
+      Dy_d = my_d * radDist_d + p1 * (rho2_d + 2 * my2_d) + 2 * p2 * mxy_d;
+      inv_denom_d = 1 / (1 + 4 * k1 * rho2_d + 6 * k2 * rho4_d + 8 * p1 * my_d +
+                         8 * p2 * mx_d);
+
+      mx_u = mx_d - inv_denom_d * Dx_d;
+      my_u = my_d - inv_denom_d * Dy_d;
+    } else {
+      // Recursive distortion model
+      int n = 8;
+      Eigen::Vector2d d_u;
+      distortion(Eigen::Vector2d(mx_d, my_d), d_u);
+      // Approximate value
+      mx_u = mx_d - d_u(0);
+      my_u = my_d - d_u(1);
+
+      for (int i = 1; i < n; ++i) {
+        distortion(Eigen::Vector2d(mx_u, my_u), d_u);
+        mx_u = mx_d - d_u(0);
+        my_u = my_d - d_u(1);
+      }
+    }
+  }
+
+  // Obtain a projective ray
+  double xi = mParameters.xi();
+  if (xi == 1.0) {
+    P << mx_u, my_u, (1.0 - mx_u * mx_u - my_u * my_u) / 2.0;
+  } else {
+    // Reuse variable
+    rho2_d = mx_u * mx_u + my_u * my_u;
+    P << mx_u, my_u,
+        1.0 - xi * (rho2_d + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * rho2_d));
+  }
+}
+
+/**
+ * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v)
+ *
+ * \param P 3D point coordinates
+ * \param p return value, contains the image point coordinates
+ */
+void CataCamera::spaceToPlane(
+    const Eigen::Vector3d &P, Eigen::Vector2d &p) const {
+  Eigen::Vector2d p_u, p_d;
+
+  // Project points to the normalised plane
+  double z = P(2) + mParameters.xi() * P.norm();
+  p_u << P(0) / z, P(1) / z;
+
+  if (m_noDistortion) {
+    p_d = p_u;
+  } else {
+    // Apply distortion
+    Eigen::Vector2d d_u;
+    distortion(p_u, d_u);
+    p_d = p_u + d_u;
+  }
+
+  // Apply generalised projection matrix
+  p << mParameters.gamma1() * p_d(0) + mParameters.u0(),
+      mParameters.gamma2() * p_d(1) + mParameters.v0();
+}
+
+#if 0
+/** 
+ * \brief Project a 3D point to the image plane and calculate Jacobian
+ *
+ * \param P 3D point coordinates
+ * \param p return value, contains the image point coordinates
+ */
+void
+CataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
+                        Eigen::Matrix<double,2,3>& J) const
+{
+    double xi = mParameters.xi();
+
+    Eigen::Vector2d p_u, p_d;
+    double norm, inv_denom;
+    double dxdmx, dydmx, dxdmy, dydmy;
+
+    norm = P.norm();
+    // Project points to the normalised plane
+    inv_denom = 1.0 / (P(2) + xi * norm);
+    p_u << inv_denom * P(0), inv_denom * P(1);
+
+    // Calculate jacobian
+    inv_denom = inv_denom * inv_denom / norm;
+    double dudx = inv_denom * (norm * P(2) + xi * (P(1) * P(1) + P(2) * P(2)));
+    double dvdx = -inv_denom * xi * P(0) * P(1);
+    double dudy = dvdx;
+    double dvdy = inv_denom * (norm * P(2) + xi * (P(0) * P(0) + P(2) * P(2)));
+    inv_denom = inv_denom * (-xi * P(2) - norm); // reuse variable
+    double dudz = P(0) * inv_denom;
+    double dvdz = P(1) * inv_denom;
+
+    if (m_noDistortion)
+    {
+        p_d = p_u;
+    }
+    else
+    {
+        // Apply distortion
+        Eigen::Vector2d d_u;
+        distortion(p_u, d_u);
+        p_d = p_u + d_u;
+    }
+
+    double gamma1 = mParameters.gamma1();
+    double gamma2 = mParameters.gamma2();
+
+    // Make the product of the jacobians
+    // and add projection matrix jacobian
+    inv_denom = gamma1 * (dudx * dxdmx + dvdx * dxdmy); // reuse
+    dvdx = gamma2 * (dudx * dydmx + dvdx * dydmy);
+    dudx = inv_denom;
+
+    inv_denom = gamma1 * (dudy * dxdmx + dvdy * dxdmy); // reuse
+    dvdy = gamma2 * (dudy * dydmx + dvdy * dydmy);
+    dudy = inv_denom;
+
+    inv_denom = gamma1 * (dudz * dxdmx + dvdz * dxdmy); // reuse
+    dvdz = gamma2 * (dudz * dydmx + dvdz * dydmy);
+    dudz = inv_denom;
+    
+    // Apply generalised projection matrix
+    p << gamma1 * p_d(0) + mParameters.u0(),
+         gamma2 * p_d(1) + mParameters.v0();
+
+    J << dudx, dudy, dudz,
+         dvdx, dvdy, dvdz;
+}
+#endif
+
+/**
+ * \brief Projects an undistorted 2D point p_u to the image plane
+ *
+ * \param p_u 2D point coordinates
+ * \return image point coordinates
+ */
+void CataCamera::undistToPlane(
+    const Eigen::Vector2d &p_u, Eigen::Vector2d &p) const {
+  Eigen::Vector2d p_d;
+
+  if (m_noDistortion) {
+    p_d = p_u;
+  } else {
+    // Apply distortion
+    Eigen::Vector2d d_u;
+    distortion(p_u, d_u);
+    p_d = p_u + d_u;
+  }
+
+  // Apply generalised projection matrix
+  p << mParameters.gamma1() * p_d(0) + mParameters.u0(),
+      mParameters.gamma2() * p_d(1) + mParameters.v0();
+}
+
+/**
+ * \brief Apply distortion to input point (from the normalised plane)
+ *
+ * \param p_u undistorted coordinates of point on the normalised plane
+ * \return to obtain the distorted point: p_d = p_u + d_u
+ */
+void CataCamera::distortion(
+    const Eigen::Vector2d &p_u, Eigen::Vector2d &d_u) const {
+  double k1 = mParameters.k1();
+  double k2 = mParameters.k2();
+  double p1 = mParameters.p1();
+  double p2 = mParameters.p2();
+
+  double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;
+
+  mx2_u = p_u(0) * p_u(0);
+  my2_u = p_u(1) * p_u(1);
+  mxy_u = p_u(0) * p_u(1);
+  rho2_u = mx2_u + my2_u;
+  rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;
+  d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),
+      p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);
+}
+
+/**
+ * \brief Apply distortion to input point (from the normalised plane)
+ *        and calculate Jacobian
+ *
+ * \param p_u undistorted coordinates of point on the normalised plane
+ * \return to obtain the distorted point: p_d = p_u + d_u
+ */
+void CataCamera::distortion(
+    const Eigen::Vector2d &p_u, Eigen::Vector2d &d_u,
+    Eigen::Matrix2d &J) const {
+  double k1 = mParameters.k1();
+  double k2 = mParameters.k2();
+  double p1 = mParameters.p1();
+  double p2 = mParameters.p2();
+
+  double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;
+
+  mx2_u = p_u(0) * p_u(0);
+  my2_u = p_u(1) * p_u(1);
+  mxy_u = p_u(0) * p_u(1);
+  rho2_u = mx2_u + my2_u;
+  rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;
+  d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),
+      p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);
+
+  double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u +
+                 k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) +
+                 6.0 * p2 * p_u(0);
+  double dydmx = k1 * 2.0 * p_u(0) * p_u(1) +
+                 k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) +
+                 2.0 * p2 * p_u(1);
+  double dxdmy = dydmx;
+  double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u +
+                 k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) +
+                 2.0 * p2 * p_u(0);
+
+  J << dxdmx, dxdmy, dydmx, dydmy;
+}
+
+void CataCamera::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);
+
+  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 xi = mParameters.xi();
+      double d2 = mx_u * mx_u + my_u * my_u;
+
+      Eigen::Vector3d P;
+      P << mx_u, my_u,
+          1.0 - xi * (d2 + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * d2));
+
+      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);
+}
+
+cv::Mat CataCamera::initUndistortRectifyMap(
+    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);
+
+  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.gamma1();
+    K_rect(1, 1) = mParameters.gamma2();
+  }
+
+  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;
+}
+
+int CataCamera::parameterCount(void) const {
+  return 9;
+}
+
+const CataCamera::Parameters &CataCamera::getParameters(void) const {
+  return mParameters;
+}
+
+void CataCamera::setParameters(const CataCamera::Parameters &parameters) {
+  mParameters = parameters;
+
+  if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) &&
+      (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) {
+    m_noDistortion = true;
+  } else {
+    m_noDistortion = false;
+  }
+
+  m_inv_K11 = 1.0 / mParameters.gamma1();
+  m_inv_K13 = -mParameters.u0() / mParameters.gamma1();
+  m_inv_K22 = 1.0 / mParameters.gamma2();
+  m_inv_K23 = -mParameters.v0() / mParameters.gamma2();
+}
+
+void CataCamera::readParameters(const std::vector<double> &parameterVec) {
+  if ((int)parameterVec.size() != parameterCount()) {
+    return;
+  }
+
+  Parameters params = getParameters();
+
+  params.xi() = parameterVec.at(0);
+  params.k1() = parameterVec.at(1);
+  params.k2() = parameterVec.at(2);
+  params.p1() = parameterVec.at(3);
+  params.p2() = parameterVec.at(4);
+  params.gamma1() = parameterVec.at(5);
+  params.gamma2() = parameterVec.at(6);
+  params.u0() = parameterVec.at(7);
+  params.v0() = parameterVec.at(8);
+
+  setParameters(params);
+}
+
+void CataCamera::writeParameters(std::vector<double> &parameterVec) const {
+  parameterVec.resize(parameterCount());
+  parameterVec.at(0) = mParameters.xi();
+  parameterVec.at(1) = mParameters.k1();
+  parameterVec.at(2) = mParameters.k2();
+  parameterVec.at(3) = mParameters.p1();
+  parameterVec.at(4) = mParameters.p2();
+  parameterVec.at(5) = mParameters.gamma1();
+  parameterVec.at(6) = mParameters.gamma2();
+  parameterVec.at(7) = mParameters.u0();
+  parameterVec.at(8) = mParameters.v0();
+}
+
+void CataCamera::writeParametersToYamlFile(const std::string &filename) const {
+  mParameters.writeToYamlFile(filename);
+}
+
+std::string CataCamera::parametersToString(void) const {
+  std::ostringstream oss;
+  oss << mParameters;
+
+  return oss.str();
+}
+}

src/mynteye/api/camodocal/src/camera_models/EquidistantCamera.cc

@@ -0,0 +1,733 @@
+#include "camodocal/camera_models/EquidistantCamera.h"
+#include <cstdint>
+#include <cmath>
+#include <cstdio>
+#include "eigen3/Eigen/Dense"
+#include <iomanip>
+#include <iostream>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/core/eigen.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include "camodocal/gpl/gpl.h"
+
+namespace camodocal {
+#define PI M_PI
+#define PI_2 1.5707963
+float ApproxAtan2(float y, float x)
+{
+    const float n1 = 0.97239411f;
+    const float n2 = -0.19194795f;    
+    float result = 0.0f;
+    if (x != 0.0f)
+    {
+        const union { float flVal; std::uint32_t nVal; } tYSign = { y };
+        const union { float flVal; std::uint32_t nVal; } tXSign = { x };
+        if (fabsf(x) >= fabsf(y))
+        {
+            union { float flVal; std::uint32_t nVal; } tOffset = { PI };
+            // Add or subtract PI based on y's sign.
+            tOffset.nVal |= tYSign.nVal & 0x80000000u;
+            // No offset if x is positive, so multiply by 0 or based on x's sign.
+            tOffset.nVal *= tXSign.nVal >> 31;
+            result = tOffset.flVal;
+            const float z = y / x;
+            result += (n1 + n2 * z * z) * z;
+        }
+        else // Use atan(y/x) = pi/2 - atan(x/y) if |y/x| > 1.
+        {
+            union { float flVal; std::uint32_t nVal; } tOffset = { PI_2 };
+            // Add or subtract PI/2 based on y's sign.
+            tOffset.nVal |= tYSign.nVal & 0x80000000u;            
+            result = tOffset.flVal;
+            const float z = x / y;
+            result -= (n1 + n2 * z * z) * z;            
+        }
+    }
+    else if (y > 0.0f)
+    {
+        result = PI_2;
+    }
+    else if (y < 0.0f)
+    {
+        result = -PI_2;
+    }
+    return result;
+}
+
+EquidistantCamera::Parameters::Parameters()
+    : Camera::Parameters(KANNALA_BRANDT),
+      m_k2(0.0),
+      m_k3(0.0),
+      m_k4(0.0),
+      m_k5(0.0),
+      m_mu(0.0),
+      m_mv(0.0),
+      m_u0(0.0),
+      m_v0(0.0) {}
+
+EquidistantCamera::Parameters::Parameters(
+    const std::string &cameraName, int w, int h, double k2, double k3,
+    double k4, double k5, double mu, double mv, double u0, double v0)
+    : Camera::Parameters(KANNALA_BRANDT, cameraName, w, h),
+      m_k2(k2),
+      m_k3(k3),
+      m_k4(k4),
+      m_k5(k5),
+      m_mu(mu),
+      m_mv(mv),
+      m_u0(u0),
+      m_v0(v0) {}
+
+double &EquidistantCamera::Parameters::k2(void) {
+  return m_k2;
+}
+
+double &EquidistantCamera::Parameters::k3(void) {
+  return m_k3;
+}
+
+double &EquidistantCamera::Parameters::k4(void) {
+  return m_k4;
+}
+
+double &EquidistantCamera::Parameters::k5(void) {
+  return m_k5;
+}
+
+double &EquidistantCamera::Parameters::mu(void) {
+  return m_mu;
+}
+
+double &EquidistantCamera::Parameters::mv(void) {
+  return m_mv;
+}
+
+double &EquidistantCamera::Parameters::u0(void) {
+  return m_u0;
+}
+
+double &EquidistantCamera::Parameters::v0(void) {
+  return m_v0;
+}
+
+double EquidistantCamera::Parameters::k2(void) const {
+  return m_k2;
+}
+
+double EquidistantCamera::Parameters::k3(void) const {
+  return m_k3;
+}
+
+double EquidistantCamera::Parameters::k4(void) const {
+  return m_k4;
+}
+
+double EquidistantCamera::Parameters::k5(void) const {
+  return m_k5;
+}
+
+double EquidistantCamera::Parameters::mu(void) const {
+  return m_mu;
+}
+
+double EquidistantCamera::Parameters::mv(void) const {
+  return m_mv;
+}
+
+double EquidistantCamera::Parameters::u0(void) const {
+  return m_u0;
+}
+
+double EquidistantCamera::Parameters::v0(void) const {
+  return m_v0;
+}
+
+bool EquidistantCamera::Parameters::readFromYamlFile(
+    const std::string &filename) {
+  cv::FileStorage fs(filename, cv::FileStorage::READ);
+
+  if (!fs.isOpened()) {
+    return false;
+  }
+
+  if (!fs["model_type"].isNone()) {
+    std::string sModelType;
+    fs["model_type"] >> sModelType;
+
+    if (sModelType.compare("KANNALA_BRANDT") != 0) {
+      return false;
+    }
+  }
+
+  m_modelType = KANNALA_BRANDT;
+  fs["camera_name"] >> m_cameraName;
+  m_imageWidth = static_cast<int>(fs["image_width"]);
+  m_imageHeight = static_cast<int>(fs["image_height"]);
+
+  cv::FileNode n = fs["projection_parameters"];
+  m_k2 = static_cast<double>(n["k2"]);
+  m_k3 = static_cast<double>(n["k3"]);
+  m_k4 = static_cast<double>(n["k4"]);
+  m_k5 = static_cast<double>(n["k5"]);
+  m_mu = static_cast<double>(n["mu"]);
+  m_mv = static_cast<double>(n["mv"]);
+  m_u0 = static_cast<double>(n["u0"]);
+  m_v0 = static_cast<double>(n["v0"]);
+
+  return true;
+}
+
+void EquidistantCamera::Parameters::writeToYamlFile(
+    const std::string &filename) const {
+  cv::FileStorage fs(filename, cv::FileStorage::WRITE);
+
+  fs << "model_type"
+     << "KANNALA_BRANDT";
+  fs << "camera_name" << m_cameraName;
+  fs << "image_width" << m_imageWidth;
+  fs << "image_height" << m_imageHeight;
+
+  // projection: k2, k3, k4, k5, mu, mv, u0, v0
+  fs << "projection_parameters";
+  fs << "{"
+     << "k2" << m_k2 << "k3" << m_k3 << "k4" << m_k4 << "k5" << m_k5 << "mu"
+     << m_mu << "mv" << m_mv << "u0" << m_u0 << "v0" << m_v0 << "}";
+
+  fs.release();
+}
+
+EquidistantCamera::Parameters &EquidistantCamera::Parameters::operator=(
+    const EquidistantCamera::Parameters &other) {
+  if (this != &other) {
+    m_modelType = other.m_modelType;
+    m_cameraName = other.m_cameraName;
+    m_imageWidth = other.m_imageWidth;
+    m_imageHeight = other.m_imageHeight;
+    m_k2 = other.m_k2;
+    m_k3 = other.m_k3;
+    m_k4 = other.m_k4;
+    m_k5 = other.m_k5;
+    m_mu = other.m_mu;
+    m_mv = other.m_mv;
+    m_u0 = other.m_u0;
+    m_v0 = other.m_v0;
+  }
+
+  return *this;
+}
+
+std::ostream &operator<<(
+    std::ostream &out, const EquidistantCamera::Parameters &params) {
+  out << "Camera Parameters:" << std::endl;
+  out << "    model_type "
+      << "KANNALA_BRANDT" << std::endl;
+  out << "   camera_name " << params.m_cameraName << std::endl;
+  out << "   image_width " << params.m_imageWidth << std::endl;
+  out << "  image_height " << params.m_imageHeight << std::endl;
+
+  // projection: k2, k3, k4, k5, mu, mv, u0, v0
+  out << "Projection Parameters" << std::endl;
+  out << "            k2 " << params.m_k2 << std::endl
+      << "            k3 " << params.m_k3 << std::endl
+      << "            k4 " << params.m_k4 << std::endl
+      << "            k5 " << params.m_k5 << std::endl
+      << "            mu " << params.m_mu << std::endl
+      << "            mv " << params.m_mv << std::endl
+      << "            u0 " << params.m_u0 << std::endl
+      << "            v0 " << params.m_v0 << std::endl;
+
+  return out;
+}
+
+EquidistantCamera::EquidistantCamera()
+    : m_inv_K11(1.0), m_inv_K13(0.0), m_inv_K22(1.0), m_inv_K23(0.0) {}
+
+EquidistantCamera::EquidistantCamera(
+    const std::string &cameraName, int imageWidth, int imageHeight, double k2,
+    double k3, double k4, double k5, double mu, double mv, double u0, double v0)
+    : mParameters(
+          cameraName, imageWidth, imageHeight, k2, k3, k4, k5, mu, mv, u0, v0) {
+  // Inverse camera projection matrix parameters
+  m_inv_K11 = 1.0 / mParameters.mu();
+  m_inv_K13 = -mParameters.u0() / mParameters.mu();
+  m_inv_K22 = 1.0 / mParameters.mv();
+  m_inv_K23 = -mParameters.v0() / mParameters.mv();
+}
+
+EquidistantCamera::EquidistantCamera(
+    const EquidistantCamera::Parameters &params)
+    : mParameters(params) {
+  // Inverse camera projection matrix parameters
+  m_inv_K11 = 1.0 / mParameters.mu();
+  m_inv_K13 = -mParameters.u0() / mParameters.mu();
+  m_inv_K22 = 1.0 / mParameters.mv();
+  m_inv_K23 = -mParameters.v0() / mParameters.mv();
+}
+
+Camera::ModelType EquidistantCamera::modelType(void) const {
+  return mParameters.modelType();
+}
+
+const std::string &EquidistantCamera::cameraName(void) const {
+  return mParameters.cameraName();
+}
+
+int EquidistantCamera::imageWidth(void) const {
+  return mParameters.imageWidth();
+}
+
+int EquidistantCamera::imageHeight(void) const {
+  return mParameters.imageHeight();
+}
+
+void EquidistantCamera::estimateIntrinsics(
+    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);
+    double radius[boardSize.height];
+    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;
+        }
+      }
+    }
+  }
+
+  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 the unit sphere
+ *
+ * \param p image coordinates
+ * \param P coordinates of the point on the sphere
+ */
+void EquidistantCamera::liftSphere(
+    const Eigen::Vector2d &p, Eigen::Vector3d &P) const {
+  liftProjective(p, P);
+}
+
+/**
+ * \brief Lifts a point from the image plane to its projective ray
+ *
+ * \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);
+}
+
+/**
+ * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v)
+ *
+ * \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();
+}
+
+/**
+ * \brief Project a 3D point to the image plane and calculate Jacobian
+ *
+ * \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();
+}
+
+/**
+ * \brief Projects an undistorted 2D point p_u to the image plane
+ *
+ * \param p_u 2D point coordinates
+ * \return image point coordinates
+ */
+void EquidistantCamera::undistToPlane(
+    const Eigen::Vector2d &p_u, Eigen::Vector2d &p) const {
+  //    Eigen::Vector2d p_d;
+  //
+  //    if (m_noDistortion)
+  //    {
+  //        p_d = p_u;
+  //    }
+  //    else
+  //    {
+  //        // Apply distortion
+  //        Eigen::Vector2d d_u;
+  //        distortion(p_u, d_u);
+  //        p_d = p_u + d_u;
+  //    }
+  //
+  //    // Apply generalised projection matrix
+  //    p << mParameters.gamma1() * p_d(0) + mParameters.u0(),
+  //         mParameters.gamma2() * p_d(1) + mParameters.v0();
+}
+
+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);
+
+  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);
+
+      Eigen::Vector3d P;
+      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);
+}
+
+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 {
+  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);
+
+  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;
+}
+
+int EquidistantCamera::parameterCount(void) const {
+  return 8;
+}
+
+const EquidistantCamera::Parameters &EquidistantCamera::getParameters(
+    void) const {
+  return mParameters;
+}
+
+void EquidistantCamera::setParameters(
+    const EquidistantCamera::Parameters &parameters) {
+  mParameters = parameters;
+
+  // Inverse camera projection matrix parameters
+  m_inv_K11 = 1.0 / mParameters.mu();
+  m_inv_K13 = -mParameters.u0() / mParameters.mu();
+  m_inv_K22 = 1.0 / mParameters.mv();
+  m_inv_K23 = -mParameters.v0() / mParameters.mv();
+}
+
+void EquidistantCamera::readParameters(
+    const std::vector<double> &parameterVec) {
+  if (parameterVec.size() != parameterCount()) {
+    return;
+  }
+
+  Parameters params = getParameters();
+
+  params.k2() = parameterVec.at(0);
+  params.k3() = parameterVec.at(1);
+  params.k4() = parameterVec.at(2);
+  params.k5() = parameterVec.at(3);
+  params.mu() = parameterVec.at(4);
+  params.mv() = parameterVec.at(5);
+  params.u0() = parameterVec.at(6);
+  params.v0() = parameterVec.at(7);
+
+  setParameters(params);
+}
+
+void EquidistantCamera::writeParameters(
+    std::vector<double> &parameterVec) const {
+  parameterVec.resize(parameterCount());
+  parameterVec.at(0) = mParameters.k2();
+  parameterVec.at(1) = mParameters.k3();
+  parameterVec.at(2) = mParameters.k4();
+  parameterVec.at(3) = mParameters.k5();
+  parameterVec.at(4) = mParameters.mu();
+  parameterVec.at(5) = mParameters.mv();
+  parameterVec.at(6) = mParameters.u0();
+  parameterVec.at(7) = mParameters.v0();
+}
+
+void EquidistantCamera::writeParametersToYamlFile(
+    const std::string &filename) const {
+  mParameters.writeToYamlFile(filename);
+}
+
+std::string EquidistantCamera::parametersToString(void) const {
+  std::ostringstream oss;
+  oss << mParameters;
+
+  return oss.str();
+}
+
+void EquidistantCamera::fitOddPoly(
+    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);
+  }
+
+  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);
+  }
+}
+
+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());
+    }
+  }
+}
+}

src/mynteye/api/camodocal/src/camera_models/PinholeCamera.cc

@@ -0,0 +1,752 @@
+#include "camodocal/camera_models/PinholeCamera.h"
+
+#include <cmath>
+#include <cstdio>
+#include "eigen3/Eigen/Dense"
+#include <iomanip>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/core/eigen.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include "camodocal/gpl/gpl.h"
+
+namespace camodocal {
+
+PinholeCamera::Parameters::Parameters()
+    : Camera::Parameters(PINHOLE),
+      m_k1(0.0),
+      m_k2(0.0),
+      m_p1(0.0),
+      m_p2(0.0),
+      m_fx(0.0),
+      m_fy(0.0),
+      m_cx(0.0),
+      m_cy(0.0) {}
+
+PinholeCamera::Parameters::Parameters(
+    const std::string &cameraName, int w, int h, double k1, double k2,
+    double p1, double p2, double fx, double fy, double cx, double cy)
+    : Camera::Parameters(PINHOLE, cameraName, w, h),
+      m_k1(k1),
+      m_k2(k2),
+      m_p1(p1),
+      m_p2(p2),
+      m_fx(fx),
+      m_fy(fy),
+      m_cx(cx),
+      m_cy(cy) {}
+
+double &PinholeCamera::Parameters::k1(void) {
+  return m_k1;
+}
+
+double &PinholeCamera::Parameters::k2(void) {
+  return m_k2;
+}
+
+double &PinholeCamera::Parameters::p1(void) {
+  return m_p1;
+}
+
+double &PinholeCamera::Parameters::p2(void) {
+  return m_p2;
+}
+
+double &PinholeCamera::Parameters::fx(void) {
+  return m_fx;
+}
+
+double &PinholeCamera::Parameters::fy(void) {
+  return m_fy;
+}
+
+double &PinholeCamera::Parameters::cx(void) {
+  return m_cx;
+}
+
+double &PinholeCamera::Parameters::cy(void) {
+  return m_cy;
+}
+
+double PinholeCamera::Parameters::k1(void) const {
+  return m_k1;
+}
+
+double PinholeCamera::Parameters::k2(void) const {
+  return m_k2;
+}
+
+double PinholeCamera::Parameters::p1(void) const {
+  return m_p1;
+}
+
+double PinholeCamera::Parameters::p2(void) const {
+  return m_p2;
+}
+
+double PinholeCamera::Parameters::fx(void) const {
+  return m_fx;
+}
+
+double PinholeCamera::Parameters::fy(void) const {
+  return m_fy;
+}
+
+double PinholeCamera::Parameters::cx(void) const {
+  return m_cx;
+}
+
+double PinholeCamera::Parameters::cy(void) const {
+  return m_cy;
+}
+
+bool PinholeCamera::Parameters::readFromYamlFile(const std::string &filename) {
+  cv::FileStorage fs(filename, cv::FileStorage::READ);
+
+  if (!fs.isOpened()) {
+    return false;
+  }
+
+  if (!fs["model_type"].isNone()) {
+    std::string sModelType;
+    fs["model_type"] >> sModelType;
+
+    if (sModelType.compare("PINHOLE") != 0) {
+      return false;
+    }
+  }
+
+  m_modelType = PINHOLE;
+  fs["camera_name"] >> m_cameraName;
+  m_imageWidth = static_cast<int>(fs["image_width"]);
+  m_imageHeight = static_cast<int>(fs["image_height"]);
+
+  cv::FileNode n = fs["distortion_parameters"];
+  m_k1 = static_cast<double>(n["k1"]);
+  m_k2 = static_cast<double>(n["k2"]);
+  m_p1 = static_cast<double>(n["p1"]);
+  m_p2 = static_cast<double>(n["p2"]);
+
+  n = fs["projection_parameters"];
+  m_fx = static_cast<double>(n["fx"]);
+  m_fy = static_cast<double>(n["fy"]);
+  m_cx = static_cast<double>(n["cx"]);
+  m_cy = static_cast<double>(n["cy"]);
+
+  return true;
+}
+
+void PinholeCamera::Parameters::writeToYamlFile(
+    const std::string &filename) const {
+  cv::FileStorage fs(filename, cv::FileStorage::WRITE);
+
+  fs << "model_type"
+     << "PINHOLE";
+  fs << "camera_name" << m_cameraName;
+  fs << "image_width" << m_imageWidth;
+  fs << "image_height" << m_imageHeight;
+
+  // radial distortion: k1, k2
+  // tangential distortion: p1, p2
+  fs << "distortion_parameters";
+  fs << "{"
+     << "k1" << m_k1 << "k2" << m_k2 << "p1" << m_p1 << "p2" << m_p2 << "}";
+
+  // projection: fx, fy, cx, cy
+  fs << "projection_parameters";
+  fs << "{"
+     << "fx" << m_fx << "fy" << m_fy << "cx" << m_cx << "cy" << m_cy << "}";
+
+  fs.release();
+}
+
+PinholeCamera::Parameters &PinholeCamera::Parameters::operator=(
+    const PinholeCamera::Parameters &other) {
+  if (this != &other) {
+    m_modelType = other.m_modelType;
+    m_cameraName = other.m_cameraName;
+    m_imageWidth = other.m_imageWidth;
+    m_imageHeight = other.m_imageHeight;
+    m_k1 = other.m_k1;
+    m_k2 = other.m_k2;
+    m_p1 = other.m_p1;
+    m_p2 = other.m_p2;
+    m_fx = other.m_fx;
+    m_fy = other.m_fy;
+    m_cx = other.m_cx;
+    m_cy = other.m_cy;
+  }
+
+  return *this;
+}
+
+std::ostream &operator<<(
+    std::ostream &out, const PinholeCamera::Parameters &params) {
+  out << "Camera Parameters:" << std::endl;
+  out << "    model_type "
+      << "PINHOLE" << std::endl;
+  out << "   camera_name " << params.m_cameraName << std::endl;
+  out << "   image_width " << params.m_imageWidth << std::endl;
+  out << "  image_height " << params.m_imageHeight << std::endl;
+
+  // radial distortion: k1, k2
+  // tangential distortion: p1, p2
+  out << "Distortion Parameters" << std::endl;
+  out << "            k1 " << params.m_k1 << std::endl
+      << "            k2 " << params.m_k2 << std::endl
+      << "            p1 " << params.m_p1 << std::endl
+      << "            p2 " << params.m_p2 << std::endl;
+
+  // projection: fx, fy, cx, cy
+  out << "Projection Parameters" << std::endl;
+  out << "            fx " << params.m_fx << std::endl
+      << "            fy " << params.m_fy << std::endl
+      << "            cx " << params.m_cx << std::endl
+      << "            cy " << params.m_cy << std::endl;
+
+  return out;
+}
+
+PinholeCamera::PinholeCamera()
+    : m_inv_K11(1.0),
+      m_inv_K13(0.0),
+      m_inv_K22(1.0),
+      m_inv_K23(0.0),
+      m_noDistortion(true) {}
+
+PinholeCamera::PinholeCamera(
+    const std::string &cameraName, int imageWidth, int imageHeight, double k1,
+    double k2, double p1, double p2, double fx, double fy, double cx, double cy)
+    : mParameters(
+          cameraName, imageWidth, imageHeight, k1, k2, p1, p2, fx, fy, cx, cy) {
+  if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) &&
+      (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) {
+    m_noDistortion = true;
+  } else {
+    m_noDistortion = false;
+  }
+
+  // Inverse camera projection matrix parameters
+  m_inv_K11 = 1.0 / mParameters.fx();
+  m_inv_K13 = -mParameters.cx() / mParameters.fx();
+  m_inv_K22 = 1.0 / mParameters.fy();
+  m_inv_K23 = -mParameters.cy() / mParameters.fy();
+}
+
+PinholeCamera::PinholeCamera(const PinholeCamera::Parameters &params)
+    : mParameters(params) {
+  if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) &&
+      (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) {
+    m_noDistortion = true;
+  } else {
+    m_noDistortion = false;
+  }
+
+  // Inverse camera projection matrix parameters
+  m_inv_K11 = 1.0 / mParameters.fx();
+  m_inv_K13 = -mParameters.cx() / mParameters.fx();
+  m_inv_K22 = 1.0 / mParameters.fy();
+  m_inv_K23 = -mParameters.cy() / mParameters.fy();
+}
+
+Camera::ModelType PinholeCamera::modelType(void) const {
+  return mParameters.modelType();
+}
+
+const std::string &PinholeCamera::cameraName(void) const {
+  return mParameters.cameraName();
+}
+
+int PinholeCamera::imageWidth(void) const {
+  return mParameters.imageWidth();
+}
+
+int PinholeCamera::imageHeight(void) const {
+  return mParameters.imageHeight();
+}
+
+void PinholeCamera::estimateIntrinsics(
+    const cv::Size &boardSize,
+    const std::vector<std::vector<cv::Point3f> > &objectPoints,
+    const std::vector<std::vector<cv::Point2f> > &imagePoints) {
+  // Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000
+
+  Parameters params = getParameters();
+
+  params.k1() = 0.0;
+  params.k2() = 0.0;
+  params.p1() = 0.0;
+  params.p2() = 0.0;
+
+  double cx = params.imageWidth() / 2.0;
+  double cy = params.imageHeight() / 2.0;
+  params.cx() = cx;
+  params.cy() = cy;
+
+  size_t nImages = imagePoints.size();
+
+  cv::Mat A(nImages * 2, 2, CV_64F);
+  cv::Mat b(nImages * 2, 1, CV_64F);
+
+  for (size_t i = 0; i < nImages; ++i) {
+    const std::vector<cv::Point3f> &oPoints = objectPoints.at(i);
+
+    std::vector<cv::Point2f> M(oPoints.size());
+    for (size_t j = 0; j < M.size(); ++j) {
+      M.at(j) = cv::Point2f(oPoints.at(j).x, oPoints.at(j).y);
+    }
+
+    cv::Mat H = cv::findHomography(M, imagePoints.at(i));
+
+    H.at<double>(0, 0) -= H.at<double>(2, 0) * cx;
+    H.at<double>(0, 1) -= H.at<double>(2, 1) * cx;
+    H.at<double>(0, 2) -= H.at<double>(2, 2) * cx;
+    H.at<double>(1, 0) -= H.at<double>(2, 0) * cy;
+    H.at<double>(1, 1) -= H.at<double>(2, 1) * cy;
+    H.at<double>(1, 2) -= H.at<double>(2, 2) * cy;
+
+    double h[3], v[3], d1[3], d2[3];
+    double n[4] = {0, 0, 0, 0};
+
+    for (int j = 0; j < 3; ++j) {
+      double t0 = H.at<double>(j, 0);
+      double t1 = H.at<double>(j, 1);
+      h[j] = t0;
+      v[j] = t1;
+      d1[j] = (t0 + t1) * 0.5;
+      d2[j] = (t0 - t1) * 0.5;
+      n[0] += t0 * t0;
+      n[1] += t1 * t1;
+      n[2] += d1[j] * d1[j];
+      n[3] += d2[j] * d2[j];
+    }
+
+    for (int j = 0; j < 4; ++j) {
+      n[j] = 1.0 / sqrt(n[j]);
+    }
+
+    for (int j = 0; j < 3; ++j) {
+      h[j] *= n[0];
+      v[j] *= n[1];
+      d1[j] *= n[2];
+      d2[j] *= n[3];
+    }
+
+    A.at<double>(i * 2, 0) = h[0] * v[0];
+    A.at<double>(i * 2, 1) = h[1] * v[1];
+    A.at<double>(i * 2 + 1, 0) = d1[0] * d2[0];
+    A.at<double>(i * 2 + 1, 1) = d1[1] * d2[1];
+    b.at<double>(i * 2, 0) = -h[2] * v[2];
+    b.at<double>(i * 2 + 1, 0) = -d1[2] * d2[2];
+  }
+
+  cv::Mat f(2, 1, CV_64F);
+  cv::solve(A, b, f, cv::DECOMP_NORMAL | cv::DECOMP_LU);
+
+  params.fx() = sqrt(fabs(1.0 / f.at<double>(0)));
+  params.fy() = sqrt(fabs(1.0 / f.at<double>(1)));
+
+  setParameters(params);
+}
+
+/**
+ * \brief Lifts a point from the image plane to the unit sphere
+ *
+ * \param p image coordinates
+ * \param P coordinates of the point on the sphere
+ */
+void PinholeCamera::liftSphere(
+    const Eigen::Vector2d &p, Eigen::Vector3d &P) const {
+  liftProjective(p, P);
+
+  P.normalize();
+}
+
+/**
+ * \brief Lifts a point from the image plane to its projective ray
+ *
+ * \param p image coordinates
+ * \param P coordinates of the projective ray
+ */
+void PinholeCamera::liftProjective(
+    const Eigen::Vector2d &p, Eigen::Vector3d &P) const {
+  double mx_d, my_d, mx2_d, mxy_d, my2_d, mx_u, my_u;
+  double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;
+  // double lambda;
+
+  // Lift points to normalised plane
+  mx_d = m_inv_K11 * p(0) + m_inv_K13;
+  my_d = m_inv_K22 * p(1) + m_inv_K23;
+
+  if (m_noDistortion) {
+    mx_u = mx_d;
+    my_u = my_d;
+  } else {
+    if (0) {
+      double k1 = mParameters.k1();
+      double k2 = mParameters.k2();
+      double p1 = mParameters.p1();
+      double p2 = mParameters.p2();
+
+      // Apply inverse distortion model
+      // proposed by Heikkila
+      mx2_d = mx_d * mx_d;
+      my2_d = my_d * my_d;
+      mxy_d = mx_d * my_d;
+      rho2_d = mx2_d + my2_d;
+      rho4_d = rho2_d * rho2_d;
+      radDist_d = k1 * rho2_d + k2 * rho4_d;
+      Dx_d = mx_d * radDist_d + p2 * (rho2_d + 2 * mx2_d) + 2 * p1 * mxy_d;
+      Dy_d = my_d * radDist_d + p1 * (rho2_d + 2 * my2_d) + 2 * p2 * mxy_d;
+      inv_denom_d = 1 / (1 + 4 * k1 * rho2_d + 6 * k2 * rho4_d + 8 * p1 * my_d +
+                         8 * p2 * mx_d);
+
+      mx_u = mx_d - inv_denom_d * Dx_d;
+      my_u = my_d - inv_denom_d * Dy_d;
+    } else {
+      // Recursive distortion model
+      int n = 8;
+      Eigen::Vector2d d_u;
+      distortion(Eigen::Vector2d(mx_d, my_d), d_u);
+      // Approximate value
+      mx_u = mx_d - d_u(0);
+      my_u = my_d - d_u(1);
+
+      for (int i = 1; i < n; ++i) {
+        distortion(Eigen::Vector2d(mx_u, my_u), d_u);
+        mx_u = mx_d - d_u(0);
+        my_u = my_d - d_u(1);
+      }
+    }
+  }
+
+  // Obtain a projective ray
+  P << mx_u, my_u, 1.0;
+}
+
+/**
+ * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v)
+ *
+ * \param P 3D point coordinates
+ * \param p return value, contains the image point coordinates
+ */
+void PinholeCamera::spaceToPlane(
+    const Eigen::Vector3d &P, Eigen::Vector2d &p) const {
+  Eigen::Vector2d p_u, p_d;
+
+  // Project points to the normalised plane
+  p_u << P(0) / P(2), P(1) / P(2);
+
+  if (m_noDistortion) {
+    p_d = p_u;
+  } else {
+    // Apply distortion
+    Eigen::Vector2d d_u;
+    distortion(p_u, d_u);
+    p_d = p_u + d_u;
+  }
+
+  // Apply generalised projection matrix
+  p << mParameters.fx() * p_d(0) + mParameters.cx(),
+      mParameters.fy() * p_d(1) + mParameters.cy();
+}
+
+#if 0
+/**
+ * \brief Project a 3D point to the image plane and calculate Jacobian
+ *
+ * \param P 3D point coordinates
+ * \param p return value, contains the image point coordinates
+ */
+void
+PinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
+                            Eigen::Matrix<double,2,3>& J) const
+{
+    Eigen::Vector2d p_u, p_d;
+    double norm, inv_denom;
+    double dxdmx, dydmx, dxdmy, dydmy;
+
+    norm = P.norm();
+    // Project points to the normalised plane
+    inv_denom = 1.0 / P(2);
+    p_u << inv_denom * P(0), inv_denom * P(1);
+
+    // Calculate jacobian
+    double dudx = inv_denom;
+    double dvdx = 0.0;
+    double dudy = 0.0;
+    double dvdy = inv_denom;
+    inv_denom = - inv_denom * inv_denom;
+    double dudz = P(0) * inv_denom;
+    double dvdz = P(1) * inv_denom;
+
+    if (m_noDistortion)
+    {
+        p_d = p_u;
+    }
+    else
+    {
+        // Apply distortion
+        Eigen::Vector2d d_u;
+        distortion(p_u, d_u);
+        p_d = p_u + d_u;
+    }
+
+    double fx = mParameters.fx();
+    double fy = mParameters.fy();
+
+    // Make the product of the jacobians
+    // and add projection matrix jacobian
+    inv_denom = fx * (dudx * dxdmx + dvdx * dxdmy); // reuse
+    dvdx = fy * (dudx * dydmx + dvdx * dydmy);
+    dudx = inv_denom;
+
+    inv_denom = fx * (dudy * dxdmx + dvdy * dxdmy); // reuse
+    dvdy = fy * (dudy * dydmx + dvdy * dydmy);
+    dudy = inv_denom;
+
+    inv_denom = fx * (dudz * dxdmx + dvdz * dxdmy); // reuse
+    dvdz = fy * (dudz * dydmx + dvdz * dydmy);
+    dudz = inv_denom;
+
+    // Apply generalised projection matrix
+    p << fx * p_d(0) + mParameters.cx(),
+         fy * p_d(1) + mParameters.cy();
+
+    J << dudx, dudy, dudz,
+         dvdx, dvdy, dvdz;
+}
+#endif
+
+/**
+ * \brief Projects an undistorted 2D point p_u to the image plane
+ *
+ * \param p_u 2D point coordinates
+ * \return image point coordinates
+ */
+void PinholeCamera::undistToPlane(
+    const Eigen::Vector2d &p_u, Eigen::Vector2d &p) const {
+  Eigen::Vector2d p_d;
+
+  if (m_noDistortion) {
+    p_d = p_u;
+  } else {
+    // Apply distortion
+    Eigen::Vector2d d_u;
+    distortion(p_u, d_u);
+    p_d = p_u + d_u;
+  }
+
+  // Apply generalised projection matrix
+  p << mParameters.fx() * p_d(0) + mParameters.cx(),
+      mParameters.fy() * p_d(1) + mParameters.cy();
+}
+
+/**
+ * \brief Apply distortion to input point (from the normalised plane)
+ *
+ * \param p_u undistorted coordinates of point on the normalised plane
+ * \return to obtain the distorted point: p_d = p_u + d_u
+ */
+void PinholeCamera::distortion(
+    const Eigen::Vector2d &p_u, Eigen::Vector2d &d_u) const {
+  double k1 = mParameters.k1();
+  double k2 = mParameters.k2();
+  double p1 = mParameters.p1();
+  double p2 = mParameters.p2();
+
+  double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;
+
+  mx2_u = p_u(0) * p_u(0);
+  my2_u = p_u(1) * p_u(1);
+  mxy_u = p_u(0) * p_u(1);
+  rho2_u = mx2_u + my2_u;
+  rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;
+  d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),
+      p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);
+}
+
+/**
+ * \brief Apply distortion to input point (from the normalised plane)
+ *        and calculate Jacobian
+ *
+ * \param p_u undistorted coordinates of point on the normalised plane
+ * \return to obtain the distorted point: p_d = p_u + d_u
+ */
+void PinholeCamera::distortion(
+    const Eigen::Vector2d &p_u, Eigen::Vector2d &d_u,
+    Eigen::Matrix2d &J) const {
+  double k1 = mParameters.k1();
+  double k2 = mParameters.k2();
+  double p1 = mParameters.p1();
+  double p2 = mParameters.p2();
+
+  double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;
+
+  mx2_u = p_u(0) * p_u(0);
+  my2_u = p_u(1) * p_u(1);
+  mxy_u = p_u(0) * p_u(1);
+  rho2_u = mx2_u + my2_u;
+  rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;
+  d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),
+      p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);
+
+  double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u +
+                 k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) +
+                 6.0 * p2 * p_u(0);
+  double dydmx = k1 * 2.0 * p_u(0) * p_u(1) +
+                 k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) +
+                 2.0 * p2 * p_u(1);
+  double dxdmy = dydmx;
+  double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u +
+                 k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) +
+                 2.0 * p2 * p_u(0);
+
+  J << dxdmx, dxdmy, dydmx, dydmy;
+}
+
+void PinholeCamera::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);
+
+  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;
+
+      Eigen::Vector3d P;
+      P << mx_u, my_u, 1.0;
+
+      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);
+}
+
+cv::Mat PinholeCamera::initUndistortRectifyMap(
+    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);
+
+  Eigen::Matrix3f R, R_inv;
+  cv::cv2eigen(rmat, R);
+  R_inv = R.inverse();
+
+  // assume no skew
+  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.fx();
+    K_rect(1, 1) = mParameters.fy();
+  }
+
+  Eigen::Matrix3f K_rect_inv = K_rect.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;
+}
+
+int PinholeCamera::parameterCount(void) const {
+  return 8;
+}
+
+const PinholeCamera::Parameters &PinholeCamera::getParameters(void) const {
+  return mParameters;
+}
+
+void PinholeCamera::setParameters(const PinholeCamera::Parameters &parameters) {
+  mParameters = parameters;
+
+  if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) &&
+      (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) {
+    m_noDistortion = true;
+  } else {
+    m_noDistortion = false;
+  }
+
+  m_inv_K11 = 1.0 / mParameters.fx();
+  m_inv_K13 = -mParameters.cx() / mParameters.fx();
+  m_inv_K22 = 1.0 / mParameters.fy();
+  m_inv_K23 = -mParameters.cy() / mParameters.fy();
+}
+
+void PinholeCamera::readParameters(const std::vector<double> &parameterVec) {
+  if ((int)parameterVec.size() != parameterCount()) {
+    return;
+  }
+
+  Parameters params = getParameters();
+
+  params.k1() = parameterVec.at(0);
+  params.k2() = parameterVec.at(1);
+  params.p1() = parameterVec.at(2);
+  params.p2() = parameterVec.at(3);
+  params.fx() = parameterVec.at(4);
+  params.fy() = parameterVec.at(5);
+  params.cx() = parameterVec.at(6);
+  params.cy() = parameterVec.at(7);
+
+  setParameters(params);
+}
+
+void PinholeCamera::writeParameters(std::vector<double> &parameterVec) const {
+  parameterVec.resize(parameterCount());
+  parameterVec.at(0) = mParameters.k1();
+  parameterVec.at(1) = mParameters.k2();
+  parameterVec.at(2) = mParameters.p1();
+  parameterVec.at(3) = mParameters.p2();
+  parameterVec.at(4) = mParameters.fx();
+  parameterVec.at(5) = mParameters.fy();
+  parameterVec.at(6) = mParameters.cx();
+  parameterVec.at(7) = mParameters.cy();
+}
+
+void PinholeCamera::writeParametersToYamlFile(
+    const std::string &filename) const {
+  mParameters.writeToYamlFile(filename);
+}
+
+std::string PinholeCamera::parametersToString(void) const {
+  std::ostringstream oss;
+  oss << mParameters;
+
+  return oss.str();
+}
+}

src/mynteye/api/camodocal/src/camera_models/ScaramuzzaCamera.cc

@@ -0,0 +1,802 @@
+#include "camodocal/camera_models/ScaramuzzaCamera.h"
+
+#include <boost/algorithm/string.hpp>
+#include <boost/lexical_cast.hpp>
+#include <cmath>
+#include <cstdio>
+#include "eigen3/Eigen/Dense"
+#include "eigen3/Eigen/SVD"
+#include <iomanip>
+#include <iostream>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/core/eigen.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include "camodocal/gpl/gpl.h"
+
+Eigen::VectorXd polyfit(
+    Eigen::VectorXd &xVec, Eigen::VectorXd &yVec, int poly_order) {
+  assert(poly_order > 0);
+  assert(xVec.size() > poly_order);
+  assert(xVec.size() == yVec.size());
+
+  Eigen::MatrixXd A(xVec.size(), poly_order + 1);
+  Eigen::VectorXd B(xVec.size());
+
+  for (int i = 0; i < xVec.size(); ++i) {
+    const double x = xVec(i);
+    const double y = yVec(i);
+
+    double x_pow_k = 1.0;
+
+    for (int k = 0; k <= poly_order; ++k) {
+      A(i, k) = x_pow_k;
+      x_pow_k *= x;
+    }
+
+    B(i) = y;
+  }
+
+  Eigen::JacobiSVD<Eigen::MatrixXd> svd(
+      A, Eigen::ComputeThinU | Eigen::ComputeThinV);
+  Eigen::VectorXd x = svd.solve(B);
+
+  return x;
+}
+
+namespace camodocal {
+
+OCAMCamera::Parameters::Parameters()
+    : Camera::Parameters(SCARAMUZZA),
+      m_C(0.0),
+      m_D(0.0),
+      m_E(0.0),
+      m_center_x(0.0),
+      m_center_y(0.0) {
+  memset(m_poly, 0, sizeof(double) * SCARAMUZZA_POLY_SIZE);
+  memset(m_inv_poly, 0, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE);
+}
+
+bool OCAMCamera::Parameters::readFromYamlFile(const std::string &filename) {
+  cv::FileStorage fs(filename, cv::FileStorage::READ);
+
+  if (!fs.isOpened()) {
+    return false;
+  }
+
+  if (!fs["model_type"].isNone()) {
+    std::string sModelType;
+    fs["model_type"] >> sModelType;
+
+    if (!boost::iequals(sModelType, "scaramuzza")) {
+      return false;
+    }
+  }
+
+  m_modelType = SCARAMUZZA;
+  fs["camera_name"] >> m_cameraName;
+  m_imageWidth = static_cast<int>(fs["image_width"]);
+  m_imageHeight = static_cast<int>(fs["image_height"]);
+
+  cv::FileNode n = fs["poly_parameters"];
+  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
+    m_poly[i] = static_cast<double>(
+        n[std::string("p") + boost::lexical_cast<std::string>(i)]);
+
+  n = fs["inv_poly_parameters"];
+  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
+    m_inv_poly[i] = static_cast<double>(
+        n[std::string("p") + boost::lexical_cast<std::string>(i)]);
+
+  n = fs["affine_parameters"];
+  m_C = static_cast<double>(n["ac"]);
+  m_D = static_cast<double>(n["ad"]);
+  m_E = static_cast<double>(n["ae"]);
+
+  m_center_x = static_cast<double>(n["cx"]);
+  m_center_y = static_cast<double>(n["cy"]);
+
+  return true;
+}
+
+void OCAMCamera::Parameters::writeToYamlFile(
+    const std::string &filename) const {
+  cv::FileStorage fs(filename, cv::FileStorage::WRITE);
+
+  fs << "model_type"
+     << "scaramuzza";
+  fs << "camera_name" << m_cameraName;
+  fs << "image_width" << m_imageWidth;
+  fs << "image_height" << m_imageHeight;
+
+  fs << "poly_parameters";
+  fs << "{";
+  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
+    fs << std::string("p") + boost::lexical_cast<std::string>(i) << m_poly[i];
+  fs << "}";
+
+  fs << "inv_poly_parameters";
+  fs << "{";
+  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
+    fs << std::string("p") + boost::lexical_cast<std::string>(i)
+       << m_inv_poly[i];
+  fs << "}";
+
+  fs << "affine_parameters";
+  fs << "{"
+     << "ac" << m_C << "ad" << m_D << "ae" << m_E << "cx" << m_center_x << "cy"
+     << m_center_y << "}";
+
+  fs.release();
+}
+
+OCAMCamera::Parameters &OCAMCamera::Parameters::operator=(
+    const OCAMCamera::Parameters &other) {
+  if (this != &other) {
+    m_modelType = other.m_modelType;
+    m_cameraName = other.m_cameraName;
+    m_imageWidth = other.m_imageWidth;
+    m_imageHeight = other.m_imageHeight;
+    m_C = other.m_C;
+    m_D = other.m_D;
+    m_E = other.m_E;
+    m_center_x = other.m_center_x;
+    m_center_y = other.m_center_y;
+
+    memcpy(m_poly, other.m_poly, sizeof(double) * SCARAMUZZA_POLY_SIZE);
+    memcpy(
+        m_inv_poly, other.m_inv_poly,
+        sizeof(double) * SCARAMUZZA_INV_POLY_SIZE);
+  }
+
+  return *this;
+}
+
+std::ostream &operator<<(
+    std::ostream &out, const OCAMCamera::Parameters &params) {
+  out << "Camera Parameters:" << std::endl;
+  out << "    model_type "
+      << "scaramuzza" << std::endl;
+  out << "   camera_name " << params.m_cameraName << std::endl;
+  out << "   image_width " << params.m_imageWidth << std::endl;
+  out << "  image_height " << params.m_imageHeight << std::endl;
+
+  out << std::fixed << std::setprecision(10);
+
+  out << "Poly Parameters" << std::endl;
+  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
+    out << std::string("p") + boost::lexical_cast<std::string>(i) << ": "
+        << params.m_poly[i] << std::endl;
+
+  out << "Inverse Poly Parameters" << std::endl;
+  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
+    out << std::string("p") + boost::lexical_cast<std::string>(i) << ": "
+        << params.m_inv_poly[i] << std::endl;
+
+  out << "Affine Parameters" << std::endl;
+  out << "            ac " << params.m_C << std::endl
+      << "            ad " << params.m_D << std::endl
+      << "            ae " << params.m_E << std::endl;
+  out << "            cx " << params.m_center_x << std::endl
+      << "            cy " << params.m_center_y << std::endl;
+
+  return out;
+}
+
+OCAMCamera::OCAMCamera() : m_inv_scale(0.0) {}
+
+OCAMCamera::OCAMCamera(const OCAMCamera::Parameters &params)
+    : mParameters(params) {
+  m_inv_scale = 1.0 / (params.C() - params.D() * params.E());
+}
+
+Camera::ModelType OCAMCamera::modelType(void) const {
+  return mParameters.modelType();
+}
+
+const std::string &OCAMCamera::cameraName(void) const {
+  return mParameters.cameraName();
+}
+
+int OCAMCamera::imageWidth(void) const {
+  return mParameters.imageWidth();
+}
+
+int OCAMCamera::imageHeight(void) const {
+  return mParameters.imageHeight();
+}
+
+void OCAMCamera::estimateIntrinsics(
+    const cv::Size &boardSize,
+    const std::vector<std::vector<cv::Point3f> > &objectPoints,
+    const std::vector<std::vector<cv::Point2f> > &imagePoints) {
+  // std::cout << "OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED" <<
+  // std::endl;
+  // throw std::string("OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED");
+
+  // Reference: Page 30 of
+  // " Scaramuzza, D. Omnidirectional Vision: from Calibration to Robot Motion
+  // Estimation, ETH Zurich. Thesis no. 17635."
+  // http://e-collection.library.ethz.ch/eserv/eth:30301/eth-30301-02.pdf
+  // Matlab code: calibrate.m
+
+  // First, estimate every image's extrinsics parameters
+  std::vector<Eigen::Matrix3d> RList;
+  std::vector<Eigen::Vector3d> TList;
+
+  RList.reserve(imagePoints.size());
+  TList.reserve(imagePoints.size());
+
+  // i-th image
+  for (size_t image_index = 0; image_index < imagePoints.size();
+       ++image_index) {
+    const std::vector<cv::Point3f> &objPts = objectPoints.at(image_index);
+    const std::vector<cv::Point2f> &imgPts = imagePoints.at(image_index);
+
+    assert(objPts.size() == imgPts.size());
+    assert(
+        objPts.size() ==
+        static_cast<unsigned int>(boardSize.width * boardSize.height));
+
+    Eigen::MatrixXd M(objPts.size(), 6);
+
+    for (size_t corner_index = 0; corner_index < objPts.size();
+         ++corner_index) {
+      double X = objPts.at(corner_index).x;
+      double Y = objPts.at(corner_index).y;
+      assert(objPts.at(corner_index).z == 0.0);
+
+      double u = imgPts.at(corner_index).x;
+      double v = imgPts.at(corner_index).y;
+
+      M(corner_index, 0) = -v * X;
+      M(corner_index, 1) = -v * Y;
+      M(corner_index, 2) = u * X;
+      M(corner_index, 3) = u * Y;
+      M(corner_index, 4) = -v;
+      M(corner_index, 5) = u;
+    }
+
+    Eigen::JacobiSVD<Eigen::MatrixXd> svd(
+        M, Eigen::ComputeFullU | Eigen::ComputeFullV);
+    assert(svd.matrixV().cols() == 6);
+    Eigen::VectorXd h = -svd.matrixV().col(5);
+
+    // scaled version of R and T
+    const double sr11 = h(0);
+    const double sr12 = h(1);
+    const double sr21 = h(2);
+    const double sr22 = h(3);
+    const double st1 = h(4);
+    const double st2 = h(5);
+
+    const double AA = square(sr11 * sr12 + sr21 * sr22);
+    const double BB = square(sr11) + square(sr21);
+    const double CC = square(sr12) + square(sr22);
+
+    const double sr32_squared_1 =
+        (-(CC - BB) + sqrt(square(CC - BB) + 4.0 * AA)) / 2.0;
+    const double sr32_squared_2 =
+        (-(CC - BB) - sqrt(square(CC - BB) + 4.0 * AA)) / 2.0;
+
+    // printf("rst = %.12f\n", sr32_squared_1*sr32_squared_1 +
+    // (CC-BB)*sr32_squared_1 - AA);
+
+    std::vector<double> sr32_squared_values;
+    if (sr32_squared_1 > 0)
+      sr32_squared_values.push_back(sr32_squared_1);
+    if (sr32_squared_2 > 0)
+      sr32_squared_values.push_back(sr32_squared_2);
+    assert(!sr32_squared_values.empty());
+
+    std::vector<double> sr32_values;
+    std::vector<double> sr31_values;
+    for (auto sr32_squared : sr32_squared_values) {
+      for (int sign = -1; sign <= 1; sign += 2) {
+        const double sr32 = static_cast<double>(sign) * std::sqrt(sr32_squared);
+        sr32_values.push_back(sr32);
+        if (sr32_squared == 0.0) {
+          // sr31 can be calculated through norm equality,
+          // but it has positive and negative posibilities
+          // positive one
+          sr31_values.push_back(std::sqrt(CC - BB));
+          // negative one
+          sr32_values.push_back(sr32);
+          sr31_values.push_back(-std::sqrt(CC - BB));
+
+          break;  // skip the same situation
+        } else {
+          // sr31 can be calculated throught dot product == 0
+          sr31_values.push_back(-(sr11 * sr12 + sr21 * sr22) / sr32);
+        }
+      }
+    }
+
+    // std::cout << "h= " << std::setprecision(12) << h.transpose() <<
+    // std::endl;
+    // std::cout << "length: " << sr32_values.size() << " & " <<
+    // sr31_values.size() << std::endl;
+
+    assert(!sr31_values.empty());
+    assert(sr31_values.size() == sr32_values.size());
+
+    std::vector<Eigen::Matrix3d> H_values;
+    for (size_t i = 0; i < sr31_values.size(); ++i) {
+      const double sr31 = sr31_values.at(i);
+      const double sr32 = sr32_values.at(i);
+      const double lambda = 1.0 / sqrt(sr11 * sr11 + sr21 * sr21 + sr31 * sr31);
+      Eigen::Matrix3d H;
+      H.setZero();
+      H(0, 0) = sr11;
+      H(0, 1) = sr12;
+      H(0, 2) = st1;
+      H(1, 0) = sr21;
+      H(1, 1) = sr22;
+      H(1, 2) = st2;
+      H(2, 0) = sr31;
+      H(2, 1) = sr32;
+      H(2, 2) = 0;
+
+      H_values.push_back(lambda * H);
+      H_values.push_back(-lambda * H);
+    }
+
+    for (auto &H : H_values) {
+      // std::cout << "H=\n" << H << std::endl;
+      Eigen::Matrix3d R;
+      R.col(0) = H.col(0);
+      R.col(1) = H.col(1);
+      R.col(2) = H.col(0).cross(H.col(1));
+      // std::cout << "R33 = " << R(2,2) << std::endl;
+    }
+
+    std::vector<Eigen::Matrix3d> H_candidates;
+
+    for (auto &H : H_values) {
+      Eigen::MatrixXd A_mat(2 * imagePoints.at(image_index).size(), 4);
+      Eigen::VectorXd B_vec(2 * imagePoints.at(image_index).size());
+      A_mat.setZero();
+      B_vec.setZero();
+
+      size_t line_index = 0;
+
+      // iterate images
+      const double &r11 = H(0, 0);
+      const double &r12 = H(0, 1);
+      // const double& r13 = H(0,2);
+      const double &r21 = H(1, 0);
+      const double &r22 = H(1, 1);
+      // const double& r23 = H(1,2);
+      const double &r31 = H(2, 0);
+      const double &r32 = H(2, 1);
+      // const double& r33 = H(2,2);
+      const double &t1 = H(0);
+      const double &t2 = H(1);
+
+      // iterate chessboard corners in the image
+      for (size_t j = 0; j < imagePoints.at(image_index).size(); ++j) {
+        assert(line_index == 2 * j);
+
+        const double &X = objectPoints.at(image_index).at(j).x;
+        const double &Y = objectPoints.at(image_index).at(j).y;
+        const double &u = imagePoints.at(image_index).at(j).x;
+        const double &v = imagePoints.at(image_index).at(j).y;
+
+        double A = r21 * X + r22 * Y + t2;
+        double B = v * (r31 * X + r32 * Y);
+        double C = r11 * X + r12 * Y + t1;
+        double D = u * (r31 * X + r32 * Y);
+        double rou = std::sqrt(u * u + v * v);
+
+        A_mat(line_index + 0, 0) = A;
+        A_mat(line_index + 1, 0) = C;
+        A_mat(line_index + 0, 1) = A * rou;
+        A_mat(line_index + 1, 1) = C * rou;
+        A_mat(line_index + 0, 2) = A * rou * rou;
+        A_mat(line_index + 1, 2) = C * rou * rou;
+
+        A_mat(line_index + 0, 3) = -v;
+        A_mat(line_index + 1, 3) = -u;
+        B_vec(line_index + 0) = B;
+        B_vec(line_index + 1) = D;
+
+        line_index += 2;
+      }
+
+      assert(line_index == static_cast<unsigned int>(A_mat.rows()));
+
+      // pseudo-inverse for polynomial parameters and all t3s
+      {
+        Eigen::JacobiSVD<Eigen::MatrixXd> svd(
+            A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+        Eigen::VectorXd x = svd.solve(B_vec);
+
+        // std::cout << "x(poly and t3) = " << x << std::endl;
+
+        if (x(2) > 0 && x(3) > 0) {
+          H_candidates.push_back(H);
+        }
+      }
+    }
+
+    // printf("H_candidates.size()=%zu\n", H_candidates.size());
+    assert(H_candidates.size() == 1);
+
+    Eigen::Matrix3d &H = H_candidates.front();
+
+    Eigen::Matrix3d R;
+    R.col(0) = H.col(0);
+    R.col(1) = H.col(1);
+    R.col(2) = H.col(0).cross(H.col(1));
+
+    Eigen::Vector3d T = H.col(2);
+    RList.push_back(R);
+    TList.push_back(T);
+
+    // std::cout << "#" << image_index << " frame" << " R =" << R << " \nT = "
+    // << T.transpose() << std::endl;
+  }
+
+  // Second, estimate camera intrinsic parameters and all t3
+  Eigen::MatrixXd A_mat(
+      2 * imagePoints.size() * imagePoints.at(0).size(),
+      SCARAMUZZA_POLY_SIZE - 1 + imagePoints.size());
+  Eigen::VectorXd B_vec(2 * imagePoints.size() * imagePoints.at(0).size());
+  A_mat.setZero();
+  B_vec.setZero();
+
+  size_t line_index = 0;
+
+  // iterate images
+  for (size_t i = 0; i < imagePoints.size(); ++i) {
+    const double &r11 = RList.at(i)(0, 0);
+    const double &r12 = RList.at(i)(0, 1);
+    // const double& r13 = RList.at(i)(0,2);
+    const double &r21 = RList.at(i)(1, 0);
+    const double &r22 = RList.at(i)(1, 1);
+    // const double& r23 = RList.at(i)(1,2);
+    const double &r31 = RList.at(i)(2, 0);
+    const double &r32 = RList.at(i)(2, 1);
+    // const double& r33 = RList.at(i)(2,2);
+    const double &t1 = TList.at(i)(0);
+    const double &t2 = TList.at(i)(1);
+
+    // iterate chessboard corners in the image
+    for (size_t j = 0; j < imagePoints.at(i).size(); ++j) {
+      assert(line_index == 2 * (i * imagePoints.at(0).size() + j));
+
+      const double &X = objectPoints.at(i).at(j).x;
+      const double &Y = objectPoints.at(i).at(j).y;
+      const double &u = imagePoints.at(i).at(j).x;
+      const double &v = imagePoints.at(i).at(j).y;
+
+      double A = r21 * X + r22 * Y + t2;
+      double B = v * (r31 * X + r32 * Y);
+      double C = r11 * X + r12 * Y + t1;
+      double D = u * (r31 * X + r32 * Y);
+      double rou = std::sqrt(u * u + v * v);
+
+      for (int k = 1; k <= SCARAMUZZA_POLY_SIZE - 1; ++k) {
+        double pow_rou = 0.0;
+        if (k == 1) {
+          pow_rou = 1.0;
+        } else {
+          pow_rou = std::pow(rou, k);
+        }
+
+        A_mat(line_index + 0, k - 1) = A * pow_rou;
+        A_mat(line_index + 1, k - 1) = C * pow_rou;
+      }
+
+      A_mat(line_index + 0, SCARAMUZZA_POLY_SIZE - 1 + i) = -v;
+      A_mat(line_index + 1, SCARAMUZZA_POLY_SIZE - 1 + i) = -u;
+      B_vec(line_index + 0) = B;
+      B_vec(line_index + 1) = D;
+
+      line_index += 2;
+    }
+  }
+
+  assert(line_index == static_cast<unsigned int>(A_mat.rows()));
+
+  Eigen::Matrix<double, SCARAMUZZA_POLY_SIZE, 1> poly_coeff;
+  // pseudo-inverse for polynomial parameters and all t3s
+  {
+    Eigen::JacobiSVD<Eigen::MatrixXd> svd(
+        A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+    Eigen::VectorXd x = svd.solve(B_vec);
+
+    poly_coeff[0] = x(0);
+    poly_coeff[1] = 0.0;
+    for (int i = 1; i < poly_coeff.size() - 1; ++i) {
+      poly_coeff[i + 1] = x(i);
+    }
+    assert(
+        x.size() ==
+        static_cast<unsigned int>(SCARAMUZZA_POLY_SIZE - 1 + TList.size()));
+  }
+
+  Parameters params = getParameters();
+
+  // Affine matrix A is constructed as [C D; E 1]
+  params.C() = 1.0;
+  params.D() = 0.0;
+  params.E() = 0.0;
+
+  params.center_x() = params.imageWidth() / 2.0;
+  params.center_y() = params.imageHeight() / 2.0;
+
+  for (size_t i = 0; i < SCARAMUZZA_POLY_SIZE; ++i) {
+    params.poly(i) = poly_coeff[i];
+  }
+
+  // params.poly(0) = -216.9657476318;
+  // params.poly(1) = 0.0;
+  // params.poly(2) = 0.0017866911;
+  // params.poly(3) = -0.0000019866;
+  // params.poly(4) =  0.0000000077;
+
+  // inv_poly
+  {
+    std::vector<double> rou_vec;
+    std::vector<double> z_vec;
+    for (double rou = 0.0;
+         rou <= (params.imageWidth() + params.imageHeight()) / 2; rou += 0.1) {
+      double rou_pow_k = 1.0;
+      double z = 0.0;
+
+      for (int k = 0; k < SCARAMUZZA_POLY_SIZE; k++) {
+        z += rou_pow_k * params.poly(k);
+        rou_pow_k *= rou;
+      }
+
+      rou_vec.push_back(rou);
+      z_vec.push_back(z);
+    }
+
+    assert(rou_vec.size() == z_vec.size());
+    Eigen::VectorXd xVec(rou_vec.size());
+    Eigen::VectorXd yVec(rou_vec.size());
+
+    for (size_t i = 0; i < rou_vec.size(); ++i) {
+      xVec(i) = std::atan2(-z_vec.at(i), rou_vec.at(i));
+      yVec(i) = rou_vec.at(i);
+    }
+
+    // use lower order poly to eliminate over-fitting cause by noisy/inaccurate
+    // data
+    const int poly_fit_order = 4;
+    Eigen::VectorXd inv_poly_coeff = polyfit(xVec, yVec, poly_fit_order);
+
+    for (int i = 0; i <= poly_fit_order; ++i) {
+      params.inv_poly(i) = inv_poly_coeff(i);
+    }
+  }
+
+  setParameters(params);
+
+  std::cout << "initial params:\n" << params << std::endl;
+}
+
+/**
+ * \brief Lifts a point from the image plane to the unit sphere
+ *
+ * \param p image coordinates
+ * \param P coordinates of the point on the sphere
+ */
+void OCAMCamera::liftSphere(
+    const Eigen::Vector2d &p, Eigen::Vector3d &P) const {
+  liftProjective(p, P);
+  P.normalize();
+}
+
+/**
+ * \brief Lifts a point from the image plane to its projective ray
+ *
+ * \param p image coordinates
+ * \param P coordinates of the projective ray
+ */
+void OCAMCamera::liftProjective(
+    const Eigen::Vector2d &p, Eigen::Vector3d &P) const {
+  // Relative to Center
+  Eigen::Vector2d xc(
+      p[0] - mParameters.center_x(), p[1] - mParameters.center_y());
+
+  // Affine Transformation
+  // xc_a = inv(A) * xc;
+  Eigen::Vector2d xc_a(
+      m_inv_scale * (xc[0] - mParameters.D() * xc[1]),
+      m_inv_scale * (-mParameters.E() * xc[0] + mParameters.C() * xc[1]));
+
+  double phi = std::sqrt(xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1]);
+  double phi_i = 1.0;
+  double z = 0.0;
+
+  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) {
+    z += phi_i * mParameters.poly(i);
+    phi_i *= phi;
+  }
+
+  P << xc[0], xc[1], -z;
+}
+
+/**
+ * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v)
+ *
+ * \param P 3D point coordinates
+ * \param p return value, contains the image point coordinates
+ */
+void OCAMCamera::spaceToPlane(
+    const Eigen::Vector3d &P, Eigen::Vector2d &p) const {
+  double norm = std::sqrt(P[0] * P[0] + P[1] * P[1]);
+  double theta = std::atan2(-P[2], norm);
+  double rho = 0.0;
+  double theta_i = 1.0;
+
+  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) {
+    rho += theta_i * mParameters.inv_poly(i);
+    theta_i *= theta;
+  }
+
+  double invNorm = 1.0 / norm;
+  Eigen::Vector2d xn(P[0] * invNorm * rho, P[1] * invNorm * rho);
+
+  p << xn[0] * mParameters.C() + xn[1] * mParameters.D() +
+           mParameters.center_x(),
+      xn[0] * mParameters.E() + xn[1] + mParameters.center_y();
+}
+
+/**
+ * \brief Projects an undistorted 2D point p_u to the image plane
+ *
+ * \param p_u 2D point coordinates
+ * \return image point coordinates
+ */
+void OCAMCamera::undistToPlane(
+    const Eigen::Vector2d &p_u, Eigen::Vector2d &p) const {
+  Eigen::Vector3d P(p_u[0], p_u[1], 1.0);
+  spaceToPlane(P, p);
+}
+
+#if 0
+void
+OCAMCamera::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);
+
+    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 xi = mParameters.xi();
+            double d2 = mx_u * mx_u + my_u * my_u;
+
+            Eigen::Vector3d P;
+            P << mx_u, my_u, 1.0 - xi * (d2 + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * d2));
+
+            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);
+}
+#endif
+
+cv::Mat OCAMCamera::initUndistortRectifyMap(
+    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);
+
+  Eigen::Matrix3f K_rect;
+
+  K_rect << fx, 0, cx < 0 ? imageSize.width / 2 : cx, 0, fy,
+      cy < 0 ? imageSize.height / 2 : cy, 0, 0, 1;
+
+  if (fx < 0 || fy < 0) {
+    throw std::string(
+        std::string(__FUNCTION__) + ": Focal length must be specified");
+  }
+
+  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;
+}
+
+int OCAMCamera::parameterCount(void) const {
+  return SCARAMUZZA_CAMERA_NUM_PARAMS;
+}
+
+const OCAMCamera::Parameters &OCAMCamera::getParameters(void) const {
+  return mParameters;
+}
+
+void OCAMCamera::setParameters(const OCAMCamera::Parameters &parameters) {
+  mParameters = parameters;
+
+  m_inv_scale = 1.0 / (parameters.C() - parameters.D() * parameters.E());
+}
+
+void OCAMCamera::readParameters(const std::vector<double> &parameterVec) {
+  if ((int)parameterVec.size() != parameterCount()) {
+    return;
+  }
+
+  Parameters params = getParameters();
+
+  params.C() = parameterVec.at(0);
+  params.D() = parameterVec.at(1);
+  params.E() = parameterVec.at(2);
+  params.center_x() = parameterVec.at(3);
+  params.center_y() = parameterVec.at(4);
+  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
+    params.poly(i) = parameterVec.at(5 + i);
+  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
+    params.inv_poly(i) = parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i);
+
+  setParameters(params);
+}
+
+void OCAMCamera::writeParameters(std::vector<double> &parameterVec) const {
+  parameterVec.resize(parameterCount());
+  parameterVec.at(0) = mParameters.C();
+  parameterVec.at(1) = mParameters.D();
+  parameterVec.at(2) = mParameters.E();
+  parameterVec.at(3) = mParameters.center_x();
+  parameterVec.at(4) = mParameters.center_y();
+  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
+    parameterVec.at(5 + i) = mParameters.poly(i);
+  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
+    parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i) = mParameters.inv_poly(i);
+}
+
+void OCAMCamera::writeParametersToYamlFile(const std::string &filename) const {
+  mParameters.writeToYamlFile(filename);
+}
+
+std::string OCAMCamera::parametersToString(void) const {
+  std::ostringstream oss;
+  oss << mParameters;
+
+  return oss.str();
+}
+}

src/mynteye/api/camodocal/src/gpl/EigenQuaternionParameterization.cc

@@ -0,0 +1,43 @@
+#include "camodocal/gpl/EigenQuaternionParameterization.h"
+
+#include <cmath>
+
+namespace camodocal {
+
+bool EigenQuaternionParameterization::Plus(
+    const double *x, const double *delta, double *x_plus_delta) const {
+  const double norm_delta =
+      sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
+  if (norm_delta > 0.0) {
+    const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
+    double q_delta[4];
+    q_delta[0] = sin_delta_by_delta * delta[0];
+    q_delta[1] = sin_delta_by_delta * delta[1];
+    q_delta[2] = sin_delta_by_delta * delta[2];
+    q_delta[3] = cos(norm_delta);
+    EigenQuaternionProduct(q_delta, x, x_plus_delta);
+  } else {
+    for (int i = 0; i < 4; ++i) {
+      x_plus_delta[i] = x[i];
+    }
+  }
+  return true;
+}
+
+bool EigenQuaternionParameterization::ComputeJacobian(
+    const double *x, double *jacobian) const {
+  jacobian[0] = x[3];
+  jacobian[1] = x[2];
+  jacobian[2] = -x[1];  // NOLINT
+  jacobian[3] = -x[2];
+  jacobian[4] = x[3];
+  jacobian[5] = x[0];  // NOLINT
+  jacobian[6] = x[1];
+  jacobian[7] = -x[0];
+  jacobian[8] = x[3];  // NOLINT
+  jacobian[9] = -x[0];
+  jacobian[10] = -x[1];
+  jacobian[11] = -x[2];  // NOLINT
+  return true;
+}
+}

src/mynteye/api/camodocal/src/gpl/gpl.cc

@@ -0,0 +1,754 @@
+#include "camodocal/gpl/gpl.h"
+
+#include <set>
+#ifdef _WIN32
+#include <winsock.h>
+#else
+#include <time.h>
+#endif
+
+// source:
+// https://stackoverflow.com/questions/5167269/clock-gettime-alternative-in-mac-os-x
+#ifdef __APPLE__
+#include <mach/mach_time.h>
+#define ORWL_NANO (+1.0E-9)
+#define ORWL_GIGA UINT64_C(1000000000)
+
+static double orwl_timebase = 0.0;
+static uint64_t orwl_timestart = 0;
+
+struct timespec orwl_gettime(void) {
+  // be more careful in a multithreaded environement
+  if (!orwl_timestart) {
+    mach_timebase_info_data_t tb = {0};
+    mach_timebase_info(&tb);
+    orwl_timebase = tb.numer;
+    orwl_timebase /= tb.denom;
+    orwl_timestart = mach_absolute_time();
+  }
+  struct timespec t;
+  double diff = (mach_absolute_time() - orwl_timestart) * orwl_timebase;
+  t.tv_sec = diff * ORWL_NANO;
+  t.tv_nsec = diff - (t.tv_sec * ORWL_GIGA);
+  return t;
+}
+#endif  // __APPLE__
+
+const double WGS84_A = 6378137.0;
+const double WGS84_ECCSQ = 0.00669437999013;
+
+// Windows lacks fminf
+#ifndef fminf
+#define fminf(x, y) (((x) < (y)) ? (x) : (y))
+#endif
+
+namespace camodocal {
+
+double hypot3(double x, double y, double z) {
+  return sqrt(square(x) + square(y) + square(z));
+}
+
+float hypot3f(float x, float y, float z) {
+  return sqrtf(square(x) + square(y) + square(z));
+}
+
+double d2r(double deg) {
+  return deg / 180.0 * M_PI;
+}
+
+float d2r(float deg) {
+  return deg / 180.0f * M_PI;
+}
+
+double r2d(double rad) {
+  return rad / M_PI * 180.0;
+}
+
+float r2d(float rad) {
+  return rad / M_PI * 180.0f;
+}
+
+double sinc(double theta) {
+  return sin(theta) / theta;
+}
+
+#ifdef _WIN32
+#include <sys/timeb.h>
+#include <sys/types.h>
+#include <winsock.h>
+LARGE_INTEGER
+getFILETIMEoffset() {
+  SYSTEMTIME s;
+  FILETIME f;
+  LARGE_INTEGER t;
+
+  s.wYear = 1970;
+  s.wMonth = 1;
+  s.wDay = 1;
+  s.wHour = 0;
+  s.wMinute = 0;
+  s.wSecond = 0;
+  s.wMilliseconds = 0;
+  SystemTimeToFileTime(&s, &f);
+  t.QuadPart = f.dwHighDateTime;
+  t.QuadPart <<= 32;
+  t.QuadPart |= f.dwLowDateTime;
+  return (t);
+}
+
+int clock_gettime(int X, struct timespec *tp) {
+  LARGE_INTEGER t;
+  FILETIME f;
+  double microseconds;
+  static LARGE_INTEGER offset;
+  static double frequencyToMicroseconds;
+  static int initialized = 0;
+  static BOOL usePerformanceCounter = 0;
+
+  if (!initialized) {
+    LARGE_INTEGER performanceFrequency;
+    initialized = 1;
+    usePerformanceCounter = QueryPerformanceFrequency(&performanceFrequency);
+    if (usePerformanceCounter) {
+      QueryPerformanceCounter(&offset);
+      frequencyToMicroseconds =
+          (double)performanceFrequency.QuadPart / 1000000.;
+    } else {
+      offset = getFILETIMEoffset();
+      frequencyToMicroseconds = 10.;
+    }
+  }
+  if (usePerformanceCounter)
+    QueryPerformanceCounter(&t);
+  else {
+    GetSystemTimeAsFileTime(&f);
+    t.QuadPart = f.dwHighDateTime;
+    t.QuadPart <<= 32;
+    t.QuadPart |= f.dwLowDateTime;
+  }
+
+  t.QuadPart -= offset.QuadPart;
+  microseconds = (double)t.QuadPart / frequencyToMicroseconds;
+  t.QuadPart = microseconds;
+  tp->tv_sec = t.QuadPart / 1000000;
+  tp->tv_nsec = (t.QuadPart % 1000000) * 1000;
+  return (0);
+}
+#endif
+
+unsigned long long timeInMicroseconds(void) {
+  struct timespec tp;
+#ifdef __APPLE__
+  tp = orwl_gettime();
+#else
+  clock_gettime(CLOCK_REALTIME, &tp);
+#endif
+
+  return tp.tv_sec * 1000000 + tp.tv_nsec / 1000;
+}
+
+double timeInSeconds(void) {
+  struct timespec tp;
+#ifdef __APPLE__
+  tp = orwl_gettime();
+#else
+  clock_gettime(CLOCK_REALTIME, &tp);
+#endif
+
+  return static_cast<double>(tp.tv_sec) +
+         static_cast<double>(tp.tv_nsec) / 1000000000.0;
+}
+
+float colormapAutumn[128][3] = {
+    {1.0f, 0.f, 0.f},       {1.0f, 0.007874f, 0.f}, {1.0f, 0.015748f, 0.f},
+    {1.0f, 0.023622f, 0.f}, {1.0f, 0.031496f, 0.f}, {1.0f, 0.03937f, 0.f},
+    {1.0f, 0.047244f, 0.f}, {1.0f, 0.055118f, 0.f}, {1.0f, 0.062992f, 0.f},
+    {1.0f, 0.070866f, 0.f}, {1.0f, 0.07874f, 0.f},  {1.0f, 0.086614f, 0.f},
+    {1.0f, 0.094488f, 0.f}, {1.0f, 0.10236f, 0.f},  {1.0f, 0.11024f, 0.f},
+    {1.0f, 0.11811f, 0.f},  {1.0f, 0.12598f, 0.f},  {1.0f, 0.13386f, 0.f},
+    {1.0f, 0.14173f, 0.f},  {1.0f, 0.14961f, 0.f},  {1.0f, 0.15748f, 0.f},
+    {1.0f, 0.16535f, 0.f},  {1.0f, 0.17323f, 0.f},  {1.0f, 0.1811f, 0.f},
+    {1.0f, 0.18898f, 0.f},  {1.0f, 0.19685f, 0.f},  {1.0f, 0.20472f, 0.f},
+    {1.0f, 0.2126f, 0.f},   {1.0f, 0.22047f, 0.f},  {1.0f, 0.22835f, 0.f},
+    {1.0f, 0.23622f, 0.f},  {1.0f, 0.24409f, 0.f},  {1.0f, 0.25197f, 0.f},
+    {1.0f, 0.25984f, 0.f},  {1.0f, 0.26772f, 0.f},  {1.0f, 0.27559f, 0.f},
+    {1.0f, 0.28346f, 0.f},  {1.0f, 0.29134f, 0.f},  {1.0f, 0.29921f, 0.f},
+    {1.0f, 0.30709f, 0.f},  {1.0f, 0.31496f, 0.f},  {1.0f, 0.32283f, 0.f},
+    {1.0f, 0.33071f, 0.f},  {1.0f, 0.33858f, 0.f},  {1.0f, 0.34646f, 0.f},
+    {1.0f, 0.35433f, 0.f},  {1.0f, 0.3622f, 0.f},   {1.0f, 0.37008f, 0.f},
+    {1.0f, 0.37795f, 0.f},  {1.0f, 0.38583f, 0.f},  {1.0f, 0.3937f, 0.f},
+    {1.0f, 0.40157f, 0.f},  {1.0f, 0.40945f, 0.f},  {1.0f, 0.41732f, 0.f},
+    {1.0f, 0.4252f, 0.f},   {1.0f, 0.43307f, 0.f},  {1.0f, 0.44094f, 0.f},
+    {1.0f, 0.44882f, 0.f},  {1.0f, 0.45669f, 0.f},  {1.0f, 0.46457f, 0.f},
+    {1.0f, 0.47244f, 0.f},  {1.0f, 0.48031f, 0.f},  {1.0f, 0.48819f, 0.f},
+    {1.0f, 0.49606f, 0.f},  {1.0f, 0.50394f, 0.f},  {1.0f, 0.51181f, 0.f},
+    {1.0f, 0.51969f, 0.f},  {1.0f, 0.52756f, 0.f},  {1.0f, 0.53543f, 0.f},
+    {1.0f, 0.54331f, 0.f},  {1.0f, 0.55118f, 0.f},  {1.0f, 0.55906f, 0.f},
+    {1.0f, 0.56693f, 0.f},  {1.0f, 0.5748f, 0.f},   {1.0f, 0.58268f, 0.f},
+    {1.0f, 0.59055f, 0.f},  {1.0f, 0.59843f, 0.f},  {1.0f, 0.6063f, 0.f},
+    {1.0f, 0.61417f, 0.f},  {1.0f, 0.62205f, 0.f},  {1.0f, 0.62992f, 0.f},
+    {1.0f, 0.6378f, 0.f},   {1.0f, 0.64567f, 0.f},  {1.0f, 0.65354f, 0.f},
+    {1.0f, 0.66142f, 0.f},  {1.0f, 0.66929f, 0.f},  {1.0f, 0.67717f, 0.f},
+    {1.0f, 0.68504f, 0.f},  {1.0f, 0.69291f, 0.f},  {1.0f, 0.70079f, 0.f},
+    {1.0f, 0.70866f, 0.f},  {1.0f, 0.71654f, 0.f},  {1.0f, 0.72441f, 0.f},
+    {1.0f, 0.73228f, 0.f},  {1.0f, 0.74016f, 0.f},  {1.0f, 0.74803f, 0.f},
+    {1.0f, 0.75591f, 0.f},  {1.0f, 0.76378f, 0.f},  {1.0f, 0.77165f, 0.f},
+    {1.0f, 0.77953f, 0.f},  {1.0f, 0.7874f, 0.f},   {1.0f, 0.79528f, 0.f},
+    {1.0f, 0.80315f, 0.f},  {1.0f, 0.81102f, 0.f},  {1.0f, 0.8189f, 0.f},
+    {1.0f, 0.82677f, 0.f},  {1.0f, 0.83465f, 0.f},  {1.0f, 0.84252f, 0.f},
+    {1.0f, 0.85039f, 0.f},  {1.0f, 0.85827f, 0.f},  {1.0f, 0.86614f, 0.f},
+    {1.0f, 0.87402f, 0.f},  {1.0f, 0.88189f, 0.f},  {1.0f, 0.88976f, 0.f},
+    {1.0f, 0.89764f, 0.f},  {1.0f, 0.90551f, 0.f},  {1.0f, 0.91339f, 0.f},
+    {1.0f, 0.92126f, 0.f},  {1.0f, 0.92913f, 0.f},  {1.0f, 0.93701f, 0.f},
+    {1.0f, 0.94488f, 0.f},  {1.0f, 0.95276f, 0.f},  {1.0f, 0.96063f, 0.f},
+    {1.0f, 0.9685f, 0.f},   {1.0f, 0.97638f, 0.f},  {1.0f, 0.98425f, 0.f},
+    {1.0f, 0.99213f, 0.f},  {1.0f, 1.0f, 0.0f}};
+
+float colormapJet[128][3] = {
+    {0.0f, 0.0f, 0.53125f},     {0.0f, 0.0f, 0.5625f},
+    {0.0f, 0.0f, 0.59375f},     {0.0f, 0.0f, 0.625f},
+    {0.0f, 0.0f, 0.65625f},     {0.0f, 0.0f, 0.6875f},
+    {0.0f, 0.0f, 0.71875f},     {0.0f, 0.0f, 0.75f},
+    {0.0f, 0.0f, 0.78125f},     {0.0f, 0.0f, 0.8125f},
+    {0.0f, 0.0f, 0.84375f},     {0.0f, 0.0f, 0.875f},
+    {0.0f, 0.0f, 0.90625f},     {0.0f, 0.0f, 0.9375f},
+    {0.0f, 0.0f, 0.96875f},     {0.0f, 0.0f, 1.0f},
+    {0.0f, 0.03125f, 1.0f},     {0.0f, 0.0625f, 1.0f},
+    {0.0f, 0.09375f, 1.0f},     {0.0f, 0.125f, 1.0f},
+    {0.0f, 0.15625f, 1.0f},     {0.0f, 0.1875f, 1.0f},
+    {0.0f, 0.21875f, 1.0f},     {0.0f, 0.25f, 1.0f},
+    {0.0f, 0.28125f, 1.0f},     {0.0f, 0.3125f, 1.0f},
+    {0.0f, 0.34375f, 1.0f},     {0.0f, 0.375f, 1.0f},
+    {0.0f, 0.40625f, 1.0f},     {0.0f, 0.4375f, 1.0f},
+    {0.0f, 0.46875f, 1.0f},     {0.0f, 0.5f, 1.0f},
+    {0.0f, 0.53125f, 1.0f},     {0.0f, 0.5625f, 1.0f},
+    {0.0f, 0.59375f, 1.0f},     {0.0f, 0.625f, 1.0f},
+    {0.0f, 0.65625f, 1.0f},     {0.0f, 0.6875f, 1.0f},
+    {0.0f, 0.71875f, 1.0f},     {0.0f, 0.75f, 1.0f},
+    {0.0f, 0.78125f, 1.0f},     {0.0f, 0.8125f, 1.0f},
+    {0.0f, 0.84375f, 1.0f},     {0.0f, 0.875f, 1.0f},
+    {0.0f, 0.90625f, 1.0f},     {0.0f, 0.9375f, 1.0f},
+    {0.0f, 0.96875f, 1.0f},     {0.0f, 1.0f, 1.0f},
+    {0.03125f, 1.0f, 0.96875f}, {0.0625f, 1.0f, 0.9375f},
+    {0.09375f, 1.0f, 0.90625f}, {0.125f, 1.0f, 0.875f},
+    {0.15625f, 1.0f, 0.84375f}, {0.1875f, 1.0f, 0.8125f},
+    {0.21875f, 1.0f, 0.78125f}, {0.25f, 1.0f, 0.75f},
+    {0.28125f, 1.0f, 0.71875f}, {0.3125f, 1.0f, 0.6875f},
+    {0.34375f, 1.0f, 0.65625f}, {0.375f, 1.0f, 0.625f},
+    {0.40625f, 1.0f, 0.59375f}, {0.4375f, 1.0f, 0.5625f},
+    {0.46875f, 1.0f, 0.53125f}, {0.5f, 1.0f, 0.5f},
+    {0.53125f, 1.0f, 0.46875f}, {0.5625f, 1.0f, 0.4375f},
+    {0.59375f, 1.0f, 0.40625f}, {0.625f, 1.0f, 0.375f},
+    {0.65625f, 1.0f, 0.34375f}, {0.6875f, 1.0f, 0.3125f},
+    {0.71875f, 1.0f, 0.28125f}, {0.75f, 1.0f, 0.25f},
+    {0.78125f, 1.0f, 0.21875f}, {0.8125f, 1.0f, 0.1875f},
+    {0.84375f, 1.0f, 0.15625f}, {0.875f, 1.0f, 0.125f},
+    {0.90625f, 1.0f, 0.09375f}, {0.9375f, 1.0f, 0.0625f},
+    {0.96875f, 1.0f, 0.03125f}, {1.0f, 1.0f, 0.0f},
+    {1.0f, 0.96875f, 0.0f},     {1.0f, 0.9375f, 0.0f},
+    {1.0f, 0.90625f, 0.0f},     {1.0f, 0.875f, 0.0f},
+    {1.0f, 0.84375f, 0.0f},     {1.0f, 0.8125f, 0.0f},
+    {1.0f, 0.78125f, 0.0f},     {1.0f, 0.75f, 0.0f},
+    {1.0f, 0.71875f, 0.0f},     {1.0f, 0.6875f, 0.0f},
+    {1.0f, 0.65625f, 0.0f},     {1.0f, 0.625f, 0.0f},
+    {1.0f, 0.59375f, 0.0f},     {1.0f, 0.5625f, 0.0f},
+    {1.0f, 0.53125f, 0.0f},     {1.0f, 0.5f, 0.0f},
+    {1.0f, 0.46875f, 0.0f},     {1.0f, 0.4375f, 0.0f},
+    {1.0f, 0.40625f, 0.0f},     {1.0f, 0.375f, 0.0f},
+    {1.0f, 0.34375f, 0.0f},     {1.0f, 0.3125f, 0.0f},
+    {1.0f, 0.28125f, 0.0f},     {1.0f, 0.25f, 0.0f},
+    {1.0f, 0.21875f, 0.0f},     {1.0f, 0.1875f, 0.0f},
+    {1.0f, 0.15625f, 0.0f},     {1.0f, 0.125f, 0.0f},
+    {1.0f, 0.09375f, 0.0f},     {1.0f, 0.0625f, 0.0f},
+    {1.0f, 0.03125f, 0.0f},     {1.0f, 0.0f, 0.0f},
+    {0.96875f, 0.0f, 0.0f},     {0.9375f, 0.0f, 0.0f},
+    {0.90625f, 0.0f, 0.0f},     {0.875f, 0.0f, 0.0f},
+    {0.84375f, 0.0f, 0.0f},     {0.8125f, 0.0f, 0.0f},
+    {0.78125f, 0.0f, 0.0f},     {0.75f, 0.0f, 0.0f},
+    {0.71875f, 0.0f, 0.0f},     {0.6875f, 0.0f, 0.0f},
+    {0.65625f, 0.0f, 0.0f},     {0.625f, 0.0f, 0.0f},
+    {0.59375f, 0.0f, 0.0f},     {0.5625f, 0.0f, 0.0f},
+    {0.53125f, 0.0f, 0.0f},     {0.5f, 0.0f, 0.0f}};
+
+void colorDepthImage(
+    cv::Mat &imgDepth, cv::Mat &imgColoredDepth, float minRange,
+    float maxRange) {
+  imgColoredDepth = cv::Mat::zeros(imgDepth.size(), CV_8UC3);
+
+  for (int i = 0; i < imgColoredDepth.rows; ++i) {
+    const float *depth = imgDepth.ptr<float>(i);
+    unsigned char *pixel = imgColoredDepth.ptr<unsigned char>(i);
+    for (int j = 0; j < imgColoredDepth.cols; ++j) {
+      if (depth[j] != 0) {
+        int idx = fminf(depth[j] - minRange, maxRange - minRange) /
+                  (maxRange - minRange) * 127.0f;
+        idx = 127 - idx;
+
+        pixel[0] = colormapJet[idx][2] * 255.0f;
+        pixel[1] = colormapJet[idx][1] * 255.0f;
+        pixel[2] = colormapJet[idx][0] * 255.0f;
+      }
+
+      pixel += 3;
+    }
+  }
+}
+
+bool colormap(
+    const std::string &name, unsigned char idx, float &r, float &g, float &b) {
+  if (name.compare("jet") == 0) {
+    float *color = colormapJet[idx];
+
+    r = color[0];
+    g = color[1];
+    b = color[2];
+
+    return true;
+  } else if (name.compare("autumn") == 0) {
+    float *color = colormapAutumn[idx];
+
+    r = color[0];
+    g = color[1];
+    b = color[2];
+
+    return true;
+  }
+
+  return false;
+}
+
+std::vector<cv::Point2i> bresLine(int x0, int y0, int x1, int y1) {
+  // Bresenham's line algorithm
+  // Find cells intersected by line between (x0,y0) and (x1,y1)
+
+  std::vector<cv::Point2i> cells;
+
+  int dx = std::abs(x1 - x0);
+  int dy = std::abs(y1 - y0);
+
+  int sx = (x0 < x1) ? 1 : -1;
+  int sy = (y0 < y1) ? 1 : -1;
+
+  int err = dx - dy;
+
+  while (1) {
+    cells.push_back(cv::Point2i(x0, y0));
+
+    if (x0 == x1 && y0 == y1) {
+      break;
+    }
+
+    int e2 = 2 * err;
+    if (e2 > -dy) {
+      err -= dy;
+      x0 += sx;
+    }
+    if (e2 < dx) {
+      err += dx;
+      y0 += sy;
+    }
+  }
+
+  return cells;
+}
+
+std::vector<cv::Point2i> bresCircle(int x0, int y0, int r) {
+  // Bresenham's circle algorithm
+  // Find cells intersected by circle with center (x0,y0) and radius r
+
+  std::vector<std::vector<bool> > mask(2 * r + 1);
+
+  for (int i = 0; i < 2 * r + 1; ++i) {
+    mask[i].resize(2 * r + 1);
+    for (int j = 0; j < 2 * r + 1; ++j) {
+      mask[i][j] = false;
+    }
+  }
+
+  int f = 1 - r;
+  int ddF_x = 1;
+  int ddF_y = -2 * r;
+  int x = 0;
+  int y = r;
+
+  std::vector<cv::Point2i> line;
+
+  line = bresLine(x0, y0 - r, x0, y0 + r);
+  for (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end();
+       ++it) {
+    mask[it->x - x0 + r][it->y - y0 + r] = true;
+  }
+
+  line = bresLine(x0 - r, y0, x0 + r, y0);
+  for (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end();
+       ++it) {
+    mask[it->x - x0 + r][it->y - y0 + r] = true;
+  }
+
+  while (x < y) {
+    if (f >= 0) {
+      y--;
+      ddF_y += 2;
+      f += ddF_y;
+    }
+
+    x++;
+    ddF_x += 2;
+    f += ddF_x;
+
+    line = bresLine(x0 - x, y0 + y, x0 + x, y0 + y);
+    for (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end();
+         ++it) {
+      mask[it->x - x0 + r][it->y - y0 + r] = true;
+    }
+
+    line = bresLine(x0 - x, y0 - y, x0 + x, y0 - y);
+    for (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end();
+         ++it) {
+      mask[it->x - x0 + r][it->y - y0 + r] = true;
+    }
+
+    line = bresLine(x0 - y, y0 + x, x0 + y, y0 + x);
+    for (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end();
+         ++it) {
+      mask[it->x - x0 + r][it->y - y0 + r] = true;
+    }
+
+    line = bresLine(x0 - y, y0 - x, x0 + y, y0 - x);
+    for (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end();
+         ++it) {
+      mask[it->x - x0 + r][it->y - y0 + r] = true;
+    }
+  }
+
+  std::vector<cv::Point2i> cells;
+  for (int i = 0; i < 2 * r + 1; ++i) {
+    for (int j = 0; j < 2 * r + 1; ++j) {
+      if (mask[i][j]) {
+        cells.push_back(cv::Point2i(i - r + x0, j - r + y0));
+      }
+    }
+  }
+
+  return cells;
+}
+
+void fitCircle(
+    const std::vector<cv::Point2d> &points, double &centerX, double &centerY,
+    double &radius) {
+  // D. Umbach, and K. Jones, A Few Methods for Fitting Circles to Data,
+  // IEEE Transactions on Instrumentation and Measurement, 2000
+  // We use the modified least squares method.
+  double sum_x = 0.0;
+  double sum_y = 0.0;
+  double sum_xx = 0.0;
+  double sum_xy = 0.0;
+  double sum_yy = 0.0;
+  double sum_xxx = 0.0;
+  double sum_xxy = 0.0;
+  double sum_xyy = 0.0;
+  double sum_yyy = 0.0;
+
+  int n = points.size();
+  for (int i = 0; i < n; ++i) {
+    double x = points.at(i).x;
+    double y = points.at(i).y;
+
+    sum_x += x;
+    sum_y += y;
+    sum_xx += x * x;
+    sum_xy += x * y;
+    sum_yy += y * y;
+    sum_xxx += x * x * x;
+    sum_xxy += x * x * y;
+    sum_xyy += x * y * y;
+    sum_yyy += y * y * y;
+  }
+
+  double A = n * sum_xx - square(sum_x);
+  double B = n * sum_xy - sum_x * sum_y;
+  double C = n * sum_yy - square(sum_y);
+  double D =
+      0.5 * (n * sum_xyy - sum_x * sum_yy + n * sum_xxx - sum_x * sum_xx);
+  double E =
+      0.5 * (n * sum_xxy - sum_y * sum_xx + n * sum_yyy - sum_y * sum_yy);
+
+  centerX = (D * C - B * E) / (A * C - square(B));
+  centerY = (A * E - B * D) / (A * C - square(B));
+
+  double sum_r = 0.0;
+  for (int i = 0; i < n; ++i) {
+    double x = points.at(i).x;
+    double y = points.at(i).y;
+
+    sum_r += hypot(x - centerX, y - centerY);
+  }
+
+  radius = sum_r / n;
+}
+
+std::vector<cv::Point2d> intersectCircles(
+    double x1, double y1, double r1, double x2, double y2, double r2) {
+  std::vector<cv::Point2d> ipts;
+
+  double d = hypot(x1 - x2, y1 - y2);
+  if (d > r1 + r2) {
+    // circles are separate
+    return ipts;
+  }
+  if (d < fabs(r1 - r2)) {
+    // one circle is contained within the other
+    return ipts;
+  }
+
+  double a = (square(r1) - square(r2) + square(d)) / (2.0 * d);
+  double h = sqrt(square(r1) - square(a));
+
+  double x3 = x1 + a * (x2 - x1) / d;
+  double y3 = y1 + a * (y2 - y1) / d;
+
+  if (h < 1e-10) {
+    // two circles touch at one point
+    ipts.push_back(cv::Point2d(x3, y3));
+    return ipts;
+  }
+
+  ipts.push_back(cv::Point2d(x3 + h * (y2 - y1) / d, y3 - h * (x2 - x1) / d));
+  ipts.push_back(cv::Point2d(x3 - h * (y2 - y1) / d, y3 + h * (x2 - x1) / d));
+  return ipts;
+}
+
+char UTMLetterDesignator(double latitude) {
+  // This routine determines the correct UTM letter designator for the given
+  // latitude
+  // returns 'Z' if latitude is outside the UTM limits of 84N to 80S
+  // Written by Chuck Gantz- chuck.gantz@globalstar.com
+  char letterDesignator;
+
+  if ((84.0 >= latitude) && (latitude >= 72.0))
+    letterDesignator = 'X';
+  else if ((72.0 > latitude) && (latitude >= 64.0))
+    letterDesignator = 'W';
+  else if ((64.0 > latitude) && (latitude >= 56.0))
+    letterDesignator = 'V';
+  else if ((56.0 > latitude) && (latitude >= 48.0))
+    letterDesignator = 'U';
+  else if ((48.0 > latitude) && (latitude >= 40.0))
+    letterDesignator = 'T';
+  else if ((40.0 > latitude) && (latitude >= 32.0))
+    letterDesignator = 'S';
+  else if ((32.0 > latitude) && (latitude >= 24.0))
+    letterDesignator = 'R';
+  else if ((24.0 > latitude) && (latitude >= 16.0))
+    letterDesignator = 'Q';
+  else if ((16.0 > latitude) && (latitude >= 8.0))
+    letterDesignator = 'P';
+  else if ((8.0 > latitude) && (latitude >= 0.0))
+    letterDesignator = 'N';
+  else if ((0.0 > latitude) && (latitude >= -8.0))
+    letterDesignator = 'M';
+  else if ((-8.0 > latitude) && (latitude >= -16.0))
+    letterDesignator = 'L';
+  else if ((-16.0 > latitude) && (latitude >= -24.0))
+    letterDesignator = 'K';
+  else if ((-24.0 > latitude) && (latitude >= -32.0))
+    letterDesignator = 'J';
+  else if ((-32.0 > latitude) && (latitude >= -40.0))
+    letterDesignator = 'H';
+  else if ((-40.0 > latitude) && (latitude >= -48.0))
+    letterDesignator = 'G';
+  else if ((-48.0 > latitude) && (latitude >= -56.0))
+    letterDesignator = 'F';
+  else if ((-56.0 > latitude) && (latitude >= -64.0))
+    letterDesignator = 'E';
+  else if ((-64.0 > latitude) && (latitude >= -72.0))
+    letterDesignator = 'D';
+  else if ((-72.0 > latitude) && (latitude >= -80.0))
+    letterDesignator = 'C';
+  else
+    letterDesignator = 'Z';  // This is here as an error flag to show that the
+                             // Latitude is outside the UTM limits
+
+  return letterDesignator;
+}
+
+void LLtoUTM(
+    double latitude, double longitude, double &utmNorthing, double &utmEasting,
+    std::string &utmZone) {
+  // converts lat/long to UTM coords.  Equations from USGS Bulletin 1532
+  // East Longitudes are positive, West longitudes are negative.
+  // North latitudes are positive, South latitudes are negative
+  // Lat and Long are in decimal degrees
+  // Written by Chuck Gantz- chuck.gantz@globalstar.com
+
+  double k0 = 0.9996;
+
+  double LongOrigin;
+  double eccPrimeSquared;
+  double N, T, C, A, M;
+
+  double LatRad = latitude * M_PI / 180.0;
+  double LongRad = longitude * M_PI / 180.0;
+  double LongOriginRad;
+
+  int ZoneNumber = static_cast<int>((longitude + 180.0) / 6.0) + 1;
+
+  if (latitude >= 56.0 && latitude < 64.0 && longitude >= 3.0 &&
+      longitude < 12.0) {
+    ZoneNumber = 32;
+  }
+
+  // Special zones for Svalbard
+  if (latitude >= 72.0 && latitude < 84.0) {
+    if (longitude >= 0.0 && longitude < 9.0)
+      ZoneNumber = 31;
+    else if (longitude >= 9.0 && longitude < 21.0)
+      ZoneNumber = 33;
+    else if (longitude >= 21.0 && longitude < 33.0)
+      ZoneNumber = 35;
+    else if (longitude >= 33.0 && longitude < 42.0)
+      ZoneNumber = 37;
+  }
+  LongOrigin = static_cast<double>(
+      (ZoneNumber - 1) * 6 - 180 + 3);  //+3 puts origin in middle of zone
+  LongOriginRad = LongOrigin * M_PI / 180.0;
+
+  // compute the UTM Zone from the latitude and longitude
+  std::ostringstream oss;
+  oss << ZoneNumber << UTMLetterDesignator(latitude);
+  utmZone = oss.str();
+
+  eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ);
+
+  N = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(LatRad) * sin(LatRad));
+  T = tan(LatRad) * tan(LatRad);
+  C = eccPrimeSquared * cos(LatRad) * cos(LatRad);
+  A = cos(LatRad) * (LongRad - LongOriginRad);
+
+  M = WGS84_A *
+      ((1.0 - WGS84_ECCSQ / 4.0 - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0 -
+        5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0) *
+           LatRad -
+       (3.0 * WGS84_ECCSQ / 8.0 + 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 32.0 +
+        45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0) *
+           sin(2.0 * LatRad) +
+       (15.0 * WGS84_ECCSQ * WGS84_ECCSQ / 256.0 +
+        45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0) *
+           sin(4.0 * LatRad) -
+       (35.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 3072.0) *
+           sin(6.0 * LatRad));
+
+  utmEasting =
+      k0 * N * (A + (1.0 - T + C) * A * A * A / 6.0 +
+                (5.0 - 18.0 * T + T * T + 72.0 * C - 58.0 * eccPrimeSquared) *
+                    A * A * A * A * A / 120.0) +
+      500000.0;
+
+  utmNorthing =
+      k0 *
+      (M +
+       N * tan(LatRad) *
+           (A * A / 2.0 +
+            (5.0 - T + 9.0 * C + 4.0 * C * C) * A * A * A * A / 24.0 +
+            (61.0 - 58.0 * T + T * T + 600.0 * C - 330.0 * eccPrimeSquared) *
+                A * A * A * A * A * A / 720.0));
+  if (latitude < 0.0) {
+    utmNorthing += 10000000.0;  // 10000000 meter offset for southern hemisphere
+  }
+}
+
+void UTMtoLL(
+    double utmNorthing, double utmEasting, const std::string &utmZone,
+    double &latitude, double &longitude) {
+  // converts UTM coords to lat/long.  Equations from USGS Bulletin 1532
+  // East Longitudes are positive, West longitudes are negative.
+  // North latitudes are positive, South latitudes are negative
+  // Lat and Long are in decimal degrees.
+  // Written by Chuck Gantz- chuck.gantz@globalstar.com
+
+  double k0 = 0.9996;
+  double eccPrimeSquared;
+  double e1 = (1.0 - sqrt(1.0 - WGS84_ECCSQ)) / (1.0 + sqrt(1.0 - WGS84_ECCSQ));
+  double N1, T1, C1, R1, D, M;
+  double LongOrigin;
+  double mu, phi1, phi1Rad;
+  double x, y;
+  int ZoneNumber;
+  char ZoneLetter;
+  bool NorthernHemisphere;
+
+  x = utmEasting - 500000.0;  // remove 500,000 meter offset for longitude
+  y = utmNorthing;
+
+  std::istringstream iss(utmZone);
+  iss >> ZoneNumber >> ZoneLetter;
+  if ((static_cast<int>(ZoneLetter) - static_cast<int>('N')) >= 0) {
+    NorthernHemisphere = true;  // point is in northern hemisphere
+  } else {
+    NorthernHemisphere = false;  // point is in southern hemisphere
+    y -= 10000000.0;  // remove 10,000,000 meter offset used for southern
+                      // hemisphere
+  }
+
+  LongOrigin = (ZoneNumber - 1.0) * 6.0 - 180.0 +
+               3.0;  //+3 puts origin in middle of zone
+
+  eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ);
+
+  M = y / k0;
+  mu = M / (WGS84_A *
+            (1.0 - WGS84_ECCSQ / 4.0 - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0 -
+             5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0));
+
+  phi1Rad = mu + (3.0 * e1 / 2.0 - 27.0 * e1 * e1 * e1 / 32.0) * sin(2.0 * mu) +
+            (21.0 * e1 * e1 / 16.0 - 55.0 * e1 * e1 * e1 * e1 / 32.0) *
+                sin(4.0 * mu) +
+            (151.0 * e1 * e1 * e1 / 96.0) * sin(6.0 * mu);
+  phi1 = phi1Rad / M_PI * 180.0;
+
+  N1 = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad));
+  T1 = tan(phi1Rad) * tan(phi1Rad);
+  C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad);
+  R1 = WGS84_A * (1.0 - WGS84_ECCSQ) /
+       pow(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad), 1.5);
+  D = x / (N1 * k0);
+
+  latitude = phi1Rad -
+             (N1 * tan(phi1Rad) / R1) *
+                 (D * D / 2.0 -
+                  (5.0 + 3.0 * T1 + 10.0 * C1 - 4.0 * C1 * C1 -
+                   9.0 * eccPrimeSquared) *
+                      D * D * D * D / 24.0 +
+                  (61.0 + 90.0 * T1 + 298.0 * C1 + 45.0 * T1 * T1 -
+                   252.0 * eccPrimeSquared - 3.0 * C1 * C1) *
+                      D * D * D * D * D * D / 720.0);
+  latitude *= 180.0 / M_PI;
+
+  longitude = (D - (1.0 + 2.0 * T1 + C1) * D * D * D / 6.0 +
+               (5.0 - 2.0 * C1 + 28.0 * T1 - 3.0 * C1 * C1 +
+                8.0 * eccPrimeSquared + 24.0 * T1 * T1) *
+                   D * D * D * D * D / 120.0) /
+              cos(phi1Rad);
+  longitude = LongOrigin + longitude / M_PI * 180.0;
+}
+
+long int timestampDiff(uint64_t t1, uint64_t t2) {
+  if (t2 > t1) {
+    uint64_t d = t2 - t1;
+
+    if (d > std::numeric_limits<long int>::max()) {
+      return std::numeric_limits<long int>::max();
+    } else {
+      return d;
+    }
+  } else {
+    uint64_t d = t1 - t2;
+
+    if (d > std::numeric_limits<long int>::max()) {
+      return std::numeric_limits<long int>::min();
+    } else {
+      return -static_cast<long int>(d);
+    }
+  }
+}
+}

src/mynteye/api/camodocal/src/sparse_graph/Transform.cc

@@ -0,0 +1,55 @@
+#include <camodocal/sparse_graph/Transform.h>
+
+namespace camodocal {
+
+Transform::Transform() {
+  m_q.setIdentity();
+  m_t.setZero();
+}
+
+Transform::Transform(const Eigen::Matrix4d &H) {
+  m_q = Eigen::Quaterniond(H.block<3, 3>(0, 0));
+  m_t = H.block<3, 1>(0, 3);
+}
+
+Eigen::Quaterniond &Transform::rotation(void) {
+  return m_q;
+}
+
+const Eigen::Quaterniond &Transform::rotation(void) const {
+  return m_q;
+}
+
+double *Transform::rotationData(void) {
+  return m_q.coeffs().data();
+}
+
+const double *const Transform::rotationData(void) const {
+  return m_q.coeffs().data();
+}
+
+Eigen::Vector3d &Transform::translation(void) {
+  return m_t;
+}
+
+const Eigen::Vector3d &Transform::translation(void) const {
+  return m_t;
+}
+
+double *Transform::translationData(void) {
+  return m_t.data();
+}
+
+const double *const Transform::translationData(void) const {
+  return m_t.data();
+}
+
+Eigen::Matrix4d Transform::toMatrix(void) const {
+  Eigen::Matrix4d H;
+  H.setIdentity();
+  H.block<3, 3>(0, 0) = m_q.toRotationMatrix();
+  H.block<3, 1>(0, 3) = m_t;
+
+  return H;
+}
+}