/*
 * latched_stop_rotate_controller.cpp
 *
 *  Created on: Apr 16, 2012
 *      Author: tkruse
 */

#include <base_local_planner/latched_stop_rotate_controller.h>

#include <cmath>

#include <Eigen/Core>

#include <angles/angles.h>
#include <nav_msgs/Odometry.h>

#include <base_local_planner/goal_functions.h>
#include <base_local_planner/local_planner_limits.h>

namespace base_local_planner {

LatchedStopRotateController::LatchedStopRotateController() {}

void LatchedStopRotateController::initialize(const std::string& name) {
    ros::NodeHandle private_nh("~/" + name);
    std::string param_name;
    private_nh.searchParam("latch_xy_goal_tolerance", param_name);
    private_nh.param(param_name, latch_xy_goal_tolerance_, false);
    rotating_to_goal_ = false;
  }

LatchedStopRotateController::~LatchedStopRotateController() {}

/**
 * returns true if we have passed the goal position.
 * Meaning we might have overshot on the position beyond tolerance, yet still return true.
 * Also goal orientation might not yet be true
 */
bool LatchedStopRotateController::isPositionReached(LocalPlannerUtil* planner_util,
    tf::Stamped<tf::Pose> global_pose) {
  double xy_goal_tolerance = planner_util->getCurrentLimits().xy_goal_tolerance;

  //we assume the global goal is the last point in the global plan
  tf::Stamped<tf::Pose> goal_pose;
  if ( ! planner_util->getGoal(goal_pose)) {
    return false;
  }

  double goal_x = goal_pose.getOrigin().getX();
  double goal_y = goal_pose.getOrigin().getY();

  //check to see if we've reached the goal position
  if ((latch_xy_goal_tolerance_ && xy_tolerance_latch_) ||
      base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
    xy_tolerance_latch_ = true;
    return true;
  }
  return false;
}

/**
 * returns true if we have passed the goal position and have reached goal orientation.
 * Meaning we might have overshot on the position beyond tolerance, yet still return true.
 */
bool LatchedStopRotateController::isGoalReached(LocalPlannerUtil* planner_util,
    OdometryHelperRos& odom_helper,
    tf::Stamped<tf::Pose> global_pose) {
  double xy_goal_tolerance = planner_util->getCurrentLimits().xy_goal_tolerance;
  double rot_stopped_vel = planner_util->getCurrentLimits().rot_stopped_vel;
  double trans_stopped_vel = planner_util->getCurrentLimits().trans_stopped_vel;

  //copy over the odometry information
  nav_msgs::Odometry base_odom;
  odom_helper.getOdom(base_odom);

  //we assume the global goal is the last point in the global plan
  tf::Stamped<tf::Pose> goal_pose;
  if ( ! planner_util->getGoal(goal_pose)) {
    return false;
  }

  double goal_x = goal_pose.getOrigin().getX();
  double goal_y = goal_pose.getOrigin().getY();

  base_local_planner::LocalPlannerLimits limits = planner_util->getCurrentLimits();

  //check to see if we've reached the goal position
  if ((latch_xy_goal_tolerance_ && xy_tolerance_latch_) ||
      base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
    //if the user wants to latch goal tolerance, if we ever reach the goal location, we'll
    //just rotate in place
    if (latch_xy_goal_tolerance_ && ! xy_tolerance_latch_) {
      ROS_DEBUG("Goal position reached (check), stopping and turning in place");
      xy_tolerance_latch_ = true;
    }
    double goal_th = tf::getYaw(goal_pose.getRotation());
    double angle = base_local_planner::getGoalOrientationAngleDifference(global_pose, goal_th);
    //check to see if the goal orientation has been reached
    if (fabs(angle) <= limits.yaw_goal_tolerance) {
      //make sure that we're actually stopped before returning success
      if (base_local_planner::stopped(base_odom, rot_stopped_vel, trans_stopped_vel)) {
        return true;
      }
    }
  }
  return false;
}

bool LatchedStopRotateController::stopWithAccLimits(const tf::Stamped<tf::Pose>& global_pose,
    const tf::Stamped<tf::Pose>& robot_vel,
    geometry_msgs::Twist& cmd_vel,
    Eigen::Vector3f acc_lim,
    double sim_period,
    boost::function<bool (Eigen::Vector3f pos,
                          Eigen::Vector3f vel,
                          Eigen::Vector3f vel_samples)> obstacle_check) {

  //slow down with the maximum possible acceleration... we should really use the frequency that we're running at to determine what is feasible
  //but we'll use a tenth of a second to be consistent with the implementation of the local planner.
  double vx = sign(robot_vel.getOrigin().x()) * std::max(0.0, (fabs(robot_vel.getOrigin().x()) - acc_lim[0] * sim_period));
  double vy = sign(robot_vel.getOrigin().y()) * std::max(0.0, (fabs(robot_vel.getOrigin().y()) - acc_lim[1] * sim_period));

  double vel_yaw = tf::getYaw(robot_vel.getRotation());
  double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - acc_lim[2] * sim_period));

  //we do want to check whether or not the command is valid
  double yaw = tf::getYaw(global_pose.getRotation());
  bool valid_cmd = obstacle_check(Eigen::Vector3f(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), yaw),
                                  Eigen::Vector3f(robot_vel.getOrigin().getX(), robot_vel.getOrigin().getY(), vel_yaw),
                                  Eigen::Vector3f(vx, vy, vth));

  //if we have a valid command, we'll pass it on, otherwise we'll command all zeros
  if(valid_cmd){
    ROS_DEBUG_NAMED("latched_stop_rotate", "Slowing down... using vx, vy, vth: %.2f, %.2f, %.2f", vx, vy, vth);
    cmd_vel.linear.x = vx;
    cmd_vel.linear.y = vy;
    cmd_vel.angular.z = vth;
    return true;
  }
  ROS_WARN("Stopping cmd in collision");
  cmd_vel.linear.x = 0.0;
  cmd_vel.linear.y = 0.0;
  cmd_vel.angular.z = 0.0;
  return false;
}

