#include "camodocal/camera_models/CostFunctionFactory.h"

#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 {

template <typename T>
void worldToCameraTransform(const T* const q_cam_odo,
                            const T* const t_cam_odo,
                            const T* const p_odo,
                            const T* const att_odo,
                            T* q,
                            T* t,
                            bool optimize_cam_odo_z = true) {
  Eigen::Quaternion<T> q_z_inv(cos(att_odo[0] / T(2)), T(0), T(0), -sin(att_odo[0] / T(2)));
  Eigen::Quaternion<T> q_y_inv(cos(att_odo[1] / T(2)), T(0), -sin(att_odo[1] / T(2)), T(0));
  Eigen::Quaternion<T> q_x_inv(cos(att_odo[2] / T(2)), -sin(att_odo[2] / T(2)), T(0), T(0));

  Eigen::Quaternion<T> q_zyx_inv = q_x_inv * q_y_inv * q_z_inv;

  T q_odo[4] = {q_zyx_inv.w(), q_zyx_inv.x(), q_zyx_inv.y(), q_zyx_inv.z()};

  T q_odo_cam[4] = {q_cam_odo[3], -q_cam_odo[0], -q_cam_odo[1], -q_cam_odo[2]};

  T q0[4];
  ceres::QuaternionProduct(q_odo_cam, q_odo, q0);

  T t0[3];
  T t_odo[3] = {p_odo[0], p_odo[1], p_odo[2]};

  ceres::QuaternionRotatePoint(q_odo, t_odo, t0);

  t0[0] += t_cam_odo[0];
  t0[1] += t_cam_odo[1];

  if (optimize_cam_odo_z) {
    t0[2] += t_cam_odo[2];
  }

  ceres::QuaternionRotatePoint(q_odo_cam, t0, t);
  t[0] = -t[0];
  t[1] = -t[1];
  t[2] = -t[2];

  // Convert quaternion from Ceres convention (w, x, y, z)
  // to Eigen convention (x, y, z, w)
  q[0] = q0[1];
  q[1] = q0[2];
  q[2] = q0[3];
  q[3] = q0[0];
}

template <class CameraT>
class ReprojectionError1 {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  ReprojectionError1(const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p)
      : m_observed_P(observed_P), m_observed_p(observed_p), m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {}

  ReprojectionError1(const Eigen::Vector3d& observed_P,
                     const Eigen::Vector2d& observed_p,
                     const Eigen::Matrix2d& sqrtPrecisionMat)
      : m_observed_P(observed_P), m_observed_p(observed_p), m_sqrtPrecisionMat(sqrtPrecisionMat) {}

  ReprojectionError1(const std::vector<double>& intrinsic_params,
                     const Eigen::Vector3d& observed_P,
                     const Eigen::Vector2d& observed_p)
      : m_intrinsic_params(intrinsic_params), m_observed_P(observed_P), m_observed_p(observed_p) {}

  // variables: camera intrinsics and camera extrinsics
  template <typename T>
  bool operator()(const T* const intrinsic_params, const T* const q, const T* const t, T* residuals) const {
    Eigen::Matrix<T, 3, 1> P = m_observed_P.cast<T>();

    Eigen::Matrix<T, 2, 1> predicted_p;
    CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p);

    Eigen::Matrix<T, 2, 1> e = predicted_p - m_observed_p.cast<T>();

    Eigen::Matrix<T, 2, 1> e_weighted = m_sqrtPrecisionMat.cast<T>() * e;

    residuals[0] = e_weighted(0);
    residuals[1] = e_weighted(1);

    return true;
  }

  // variables: camera-odometry transforms and odometry poses
  template <typename T>
  bool operator()(const T* const q_cam_odo,
                  const T* const t_cam_odo,
                  const T* const p_odo,
                  const T* const att_odo,
                  T* residuals) const {
    T q[4], t[3];
    worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t);

    Eigen::Matrix<T, 3, 1> P = m_observed_P.cast<T>();

    std::vector<T> intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());

    // project 3D object point to the image plane
    Eigen::Matrix<T, 2, 1> predicted_p;
    CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);

    residuals[0] = predicted_p(0) - T(m_observed_p(0));
    residuals[1] = predicted_p(1) - T(m_observed_p(1));

    return true;
  }

  // private:
  // camera intrinsics
  std::vector<double> m_intrinsic_params;

  // observed 3D point
  Eigen::Vector3d m_observed_P;

  // observed 2D point
  Eigen::Vector2d m_observed_p;

  // square root of precision matrix
  Eigen::Matrix2d m_sqrtPrecisionMat;
};

