/*
 * math_utils.h
 *
 *  Created on: Jul 20, 2012
 *      Author: cforster
 */

#ifndef MATH_UTILS_H_
#define MATH_UTILS_H_


#include <Eigen/Core>
#include <Eigen/StdVector>
#include <sophus/se3.h>
#include <iostream>

namespace vk
{

using namespace Eigen;
using namespace std;
using namespace Sophus;

Vector3d triangulateFeatureNonLin(
    const Matrix3d& R,
    const Vector3d& t,
    const Vector3d& feature1,
    const Vector3d& feature2);

/// Assumes the bearing vectors f_c and f_r are on the epipolar plane, i.e.
/// perfect triangulation without noise!
bool depthFromTriangulationExact(
    const Matrix3d& R_r_c,
    const Vector3d& t_r_c,
    const Vector3d& f_r,
    const Vector3d& f_c,
    double& depth_in_r,
    double& depth_in_c);

double reprojError(
    const Vector3d& f1,
    const Vector3d& f2,
    double error_multiplier2);

double computeInliers(
    const vector<Vector3d>& features1,
    const vector<Vector3d>& features2,
    const Matrix3d& R,
    const Vector3d& t,
    const double reproj_thresh,
    double error_multiplier2,
    vector<Vector3d>& xyz_vec,
    vector<int>& inliers,
    vector<int>& outliers);

void computeInliersOneView(
    const vector<Vector3d> & feature_sphere_vec,
    const vector<Vector3d> & xyz_vec,
    const Matrix3d &R,
    const Vector3d &t,
    const double reproj_thresh,
    const double error_multiplier2,
    vector<int>& inliers,
    vector<int>& outliers);

//! Direct Cosine Matrix to Roll Pitch Yaw
Vector3d dcm2rpy(const Matrix3d &R);

//! Roll Pitch Yaw to Direct Cosine Matrix
Matrix3d rpy2dcm(const Vector3d &rpy);

//! Angle Axis parametrization to Quaternion
Quaterniond angax2quat(const Vector3d& n, const double& angle);

//! Angle Axis parametrization to Matrix representation
Matrix3d angax2dcm(const Vector3d& n, const double& angle);

double sampsonusError(
    const Vector2d &v2Dash,
    const Matrix3d& m3Essential,
    const Vector2d& v2);

inline Matrix3d sqew(const Vector3d& v)
{
  Matrix3d v_sqew;
  v_sqew << 0, -v[2], v[1],
            v[2], 0, -v[0],
            -v[1], v[0], 0;
  return v_sqew;
}

inline double norm_max(const Eigen::VectorXd & v)
{
  double max = -1;
  for (int i=0; i<v.size(); i++)
  {
    double abs = fabs(v[i]);
    if(abs>max){
      max = abs;
    }
  }
  return max;
}

inline Vector2d project2d(const Vector3d& v)
{
  return v.head<2>()/v[2];
}

inline Vector3d unproject2d(const Vector2d& v)
{
  return Vector3d(v[0], v[1], 1.0);
}

inline Vector3d project3d(const Vector4d& v)
{
  return v.head<3>()/v[3];
}

inline Vector4d unproject3d(const Vector3d& v)
{
  return Vector4d(v[0], v[1], v[2], 1.0);
}

template<class T>
T getMedian(vector<T>& data_vec)
{
  assert(!data_vec.empty());
  typename vector<T>::iterator it = data_vec.begin()+floor(data_vec.size()/2);
  nth_element(data_vec.begin(), it, data_vec.end());
  return *it;
}

inline double pyrFromZero_d(double x_0, int level)
{
  return x_0/(1<<level); // = 1 / 2^level
}

inline Vector2d pyrFromZero_2d(const Vector2d& uv_0, int level)
{
  return Vector2d(pyrFromZero_d(uv_0[0], level),
                  pyrFromZero_d(uv_0[1], level));
}

inline void
frameJac_xyz2uv(const Vector3d & xyz,
                 const double & focal_length,
                 Matrix<double,2,6> & frame_jac)
{
  const double x = xyz[0];
  const double y = xyz[1];
  const double z = xyz[2];
  const double z_2 = z*z;

  frame_jac(0,0) = -1./z *focal_length;
  frame_jac(0,1) = 0;
  frame_jac(0,2) = x/z_2 *focal_length;
  frame_jac(0,3) =  x*y/z_2 * focal_length;
  frame_jac(0,4) = -(1+(x*x/z_2)) *focal_length;
  frame_jac(0,5) = y/z *focal_length;

  frame_jac(1,0) = 0;
  frame_jac(1,1) = -1./z *focal_length;
  frame_jac(1,2) = y/z_2 *focal_length;
  frame_jac(1,3) = (1+y*y/z_2) *focal_length;
  frame_jac(1,4) = -x*y/z_2 *focal_length;
  frame_jac(1,5) = -x/z *focal_length;
}

} // end namespace vk

#endif /* MATH_UTILS_H_ */