bool LatchedStopRotateController::rotateToGoal(
    const tf::Stamped<tf::Pose>& global_pose,
    const tf::Stamped<tf::Pose>& robot_vel,
    double goal_th,
    geometry_msgs::Twist& cmd_vel,
    Eigen::Vector3f acc_lim,
    double sim_period,
    base_local_planner::LocalPlannerLimits& limits,
    boost::function<bool (Eigen::Vector3f pos,
                          Eigen::Vector3f vel,
                          Eigen::Vector3f vel_samples)> obstacle_check) {
  double yaw = tf::getYaw(global_pose.getRotation());
  double vel_yaw = tf::getYaw(robot_vel.getRotation());
  cmd_vel.linear.x = 0;
  cmd_vel.linear.y = 0;
  double ang_diff = angles::shortest_angular_distance(yaw, goal_th);

  double v_theta_samp = std::min(limits.max_rot_vel, std::max(limits.min_rot_vel, fabs(ang_diff)));

  //take the acceleration limits of the robot into account
  double max_acc_vel = fabs(vel_yaw) + acc_lim[2] * sim_period;
  double min_acc_vel = fabs(vel_yaw) - acc_lim[2] * sim_period;

  v_theta_samp = std::min(std::max(fabs(v_theta_samp), min_acc_vel), max_acc_vel);

  //we also want to make sure to send a velocity that allows us to stop when we reach the goal given our acceleration limits
  double max_speed_to_stop = sqrt(2 * acc_lim[2] * fabs(ang_diff));
  v_theta_samp = std::min(max_speed_to_stop, fabs(v_theta_samp));

  v_theta_samp = std::min(limits.max_rot_vel, std::max(limits.min_rot_vel, v_theta_samp));

  if (ang_diff < 0) {
    v_theta_samp = - v_theta_samp;
  }

  //we still want to lay down the footprint of the robot and check if the action is legal
  bool valid_cmd = obstacle_check(Eigen::Vector3f(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), yaw),
      Eigen::Vector3f(robot_vel.getOrigin().getX(), robot_vel.getOrigin().getY(), vel_yaw),
      Eigen::Vector3f( 0.0, 0.0, v_theta_samp));

  if (valid_cmd) {
    ROS_DEBUG_NAMED("dwa_local_planner", "Moving to desired goal orientation, th cmd: %.2f, valid_cmd: %d", v_theta_samp, valid_cmd);
    cmd_vel.angular.z = v_theta_samp;
    return true;
  }
  ROS_WARN("Rotation cmd in collision");
  cmd_vel.angular.z = 0.0;
  return false;

}