// variables: camera extrinsics, 3D point
template <class CameraT>
class ReprojectionError2 {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  ReprojectionError2(const std::vector<double>& intrinsic_params, const Eigen::Vector2d& observed_p)
      : m_intrinsic_params(intrinsic_params), m_observed_p(observed_p) {}

  template <typename T>
  bool operator()(const T* const q, const T* const t, const T* const point, T* residuals) const {
    Eigen::Matrix<T, 3, 1> P;
    P(0) = T(point[0]);
    P(1) = T(point[1]);
    P(2) = T(point[2]);

    std::vector<T> intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());

    // project 3D object point to the image plane
    Eigen::Matrix<T, 2, 1> predicted_p;
    CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);

    residuals[0] = predicted_p(0) - T(m_observed_p(0));
    residuals[1] = predicted_p(1) - T(m_observed_p(1));

    return true;
  }

 private:
  // camera intrinsics
  std::vector<double> m_intrinsic_params;

  // observed 2D point
  Eigen::Vector2d m_observed_p;
};

template <class CameraT>
class ReprojectionError3 {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  ReprojectionError3(const Eigen::Vector2d& observed_p)
      : m_observed_p(observed_p), m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()), m_optimize_cam_odo_z(true) {}

  ReprojectionError3(const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat)
      : m_observed_p(observed_p), m_sqrtPrecisionMat(sqrtPrecisionMat), m_optimize_cam_odo_z(true) {}

  ReprojectionError3(const std::vector<double>& intrinsic_params, const Eigen::Vector2d& observed_p)
      : m_intrinsic_params(intrinsic_params),
        m_observed_p(observed_p),
        m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()),
        m_optimize_cam_odo_z(true) {}

  ReprojectionError3(const std::vector<double>& intrinsic_params,
                     const Eigen::Vector2d& observed_p,
                     const Eigen::Matrix2d& sqrtPrecisionMat)
      : m_intrinsic_params(intrinsic_params),
        m_observed_p(observed_p),
        m_sqrtPrecisionMat(sqrtPrecisionMat),
        m_optimize_cam_odo_z(true) {}

  ReprojectionError3(const std::vector<double>& intrinsic_params,
                     const Eigen::Vector3d& odo_pos,
                     const Eigen::Vector3d& odo_att,
                     const Eigen::Vector2d& observed_p,
                     bool optimize_cam_odo_z)
      : m_intrinsic_params(intrinsic_params),
        m_odo_pos(odo_pos),
        m_odo_att(odo_att),
        m_observed_p(observed_p),
        m_optimize_cam_odo_z(optimize_cam_odo_z) {}

  ReprojectionError3(const std::vector<double>& intrinsic_params,
                     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)
      : m_intrinsic_params(intrinsic_params),
        m_cam_odo_q(cam_odo_q),
        m_cam_odo_t(cam_odo_t),
        m_odo_pos(odo_pos),
        m_odo_att(odo_att),
        m_observed_p(observed_p),
        m_optimize_cam_odo_z(true) {}

  // variables: camera intrinsics, camera-to-odometry transform,
  //            odometry extrinsics, 3D point
  template <typename T>
  bool operator()(const T* const intrinsic_params,
                  const T* const q_cam_odo,
                  const T* const t_cam_odo,
                  const T* const p_odo,
                  const T* const att_odo,
                  const T* const point,
                  T* residuals) const {
    T q[4], t[3];
    worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);

    Eigen::Matrix<T, 3, 1> P(point[0], point[1], point[2]);

    // project 3D object point to the image plane
    Eigen::Matrix<T, 2, 1> predicted_p;
    CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p);

    Eigen::Matrix<T, 2, 1> err = predicted_p - m_observed_p.cast<T>();
    Eigen::Matrix<T, 2, 1> err_weighted = m_sqrtPrecisionMat.cast<T>() * err;

    residuals[0] = err_weighted(0);
    residuals[1] = err_weighted(1);

    return true;
  }

  // variables: camera-to-odometry transform, 3D point
  template <typename T>
  bool operator()(const T* const q_cam_odo, const T* const t_cam_odo, const T* const point, T* residuals) const {
    T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))};
    T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))};
    T q[4], t[3];

    worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);

    std::vector<T> intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());
    Eigen::Matrix<T, 3, 1> P(point[0], point[1], point[2]);

    // project 3D object point to the image plane
    Eigen::Matrix<T, 2, 1> predicted_p;
    CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);

    residuals[0] = predicted_p(0) - T(m_observed_p(0));
    residuals[1] = predicted_p(1) - T(m_observed_p(1));

    return true;
  }

  // variables: camera-to-odometry transform, odometry extrinsics, 3D point
  template <typename T>
  bool operator()(const T* const q_cam_odo,
                  const T* const t_cam_odo,
                  const T* const p_odo,
                  const T* const att_odo,
                  const T* const point,
                  T* residuals) const {
    T q[4], t[3];
    worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);

    std::vector<T> intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());
    Eigen::Matrix<T, 3, 1> P(point[0], point[1], point[2]);

    // project 3D object point to the image plane
    Eigen::Matrix<T, 2, 1> predicted_p;
    CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);

    Eigen::Matrix<T, 2, 1> err = predicted_p - m_observed_p.cast<T>();
    Eigen::Matrix<T, 2, 1> err_weighted = m_sqrtPrecisionMat.cast<T>() * err;

    residuals[0] = err_weighted(0);
    residuals[1] = err_weighted(1);

    return true;
  }

  // variables: 3D point
  template <typename T>
  bool operator()(const T* const point, T* residuals) const {
    T q_cam_odo[4] = {T(m_cam_odo_q.coeffs()(0)), T(m_cam_odo_q.coeffs()(1)), T(m_cam_odo_q.coeffs()(2)),
                      T(m_cam_odo_q.coeffs()(3))};
    T t_cam_odo[3] = {T(m_cam_odo_t(0)), T(m_cam_odo_t(1)), T(m_cam_odo_t(2))};
    T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))};
    T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))};
    T q[4], t[3];

    worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z);

    std::vector<T> intrinsic_params(m_intrinsic_params.begin(), m_intrinsic_params.end());
    Eigen::Matrix<T, 3, 1> P(point[0], point[1], point[2]);

    // project 3D object point to the image plane
    Eigen::Matrix<T, 2, 1> predicted_p;
    CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p);

    residuals[0] = predicted_p(0) - T(m_observed_p(0));
    residuals[1] = predicted_p(1) - T(m_observed_p(1));

    return true;
  }

 private:
  // camera intrinsics
  std::vector<double> m_intrinsic_params;

  // observed camera-odometry transform
  Eigen::Quaterniond m_cam_odo_q;
  Eigen::Vector3d m_cam_odo_t;

  // observed odometry
  Eigen::Vector3d m_odo_pos;
  Eigen::Vector3d m_odo_att;

  // observed 2D point
  Eigen::Vector2d m_observed_p;

  Eigen::Matrix2d m_sqrtPrecisionMat;

  bool m_optimize_cam_odo_z;
};