bool LatchedStopRotateController::computeVelocityCommandsStopRotate(geometry_msgs::Twist& cmd_vel,
    Eigen::Vector3f acc_lim,
    double sim_period,
    LocalPlannerUtil* planner_util,
    OdometryHelperRos& odom_helper_,
    tf::Stamped<tf::Pose> global_pose,
    boost::function<bool (Eigen::Vector3f pos,
                          Eigen::Vector3f vel,
                          Eigen::Vector3f vel_samples)> obstacle_check) {
  //we assume the global goal is the last point in the global plan
  tf::Stamped<tf::Pose> goal_pose;
  if ( ! planner_util->getGoal(goal_pose)) {
    ROS_ERROR("Could not get goal pose");
    return false;
  }

  base_local_planner::LocalPlannerLimits limits = planner_util->getCurrentLimits();

  //if the user wants to latch goal tolerance, if we ever reach the goal location, we'll
  //just rotate in place
  if (latch_xy_goal_tolerance_ && ! xy_tolerance_latch_ ) {
    ROS_INFO("Goal position reached, stopping and turning in place");
    xy_tolerance_latch_ = true;
  }
  //check to see if the goal orientation has been reached
  double goal_th = tf::getYaw(goal_pose.getRotation());
  double angle = base_local_planner::getGoalOrientationAngleDifference(global_pose, goal_th);
  if (fabs(angle) <= limits.yaw_goal_tolerance) {
    //set the velocity command to zero
    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = 0.0;
    rotating_to_goal_ = false;
  } else {
    ROS_DEBUG("Angle: %f Tolerance: %f", angle, limits.yaw_goal_tolerance);
    tf::Stamped<tf::Pose> robot_vel;
    odom_helper_.getRobotVel(robot_vel);
    nav_msgs::Odometry base_odom;
    odom_helper_.getOdom(base_odom);

    //if we're not stopped yet... we want to stop... taking into account the acceleration limits of the robot
    if ( ! rotating_to_goal_ && !base_local_planner::stopped(base_odom, limits.rot_stopped_vel, limits.trans_stopped_vel)) {
      if ( ! stopWithAccLimits(
          global_pose,
          robot_vel,
          cmd_vel,
          acc_lim,
          sim_period,
          obstacle_check)) {
        ROS_INFO("Error when stopping.");
        return false;
      }
      ROS_DEBUG("Stopping...");
    }
    //if we're stopped... then we want to rotate to goal
    else {
      //set this so that we know its OK to be moving
      rotating_to_goal_ = true;
      if ( ! rotateToGoal(
          global_pose,
          robot_vel,
          goal_th,
          cmd_vel,
          acc_lim,
          sim_period,
          limits,
          obstacle_check)) {
        ROS_INFO("Error when rotating.");
        return false;
      }
      ROS_DEBUG("Rotating...");
    }
  }

  return true;

}

} /* namespace base_local_planner */


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_DEFINITION_PROD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DEFINITION_PROD_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    BOOST_DISPATCH_MAKE_TAG ( ext, prod_
                            , (boost::dispatch::reduction_<prod_,multiplies_,one_>)
                            );
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, prod_)
  }

  BOOST_DISPATCH_CALLABLE_DEFINITION(tag::prod_,prod);
} }

#endif


/**
 * @file cameras/EUCM.hpp
 * @brief Header file for the extended unified camera model class.
 * @author Jianzhu Huai
 */

#ifndef INCLUDE_OKVIS_CAMERAS_EUCM_HPP_
#define INCLUDE_OKVIS_CAMERAS_EUCM_HPP_

#include <Eigen/Core>
#include <memory>
#include <stdint.h>
#include <vector>
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
#include <opencv2/core/core.hpp> // Code that causes warning goes here
#pragma GCC diagnostic pop
#include "okvis/cameras/CameraBase.hpp"
#include "okvis/cameras/extended_camera.hpp"
#include <okvis/assert_macros.hpp>