// variables: camera intrinsics and camera extrinsics
template <class CameraT>
class StereoReprojectionError {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  StereoReprojectionError(const Eigen::Vector3d& observed_P,
                          const Eigen::Vector2d& observed_p_l,
                          const Eigen::Vector2d& observed_p_r)
      : m_observed_P(observed_P), m_observed_p_l(observed_p_l), m_observed_p_r(observed_p_r) {}

  template <typename T>
  bool operator()(const T* const intrinsic_params_l,
                  const T* const intrinsic_params_r,
                  const T* const q_l,
                  const T* const t_l,
                  const T* const q_l_r,
                  const T* const t_l_r,
                  T* residuals) const {
    Eigen::Matrix<T, 3, 1> P;
    P(0) = T(m_observed_P(0));
    P(1) = T(m_observed_P(1));
    P(2) = T(m_observed_P(2));

    Eigen::Matrix<T, 2, 1> predicted_p_l;
    CameraT::spaceToPlane(intrinsic_params_l, q_l, t_l, P, predicted_p_l);

    Eigen::Quaternion<T> q_r = Eigen::Quaternion<T>(q_l_r) * Eigen::Quaternion<T>(q_l);

    Eigen::Matrix<T, 3, 1> t_r;
    t_r(0) = t_l[0];
    t_r(1) = t_l[1];
    t_r(2) = t_l[2];

    t_r = Eigen::Quaternion<T>(q_l_r) * t_r;
    t_r(0) += t_l_r[0];
    t_r(1) += t_l_r[1];
    t_r(2) += t_l_r[2];

    Eigen::Matrix<T, 2, 1> predicted_p_r;
    CameraT::spaceToPlane(intrinsic_params_r, q_r.coeffs().data(), t_r.data(), P, predicted_p_r);

    residuals[0] = predicted_p_l(0) - T(m_observed_p_l(0));
    residuals[1] = predicted_p_l(1) - T(m_observed_p_l(1));
    residuals[2] = predicted_p_r(0) - T(m_observed_p_r(0));
    residuals[3] = predicted_p_r(1) - T(m_observed_p_r(1));

    return true;
  }