/// \brief okvis Main namespace of this package.
namespace okvis {
/// \brief cameras Namespace for camera-related functionality.
namespace cameras {

class EUCM : public CameraBase {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef EUCM distortion_t; ///< To be compatible with other distortion models.

  EUCM();

  /// \brief Constructor
  /// @param[in] imageWidth The width in pixels.
  /// @param[in] imageHeight The height in pixels.
  /// @param[in] focalLengthU The horizontal focal length in pixels.
  /// @param[in] focalLengthV The vertical focal length in pixels.
  /// @param[in] imageCenterU The horizontal centre in pixels.
  /// @param[in] imageCenterV The vertical centre in pixels.
  /// @param[in] id Assign a generic ID, if desired.
  EUCM(int imageWidth, int imageHeight, double focalLengthU,
       double focalLengthV, double imageCenterU, double imageCenterV,
       double alpha, double beta, double delayTime = 0.0,
       double readoutTime = 0.0, uint64_t id = -1);

  /// \brief Destructor.
  ~EUCM() {}

  static const int NumProjectionIntrinsics =
      4;                              ///< optimisable projection intrinsics
  static const int NumIntrinsics = 6; ///< total number of intrinsics
  static const int NumDistortionIntrinsics = 2; ///< To be compatible with other distortion models.

  /// \brief Get the focal length along the u-dimension.
  /// \return The horizontal focal length in pixels.
  double focalLengthU() const { return eucm_.getParam()[0]; }

  /// \brief Get the focal length along the v-dimension.
  /// \return The vertical focal length in pixels.
  double focalLengthV() const { return eucm_.getParam()[1]; }

  /// \brief Get the image centre along the u-dimension.
  /// \return The horizontal centre in pixels.
  double imageCenterU() const { return eucm_.getParam()[2]; }

  /// \brief Get the focal image centre along the v-dimension.
  /// \return The vertical centre in pixels.
  double imageCenterV() const { return eucm_.getParam()[3]; }

  /// \brief Get the intrinsics as a concatenated vector.
  /// \return The intrinsics as a concatenated vector.
  void getIntrinsics(Eigen::VectorXd &intrinsics) const;

  /// \brief overwrite all intrinsics - use with caution !
  /// \param[in] intrinsics The intrinsics as a concatenated vector.
  bool setIntrinsics(const Eigen::VectorXd &intrinsics);

  /// \brief Get the total number of intrinsics.
  /// \return Number of intrinsics parameters.
  inline int noIntrinsicsParameters() const { return NumIntrinsics; }

  inline int noDistortionParameters() const { return 2; }
  //////////////////////////////////////////////////////////////
  /// \name Methods to project points
  /// @{

  /// \brief Projects a Euclidean point to a 2d image point (projection).
  /// @param[in]  point      The point in Euclidean coordinates.
  /// @param[out] imagePoint The image point.
  /// @return     Get information about the success of the projection. See
  ///             \ref ProjectionStatus for more information.
  CameraBase::ProjectionStatus
  project(const Eigen::Vector3d &point, Eigen::Vector2d *imagePoint) const;

  /// \brief Projects a Euclidean point to a 2d image point (projection).
  /// @param[in]  point              The point in Euclidean coordinates.
  /// @param[out] imagePoint         The image point.
  /// @param[out] pointJacobian      The Jacobian of the projection function
  /// w.r.t. the point..
  /// @param[out] intrinsicsJacobian The Jacobian of the projection function
  /// w.r.t. the intinsics.
  /// @return     Get information about the success of the projection. See
  ///             \ref ProjectionStatus for more information.
  CameraBase::ProjectionStatus
  project(const Eigen::Vector3d &point, Eigen::Vector2d *imagePoint,
          Eigen::Matrix<double, 2, 3> *pointJacobian,
          Eigen::Matrix2Xd *intrinsicsJacobian = NULL) const;