 private:
  // observed 3D point
  Eigen::Vector3d m_observed_P;

  // observed 2D point
  Eigen::Vector2d m_observed_p_l;
  Eigen::Vector2d m_observed_p_r;
};

template <class CameraT>
class ComprehensionError {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  ComprehensionError(const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p)
      : m_observed_P(observed_P), m_observed_p(observed_p), m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {}

  template <typename T>
  bool operator()(const T* const intrinsic_params, const T* const q, const T* const t, T* residuals) const {
    {
      Eigen::Matrix<T, 2, 1> p = m_observed_p.cast<T>();

      Eigen::Matrix<T, 3, 1> predicted_img_P;
      CameraT::LiftToSphere(intrinsic_params, p, predicted_img_P);

      Eigen::Matrix<T, 2, 1> predicted_p;
      CameraT::SphereToPlane(intrinsic_params, predicted_img_P, predicted_p);

      Eigen::Matrix<T, 2, 1> e = predicted_p - m_observed_p.cast<T>();

      Eigen::Matrix<T, 2, 1> e_weighted = m_sqrtPrecisionMat.cast<T>() * e;

      residuals[0] = e_weighted(0);
      residuals[1] = e_weighted(1);
    }

    {
      Eigen::Matrix<T, 3, 1> P = m_observed_P.cast<T>();

      Eigen::Matrix<T, 2, 1> predicted_p;
      CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p);

      Eigen::Matrix<T, 2, 1> e = predicted_p - m_observed_p.cast<T>();

      Eigen::Matrix<T, 2, 1> e_weighted = m_sqrtPrecisionMat.cast<T>() * e;

      residuals[2] = e_weighted(0);
      residuals[3] = e_weighted(1);
    }

    return true;
  }

  // private:
  // camera intrinsics
  // std::vector<double> m_intrinsic_params;

  // observed 3D point
  Eigen::Vector3d m_observed_P;

  // observed 2D point
  Eigen::Vector2d m_observed_p;

  // square root of precision matrix
  Eigen::Matrix2d m_sqrtPrecisionMat;
};

boost::shared_ptr<CostFunctionFactory> CostFunctionFactory::m_instance;

CostFunctionFactory::CostFunctionFactory() {}