  /// \brief Projects a Euclidean point to a 2d image point (projection).
  /// @param[in]  point              The point in Euclidean coordinates.
  /// @param[in]  parameters         The intrinsics.
  /// @param[out] imagePoint         The image point.
  /// @param[out] pointJacobian      The Jacobian of the projection function
  /// w.r.t. the point..
  /// @param[out] intrinsicsJacobian The Jacobian of the projection function
  /// w.r.t. the intinsics.
  /// @return     Get information about the success of the projection. See
  ///             \ref ProjectionStatus for more information.
  CameraBase::ProjectionStatus projectWithExternalParameters(
      const Eigen::Vector3d &point, const Eigen::VectorXd &parameters,
      Eigen::Vector2d *imagePoint, Eigen::Matrix<double, 2, 3> *pointJacobian,
      Eigen::Matrix2Xd *intrinsicsJacobian = NULL) const;

  /// \brief Projects Euclidean points to 2d image points (projection) in a
  /// batch.
  /// @param[in]  points      The points in Euclidean coordinates (one point per
  /// column).
  /// @param[out] imagePoints The image points (one point per column).
  /// @param[out] stati       Get information about the success of the
  /// projections. See
  ///                         \ref ProjectionStatus for more information.
  inline void
  projectBatch(const Eigen::Matrix3Xd &/*points*/, Eigen::Matrix2Xd */*imagePoints*/,
               std::vector<CameraBase::ProjectionStatus> */*stati*/) const {
    OKVIS_THROW(std::runtime_error, "Not implemented.");
  }

  /// \brief Projects a point in homogenous coordinates to a 2d image point
  /// (projection).
  /// @param[in]  point      The point in Homogeneous coordinates.
  /// @param[out] imagePoint The image point.
  /// @return     Get information about the success of the projection. See
  ///             \ref ProjectionStatus for more information.
  CameraBase::ProjectionStatus
  projectHomogeneous(const Eigen::Vector4d &point, Eigen::Vector2d *imagePoint) const;

  /// \brief Projects a point in homogenous coordinates to a 2d image point
  /// (projection).
  /// @param[in]  point              The point in Homogeneous coordinates.
  /// @param[out] imagePoint         The image point.
  /// @param[out] pointJacobian      The Jacobian of the projection function
  /// w.r.t. the point.
  /// @param[out] intrinsicsJacobian The Jacobian of the projection function
  /// w.r.t. the intrinsics.
  /// @return     Get information about the success of the projection. See
  ///             \ref ProjectionStatus for more information.
  CameraBase::ProjectionStatus
  projectHomogeneous(const Eigen::Vector4d &point, Eigen::Vector2d *imagePoint,
                     Eigen::Matrix<double, 2, 4> *pointJacobian,
                     Eigen::Matrix2Xd *intrinsicsJacobian = NULL) const;

  /// \brief Projects a point in homogenous coordinates to a 2d image point
  /// (projection).
  /// @param[in]  point              The point in Homogeneous coordinates.
  /// @param[in]  parameters         The intrinsics.
  /// @param[out] imagePoint         The image point.
  /// @param[out] pointJacobian      The Jacobian of the projection function
  /// w.r.t. the point.
  /// @param[out] intrinsicsJacobian The Jacobian of the projection function
  /// w.r.t. the intrinsics.
  /// @return     Get information about the success of the projection. See
  ///             \ref ProjectionStatus for more information.
  inline CameraBase::ProjectionStatus projectHomogeneousWithExternalParameters(
      const Eigen::Vector4d &point, const Eigen::VectorXd &parameters,
      Eigen::Vector2d *imagePoint,
      Eigen::Matrix<double, 2, 4> *pointJacobian = NULL,
      Eigen::Matrix2Xd *intrinsicsJacobian = NULL) const;

  /// \brief Projects points in homogenous coordinates to 2d image points
  /// (projection) in a batch.
  /// @param[in]  points      The points in homogeneous coordinates (one point
  /// per column).
  /// @param[out] imagePoints The image points (one point per column).
  /// @param[out] stati       Get information about the success of the
  /// projections. See
  ///                         \ref ProjectionStatus for more information.
  inline void projectHomogeneousBatch(
      const Eigen::Matrix4Xd &/*points*/, Eigen::Matrix2Xd */*imagePoints*/,
      std::vector<CameraBase::ProjectionStatus> */*stati*/) const {
    OKVIS_THROW(std::runtime_error, "Not implemented.");
  }

  /// @}

  //////////////////////////////////////////////////////////////
  /// \name Methods to backproject points
  /// @{

  /// \brief Back-project a 2d image point into Euclidean space (direction
  /// vector).
  /// @param[in]  imagePoint The image point.
  /// @param[out] direction  The Euclidean direction vector.
  /// @return     true on success.
  bool backProject(const Eigen::Vector2d &imagePoint,
                          Eigen::Vector3d *direction) const;

  /// \brief Back-project a 2d image point into Euclidean space (direction
  /// vector).
  /// @param[in]  imagePoint         The image point.
  /// @param[out] direction          The Euclidean direction vector.
  /// @param[out] pointJacobian      Jacobian of the back-projection function
  /// w.r.t. the point.
  /// @return     true on success.
  inline bool backProject(const Eigen::Vector2d &/*imagePoint*/,
                          Eigen::Vector3d */*direction*/,
                          Eigen::Matrix<double, 3, 2> */*pointJacobian*/) const {
    OKVIS_THROW(std::runtime_error, "Not implemented.");
    return false;
  }

  /// \brief Back-project 2d image points into Euclidean space (direction
  /// vectors).
  /// @param[in]  imagePoints The image points (one point per column).
  /// @param[out] directions  The Euclidean direction vectors (one point per
  /// column).
  /// @param[out] success     Success of each of the back-projection
  inline bool backProjectBatch(const Eigen::Matrix2Xd &/*imagePoints*/,
                               Eigen::Matrix3Xd */*directions*/,
                               std::vector<bool> */*success*/) const {
    OKVIS_THROW(std::runtime_error, "Not implemented.");
    return false;
  }

  /// \brief Back-project a 2d image point into homogeneous point (direction
  /// vector).
  /// @param[in]  imagePoint The image point.
  /// @param[out] direction  The homogeneous point as direction vector.
  /// @return     true on success.
  inline bool backProjectHomogeneous(const Eigen::Vector2d &/*imagePoint*/,
                                     Eigen::Vector4d */*direction*/) const {
    OKVIS_THROW(std::runtime_error, "Not implemented.");
    return false;
  }

  /// \brief Back-project a 2d image point into homogeneous point (direction
  /// vector).
  /// @param[in]  imagePoint         The image point.
  /// @param[out] direction          The homogeneous point as direction vector.
  /// @param[out] pointJacobian      Jacobian of the back-projection function.
  /// @return     true on success.
  inline bool
  backProjectHomogeneous(const Eigen::Vector2d &/*imagePoint*/,
                         Eigen::Vector4d */*direction*/,
                         Eigen::Matrix<double, 4, 2> */*pointJacobian*/) const {
    OKVIS_THROW(std::runtime_error, "Not implemented.");
    return false;
  }

  /// \brief Back-project 2d image points into homogeneous points (direction
  /// vectors).
  /// @param[in]  imagePoints The image points (one point per column).
  /// @param[out] directions  The homogeneous points as direction vectors (one
  /// point per column).
  /// @param[out] success     Success of each of the back-projection
  inline bool backProjectHomogeneousBatch(const Eigen::Matrix2Xd &/*imagePoints*/,
                                          Eigen::Matrix4Xd */*directions*/,
                                          std::vector<bool> */*success*/) const {
    OKVIS_THROW(std::runtime_error, "Not implemented.");
    return false;
  }

  /// @}

  /// \brief get a test instance
  static std::shared_ptr<CameraBase> createTestObject() {
    return std::shared_ptr<CameraBase>(new EUCM(
        752, 480, 460.76484651566468, 459.4051018049483, 365.8937161309615,
        249.33499869752445, 0.5903365915227143, 1.127468196965374, 0.1, 0.02));
  }

  /// \brief get a test instance
  static EUCM testObject() {
    return EUCM(752, 480, 460.76484651566468, 459.4051018049483,
                365.8937161309615, 249.33499869752445, 0.5903365915227143,
                1.127468196965374, 0.1, 0.02);
  }

  /// \brief Obtain the projection type
  std::string type() const { return "EUCM"; }

  /// \brief Obtain the projection type
  const std::string distortionType() const { return "EUCM"; }

protected:

  ExtendedUnifiedCamera<double> eucm_;
};

} // namespace cameras
} // namespace okvis

#endif /* INCLUDE_OKVIS_CAMERAS_EUCM_HPP_ */