boost::shared_ptr<CostFunctionFactory> CostFunctionFactory::instance(void) {
  if (m_instance.get() == 0) {
    m_instance.reset(new CostFunctionFactory);
  }

  return m_instance;
}

ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,
                                                               const Eigen::Vector3d& observed_P,
                                                               const Eigen::Vector2d& observed_p,
                                                               int flags) const {
  ceres::CostFunction* costFunction = 0;

  std::vector<double> intrinsic_params;
  camera->writeParameters(intrinsic_params);

  switch (flags) {
    case CAMERA_INTRINSICS | CAMERA_POSE:
      switch (camera->modelType()) {
        case Camera::KANNALA_BRANDT:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError1<EquidistantCamera>, 2, 8, 4, 3>(
              new ReprojectionError1<EquidistantCamera>(observed_P, observed_p));
          break;
        case Camera::PINHOLE:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError1<PinholeCamera>, 2, 8, 4, 3>(
              new ReprojectionError1<PinholeCamera>(observed_P, observed_p));
          break;
        case Camera::MEI:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError1<CataCamera>, 2, 9, 4, 3>(
              new ReprojectionError1<CataCamera>(observed_P, observed_p));
          break;
        case Camera::SCARAMUZZA:
          costFunction =
              new ceres::AutoDiffCostFunction<ComprehensionError<OCAMCamera>, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>(
                  new ComprehensionError<OCAMCamera>(observed_P, observed_p));
          // new ceres::AutoDiffCostFunction<ReprojectionError1<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>(
          // new ReprojectionError1<OCAMCamera>(observed_P, observed_p));
          break;
      }
      break;
    case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE:
      switch (camera->modelType()) {
        case Camera::KANNALA_BRANDT:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError1<EquidistantCamera>, 2, 4, 3, 3, 3>(
              new ReprojectionError1<EquidistantCamera>(intrinsic_params, observed_P, observed_p));
          break;
        case Camera::PINHOLE:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError1<PinholeCamera>, 2, 4, 3, 3, 3>(
              new ReprojectionError1<PinholeCamera>(intrinsic_params, observed_P, observed_p));
          break;
        case Camera::MEI:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError1<CataCamera>, 2, 4, 3, 3, 3>(
              new ReprojectionError1<CataCamera>(intrinsic_params, observed_P, observed_p));
          break;
        case Camera::SCARAMUZZA:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError1<OCAMCamera>, 2, 4, 3, 3, 3>(
              new ReprojectionError1<OCAMCamera>(intrinsic_params, observed_P, observed_p));
          break;
      }
      break;
  }

  return costFunction;
}

ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,
                                                               const Eigen::Vector3d& observed_P,
                                                               const Eigen::Vector2d& observed_p,
                                                               const Eigen::Matrix2d& sqrtPrecisionMat,
                                                               int flags) const {
  ceres::CostFunction* costFunction = 0;

  std::vector<double> intrinsic_params;
  camera->writeParameters(intrinsic_params);

  switch (flags) {
    case CAMERA_INTRINSICS | CAMERA_POSE:
      switch (camera->modelType()) {
        case Camera::KANNALA_BRANDT:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError1<EquidistantCamera>, 2, 8, 4, 3>(
              new ReprojectionError1<EquidistantCamera>(observed_P, observed_p, sqrtPrecisionMat));
          break;
        case Camera::PINHOLE:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError1<PinholeCamera>, 2, 8, 4, 3>(
              new ReprojectionError1<PinholeCamera>(observed_P, observed_p, sqrtPrecisionMat));
          break;
        case Camera::MEI:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError1<CataCamera>, 2, 9, 4, 3>(
              new ReprojectionError1<CataCamera>(observed_P, observed_p, sqrtPrecisionMat));
          break;
        case Camera::SCARAMUZZA:
          costFunction =
              new ceres::AutoDiffCostFunction<ReprojectionError1<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>(
                  new ReprojectionError1<OCAMCamera>(observed_P, observed_p, sqrtPrecisionMat));
          break;
      }
      break;
  }

  return costFunction;
}

ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,
                                                               const Eigen::Vector2d& observed_p,
                                                               int flags,
                                                               bool optimize_cam_odo_z) const {
  ceres::CostFunction* costFunction = 0;

  std::vector<double> intrinsic_params;
  camera->writeParameters(intrinsic_params);

  switch (flags) {
    case CAMERA_POSE | POINT_3D:
      switch (camera->modelType()) {
        case Camera::KANNALA_BRANDT:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError2<EquidistantCamera>, 2, 4, 3, 3>(
              new ReprojectionError2<EquidistantCamera>(intrinsic_params, observed_p));
          break;
        case Camera::PINHOLE:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError2<PinholeCamera>, 2, 4, 3, 3>(
              new ReprojectionError2<PinholeCamera>(intrinsic_params, observed_p));
          break;
        case Camera::MEI:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError2<CataCamera>, 2, 4, 3, 3>(
              new ReprojectionError2<CataCamera>(intrinsic_params, observed_p));
          break;
        case Camera::SCARAMUZZA:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError2<OCAMCamera>, 2, 4, 3, 3>(
              new ReprojectionError2<OCAMCamera>(intrinsic_params, observed_p));
          break;
      }
      break;
    case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D:
      switch (camera->modelType()) {
        case Camera::KANNALA_BRANDT:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 4, 3, 2, 1, 3>(
                new ReprojectionError3<EquidistantCamera>(intrinsic_params, observed_p));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 2, 1, 3>(
                new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));
          }
          break;
        case Camera::PINHOLE:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 3, 2, 1, 3>(
                new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 2, 1, 3>(
                new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));
          }
          break;
        case Camera::MEI:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 3, 2, 1, 3>(
                new ReprojectionError3<CataCamera>(intrinsic_params, observed_p));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 2, 2, 1, 3>(
                new ReprojectionError3<CataCamera>(intrinsic_params, observed_p));
          }
          break;
        case Camera::SCARAMUZZA:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 3, 2, 1, 3>(
                new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 2, 2, 1, 3>(
                new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p));
          }
          break;
      }
      break;
    case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:
      switch (camera->modelType()) {
        case Camera::KANNALA_BRANDT:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 4, 3, 3, 3, 3>(
                new ReprojectionError3<EquidistantCamera>(intrinsic_params, observed_p));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 3, 3, 3>(
                new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));
          }
          break;
        case Camera::PINHOLE:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 3, 3, 3, 3>(
                new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 3, 3, 3>(
                new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p));
          }
          break;
        case Camera::MEI:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 3, 3, 3, 3>(
                new ReprojectionError3<CataCamera>(intrinsic_params, observed_p));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 2, 3, 3, 3>(
                new ReprojectionError3<CataCamera>(intrinsic_params, observed_p));
          }
          break;
        case Camera::SCARAMUZZA:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 3, 3, 3, 3>(
                new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 2, 3, 3, 3>(
                new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p));
          }
          break;
      }
      break;
    case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D:
      switch (camera->modelType()) {
        case Camera::KANNALA_BRANDT:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 8, 4, 3, 2, 1, 3>(
                new ReprojectionError3<EquidistantCamera>(observed_p));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 2, 1, 3>(
                new ReprojectionError3<PinholeCamera>(observed_p));
          }
          break;
        case Camera::PINHOLE:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 3, 2, 1, 3>(
                new ReprojectionError3<PinholeCamera>(observed_p));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 2, 1, 3>(
                new ReprojectionError3<PinholeCamera>(observed_p));
          }
          break;
        case Camera::MEI:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 3, 2, 1, 3>(
                new ReprojectionError3<CataCamera>(observed_p));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 2, 2, 1, 3>(
                new ReprojectionError3<CataCamera>(observed_p));
          }
          break;
        case Camera::SCARAMUZZA:
          if (optimize_cam_odo_z) {
            costFunction =
                new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3,
                                                2, 1, 3>(new ReprojectionError3<OCAMCamera>(observed_p));
          } else {
            costFunction =
                new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2,
                                                2, 1, 3>(new ReprojectionError3<OCAMCamera>(observed_p));
          }
          break;
      }
      break;
    case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:
      switch (camera->modelType()) {
        case Camera::KANNALA_BRANDT:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 8, 4, 3, 3, 3, 3>(
                new ReprojectionError3<EquidistantCamera>(observed_p));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 3, 3, 3>(
                new ReprojectionError3<PinholeCamera>(observed_p));
          }
          break;
        case Camera::PINHOLE:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 3, 3, 3, 3>(
                new ReprojectionError3<PinholeCamera>(observed_p));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 3, 3, 3>(
                new ReprojectionError3<PinholeCamera>(observed_p));
          }
          break;
        case Camera::MEI:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 3, 3, 3, 3>(
                new ReprojectionError3<CataCamera>(observed_p));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 2, 3, 3, 3>(
                new ReprojectionError3<CataCamera>(observed_p));
          }
          break;
        case Camera::SCARAMUZZA:
          if (optimize_cam_odo_z) {
            costFunction =
                new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3,
                                                3, 3, 3>(new ReprojectionError3<OCAMCamera>(observed_p));
          } else {
            costFunction =
                new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2,
                                                3, 3, 3>(new ReprojectionError3<OCAMCamera>(observed_p));
          }
          break;
      }
      break;
  }

  return costFunction;
}

ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& camera,
                                                               const Eigen::Vector2d& observed_p,
                                                               const Eigen::Matrix2d& sqrtPrecisionMat,
                                                               int flags,
                                                               bool optimize_cam_odo_z) const {
  ceres::CostFunction* costFunction = 0;

  std::vector<double> intrinsic_params;
  camera->writeParameters(intrinsic_params);

  switch (flags) {
    case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:
      switch (camera->modelType()) {
        case Camera::KANNALA_BRANDT:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 4, 3, 3, 3, 3>(
                new ReprojectionError3<EquidistantCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 3, 3, 3>(
                new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
          }
          break;
        case Camera::PINHOLE:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 3, 3, 3, 3>(
                new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 3, 3, 3>(
                new ReprojectionError3<PinholeCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
          }
          break;
        case Camera::MEI:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 3, 3, 3, 3>(
                new ReprojectionError3<CataCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 2, 3, 3, 3>(
                new ReprojectionError3<CataCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
          }
          break;
        case Camera::SCARAMUZZA:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 3, 3, 3, 3>(
                new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 2, 3, 3, 3>(
                new ReprojectionError3<OCAMCamera>(intrinsic_params, observed_p, sqrtPrecisionMat));
          }
          break;
      }
      break;
    case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D:
      switch (camera->modelType()) {
        case Camera::KANNALA_BRANDT:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 8, 4, 3, 3, 3, 3>(
                new ReprojectionError3<EquidistantCamera>(observed_p, sqrtPrecisionMat));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 3, 3, 3>(
                new ReprojectionError3<PinholeCamera>(observed_p, sqrtPrecisionMat));
          }
          break;
        case Camera::PINHOLE:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 3, 3, 3, 3>(
                new ReprojectionError3<PinholeCamera>(observed_p, sqrtPrecisionMat));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 8, 4, 2, 3, 3, 3>(
                new ReprojectionError3<PinholeCamera>(observed_p, sqrtPrecisionMat));
          }
          break;
        case Camera::MEI:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 3, 3, 3, 3>(
                new ReprojectionError3<CataCamera>(observed_p, sqrtPrecisionMat));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 9, 4, 2, 3, 3, 3>(
                new ReprojectionError3<CataCamera>(observed_p, sqrtPrecisionMat));
          }
          break;
        case Camera::SCARAMUZZA:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2,
                                                           SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3>(
                new ReprojectionError3<OCAMCamera>(observed_p, sqrtPrecisionMat));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2,
                                                           SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3>(
                new ReprojectionError3<OCAMCamera>(observed_p, sqrtPrecisionMat));
          }
          break;
      }
      break;
  }

  return costFunction;
}

ceres::CostFunction* CostFunctionFactory::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) const {
  ceres::CostFunction* costFunction = 0;

  std::vector<double> intrinsic_params;
  camera->writeParameters(intrinsic_params);

  switch (flags) {
    case CAMERA_ODOMETRY_TRANSFORM | POINT_3D:
      switch (camera->modelType()) {
        case Camera::KANNALA_BRANDT:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 4, 3, 3>(
                new ReprojectionError3<EquidistantCamera>(intrinsic_params, odo_pos, odo_att, observed_p,
                                                          optimize_cam_odo_z));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 4, 2, 3>(
                new ReprojectionError3<EquidistantCamera>(intrinsic_params, odo_pos, odo_att, observed_p,
                                                          optimize_cam_odo_z));
          }
          break;
        case Camera::PINHOLE:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 3, 3>(
                new ReprojectionError3<PinholeCamera>(intrinsic_params, odo_pos, odo_att, observed_p,
                                                      optimize_cam_odo_z));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 4, 2, 3>(
                new ReprojectionError3<PinholeCamera>(intrinsic_params, odo_pos, odo_att, observed_p,
                                                      optimize_cam_odo_z));
          }
          break;
        case Camera::MEI:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 3, 3>(
                new ReprojectionError3<CataCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 4, 2, 3>(
                new ReprojectionError3<CataCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));
          }
          break;
        case Camera::SCARAMUZZA:
          if (optimize_cam_odo_z) {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 3, 3>(
                new ReprojectionError3<OCAMCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));
          } else {
            costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 4, 2, 3>(
                new ReprojectionError3<OCAMCamera>(intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z));
          }
          break;
      }
      break;
  }

  return costFunction;
}

ceres::CostFunction* CostFunctionFactory::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* costFunction = 0;

  std::vector<double> intrinsic_params;
  camera->writeParameters(intrinsic_params);

  switch (flags) {
    case POINT_3D:
      switch (camera->modelType()) {
        case Camera::KANNALA_BRANDT:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<EquidistantCamera>, 2, 3>(
              new ReprojectionError3<EquidistantCamera>(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att,
                                                        observed_p));
          break;
        case Camera::PINHOLE:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<PinholeCamera>, 2, 3>(
              new ReprojectionError3<PinholeCamera>(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att,
                                                    observed_p));
          break;
        case Camera::MEI:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<CataCamera>, 2, 3>(
              new ReprojectionError3<CataCamera>(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));
          break;
        case Camera::SCARAMUZZA:
          costFunction = new ceres::AutoDiffCostFunction<ReprojectionError3<OCAMCamera>, 2, 3>(
              new ReprojectionError3<OCAMCamera>(intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p));
          break;
      }
      break;
  }

  return costFunction;
}

ceres::CostFunction* CostFunctionFactory::generateCostFunction(const CameraConstPtr& cameraL,
                                                               const CameraConstPtr& cameraR,
                                                               const Eigen::Vector3d& observed_P,
                                                               const Eigen::Vector2d& observed_p_l,
                                                               const Eigen::Vector2d& observed_p_r) const {
  ceres::CostFunction* costFunction = 0;

  if (cameraL->modelType() != cameraR->modelType()) {
    return costFunction;
  }

  switch (cameraL->modelType()) {
    case Camera::KANNALA_BRANDT:
      costFunction = new ceres::AutoDiffCostFunction<StereoReprojectionError<EquidistantCamera>, 4, 8, 8, 4, 3, 4, 3>(
          new StereoReprojectionError<EquidistantCamera>(observed_P, observed_p_l, observed_p_r));
      break;
    case Camera::PINHOLE:
      costFunction = new ceres::AutoDiffCostFunction<StereoReprojectionError<PinholeCamera>, 4, 8, 8, 4, 3, 4, 3>(
          new StereoReprojectionError<PinholeCamera>(observed_P, observed_p_l, observed_p_r));
      break;
    case Camera::MEI:
      costFunction = new ceres::AutoDiffCostFunction<StereoReprojectionError<CataCamera>, 4, 9, 9, 4, 3, 4, 3>(
          new StereoReprojectionError<CataCamera>(observed_P, observed_p_l, observed_p_r));
      break;
    case Camera::SCARAMUZZA:
      costFunction =
          new ceres::AutoDiffCostFunction<StereoReprojectionError<OCAMCamera>, 4, SCARAMUZZA_CAMERA_NUM_PARAMS,
                                          SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 4, 3>(
              new StereoReprojectionError<OCAMCamera>(observed_P, observed_p_l, observed_p_r));
      break;
  }

  return costFunction;
}

}  // namespace camodocal
