#include <iostream>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include<opencv2/core/eigen.hpp>
#include <chrono>
#include <sophus/se3.hpp>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>

using namespace std;
using namespace cv;
using namespace Eigen;

// 相机内参
Mat K = (Mat_<double>(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

void find_feature_matches(
  const Mat &img_1, const Mat &img_2,
  std::vector<KeyPoint> &keypoints_1,
  std::vector<KeyPoint> &keypoints_2,
  std::vector<DMatch> &matches);

void bundleAdjustmentGaussNewton(const VecVector3d &points_3d, const VecVector2d &points_2d,
                                 const Mat &K, Sophus::SE3d &pose);
void bundleAdjustmentG2O(const VecVector3d &points_3d, const VecVector2d &points_2d,
                         const Mat &K, Sophus::SE3d &pose);

// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &p);

int main(int argc, char **argv){

  // 读取图像
  Mat img_1 = imread("../1.png", CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread("../2.png", CV_LOAD_IMAGE_COLOR);
  assert(img_1.data && img_2.data);
  cout << "读取图像 完成！" << endl;

  // 特征点匹配
  cout << "开始特征点匹配 ......" << endl;
  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
  cout << "特征点匹配 完成！ 一共找到了" << matches.size() << "组匹配点" << endl << endl;
  // for (DMatch m:matches) {
  //   cout << keypoints_1[m.queryIdx].pt.x << " " << keypoints_1[m.queryIdx].pt.y << endl;
  // }

  // 读取深度图，建立3D点
  Mat img_depth_1 = imread("../1_depth.png", CV_LOAD_IMAGE_UNCHANGED);
  Mat img_depth_2 = imread("../2_depth.png", CV_LOAD_IMAGE_UNCHANGED);

  vector<Point3d> points_3d;
  vector<Point2d> points_2d;

  for (DMatch m:matches){
    // ushort d = img_depth_1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
    ushort d = img_depth_1.at<unsigned short>((int)keypoints_1[m.queryIdx].pt.y, (int)keypoints_1[m.queryIdx].pt.x);
    if (d==0) continue;
    float dd = d / 5000.0;
    Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt);
    points_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
    points_2d.push_back(keypoints_2[m.trainIdx].pt);
  }
  cout << "Total number of valid 3d-2d pairs: " << points_3d.size() << endl;

  cout << "开始PnP求解 ......" << endl;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  Mat R, r, t;
  solvePnP(points_3d, points_2d, K, Mat(), r, t, false);
  cv::Rodrigues(r, R);
  cout << "R = " << endl << R << endl;
  cout << "t = " << t.t() << endl;
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;
  cout << endl;

  cout << "开始手写G-N优化 ......" << endl;
  VecVector3d points_3d_eig;
  VecVector2d points_2d_eig;
  for(size_t i=0; i<points_3d.size(); i++){
    Point3d p_3d = points_3d[i];
    Point2d p_2d = points_2d[i];
    points_3d_eig.push_back(Vector3d(p_3d.x, p_3d.y, p_3d.z));
    points_2d_eig.push_back(Vector2d(p_2d.x, p_2d.y));
  }

  Matrix3d R_eig;
  Vector3d t_eig;
  cv2eigen(R, R_eig);
  cv2eigen(t, t_eig);
  Sophus::SE3d pose_gn(R_eig, t_eig);
  t1 = chrono::steady_clock::now();
  bundleAdjustmentGaussNewton(points_3d_eig, points_2d_eig, K, pose_gn);
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "pose by g-n: " << endl << pose_gn.matrix() << endl;
  cout << "solve pnp by gauss newton cost time: " << time_used.count() << " seconds." << endl << endl;

  cout << "开始G2O优化 ......" << endl;
  Sophus::SE3d pose_g2o(R_eig, t_eig);
  bundleAdjustmentG2O(points_3d_eig, points_2d_eig, K, pose_g2o);
  cout << "pose by g2o: " << endl << pose_gn.matrix() << endl;

  return 0;
}

void bundleAdjustmentGaussNewton(const VecVector3d &points_3d, const VecVector2d &points_2d,
                                 const Mat &K, Sophus::SE3d &pose){

  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  const int iterations = 10;
  double cost = 0, lastCost = 0;
  double fx = K.at<double>(0, 0);
  double fy = K.at<double>(1, 1);
  double cx = K.at<double>(0, 2);
  double cy = K.at<double>(1, 2);

  for (int iter=0; iter<iterations; iter++){
    Matrix<double, 6, 6> H = Matrix<double, 6, 6>::Zero();
    Vector6d b = Vector6d::Zero();
    cost = 0;

    for (int i=0; i<points_3d.size(); i++){
      Vector3d pc = pose * points_3d[i];
      double inv_z = 1.0 / pc[2];
      double inv_z2 = inv_z * inv_z;
      Vector2d proj (fx*pc[0]*inv_z + cx,
                     fy*pc[1]*inv_z + cy);
      Vector2d e = points_2d[i] - proj;

      cost += e.squaredNorm();

      Matrix<double, 2, 6> J;
      J << -fx * inv_z,
        0,
        fx * pc[0] * inv_z2,
        fx * pc[0] * pc[1] * inv_z2,
        -fx - fx * pc[0] * pc[0] * inv_z2,
        fx * pc[1] * inv_z,
        0,
        -fy * inv_z,
        fy * pc[1] * inv_z2,
        fy + fy * pc[1] * pc[1] * inv_z2,
        -fy * pc[0] * pc[1] * inv_z2,
        -fy * pc[0] * inv_z;

      H += J.transpose() * J;
      b +=  - J.transpose() * e;
    }

    Vector6d dx;
    dx = H.ldlt().solve(b);

    if (isnan(dx[0])){
      cout << "result is nan!" << endl;
    }

    if (iter > 0 && cost > lastCost){
      cout << "the cost is larger than last cost!";
      break;
    }

    pose *= Sophus::SE3d::exp(dx);
    lastCost = cost;

    cout << "iteration " << iter << " cost=" << cost << endl;

    if (dx.norm() < 1e-6){
      cout << "have converge." << endl;
      break;
    }
  }
}

void find_feature_matches(
  const Mat &img_1, const Mat &img_2,
  std::vector<KeyPoint> &keypoints_1,
  std::vector<KeyPoint> &keypoints_2,
  std::vector<DMatch> &matches){

    Mat descriptors_1, descriptors_2;
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");

    // --第一步：检测Oriented Fast角点位置
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);
    cout << "--第一步完成：检测Oriented Fast角点位置" << endl;

    // --第二步：根据角点位置计算BRIEF描述子
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);
    cout << "--第二步完成：根据角点位置计算BRIEF描述子" << endl;

    // -- 第三步：对两幅图像中的BRIEF描述子进行匹配，使用Hamming距离
    vector<DMatch> match;
    matcher->match(descriptors_1, descriptors_2, match);
    cout << "--第三步完成：对两幅图像中的BRIEF描述子进行匹配，使用Hamming距离" << endl;

    // --第四步：匹配点对 筛选
    auto min_max = minmax_element(match.begin(), match.end(),
                [] (const DMatch &m1, const DMatch &m2) {return m1.distance < m2.distance;});
    double min_dist = min_max.first->distance;
    double max_dist = min_max.second->distance;

    for (int i=0; i<descriptors_1.rows; i++){
        if(match[i].distance <= max(2*min_dist, 30.0)){
            matches.push_back(match[i]);
        }
    }
    cout << "--第四步完成：匹配点对 筛选" << endl;
}

Point2d pixel2cam(const Point2d &p) {
  return Point2d
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

class VertexPose: public g2o::BaseVertex<6, Sophus::SE3d>{
  public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  virtual void setToOriginImpl() override {
    _estimate = Sophus::SE3d();
  }

  virtual void oplusImpl(const double *update) override {
    Eigen::Matrix<double, 6, 1> update_eigen;
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
  }

  virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}
};

class EdgeProjection: public g2o::BaseUnaryEdge<2, Vector2d, VertexPose>{
  public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeProjection(const Vector3d &pos, const Matrix3d &K): _pos3d(pos), _K(K){}

  virtual void computeError() override{
    const VertexPose *v = static_cast<VertexPose *>(_vertices[0]);
    Sophus::SE3d T = v->estimate();
    Vector3d pos_pixel = _K * (T * _pos3d);
    pos_pixel = pos_pixel / pos_pixel[2];
    _error = _measurement - pos_pixel.head<2>();
  }

  virtual void linearizeOplus() override {
    const VertexPose *v = static_cast<VertexPose *>(_vertices[0]);
    Sophus::SE3d T = v->estimate();
    Vector3d pos_cam = T * _pos3d;
    double fx = _K(0, 0);
    double fy = _K(1, 1);
    double cx = _K(0, 2);
    double cy = _K(1, 2);
    double X = pos_cam[0];
    double Y = pos_cam[1];
    double Z = pos_cam[2];
    double Z2 = Z * Z;
    _jacobianOplusXi
      << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
      0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
  }

  virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}

  private:
  Eigen::Vector3d _pos3d;
  Eigen::Matrix3d _K;
};

void bundleAdjustmentG2O(const VecVector3d &points_3d, const VecVector2d &points_2d,
                         const Mat &K, Sophus::SE3d &pose){
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;

  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;
  optimizer.setAlgorithm(solver);
  optimizer.setVerbose(true);

  VertexPose *vertex_pose = new VertexPose();
  vertex_pose->setId(0);
  vertex_pose->setEstimate(Sophus::SE3d());
  optimizer.addVertex(vertex_pose);

  Matrix3d K_eigen;
  cv2eigen(K, K_eigen);

  for(size_t i=0; i < points_3d.size(); i++){
    auto p2d = points_2d[i];
    auto p3d = points_3d[i];
    EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);
    edge->setId(i);
    edge->setVertex(0, vertex_pose);
    edge->setMeasurement(p2d);
    edge->setInformation(Matrix2d::Identity());
    optimizer.addEdge(edge);
  }

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
  pose = vertex_pose->estimate();
}


/*
    EigenQP: Fast quadradic programming template library based on Eigen.

    From https://github.com/jarredbarber/eigen-QP

    MIT License

    Copyright (c) 2017 Jarred Barber

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/
#ifndef _EIGEN_QP_H_
#define _EIGEN_QP_H_

#include <Eigen/Dense>

/**
 * Solves quadradic programs with equality constraints
 *  using direct matrix factorization of the KKT system.
 */

namespace EigenQP
{

// Default tolerance levels specialized on types
template<typename t> t defTol();
template<>
    inline double defTol<double>() { return 1E-9; }
template<>
    inline float defTol<float>()   { return 1E-4f; }

/*
 * Solver for equality constrained problems.
 *  The KKT conditions are linear here, so we just
 *  invert with an LDLT decomposition.
 */
template<typename Scalar, int NVars=-1, int NEq=-1>
class QPEqSolver
{
private:
    const int n;
    const int m;

    static constexpr int NWork =
           ((NVars == -1) || (NEq == -1)) ? -1 : (NVars+NEq);
    Eigen::Matrix<Scalar,NWork,NWork> Z;
    Eigen::Matrix<Scalar,NWork,1> C;

public:
    QPEqSolver(int n_vars=NVars, int n_const=NEq) : n(n_vars),m(n_const),
            Z(n+m,n+m), C(n+m,1)
        {
            Z.block(n,n,m,m).setZero();
        }
    void solve(Eigen::Matrix<Scalar,NVars,NVars> &Q, Eigen::Matrix<Scalar,NVars,1> &c,
              Eigen::Matrix<Scalar,NEq,NVars> &A, Eigen::Matrix<Scalar,NEq,1> &b,
              Eigen::Matrix<Scalar,NVars,1> &x)
    {
        Z.block(0,0,n,n) = Q;
        Z.block(0,n,n,m) = A.adjoint();
        Z.block(n,0,m,n) = A;

        C.head(n) = -c;
        C.tail(m) = b;

        x = Z.ldlt().solve(C).head(n);
    }
};

/*
 * Solver for inequality constrained problems
 *
 * This uses a predictor-corrector interior point method from
 * "Interior-Point Algorithms for Quadratic Programming" by Thomas Kruth
 *
 * Some small notation changes from Kruth => this code:
 *
 * G => Q
 * g => c
 * A => A.adjoint() (i.e., the constraint matrix is transposed)
 * lambda => z
 */
template<typename Scalar, int NVars=-1, int NIneq=-1>
class QPIneqSolver
{
    typedef Eigen::Matrix<Scalar,NVars,1> PVec;
    typedef Eigen::Matrix<Scalar,NIneq,1> DVec; // Dual (i.e., Lagrange multiplier) vector
    typedef Eigen::Matrix<Scalar,NVars,NVars> PMat;
private:
    // Problem size
    const int n;
    const int m;

    // Work buffers
    DVec s;
    DVec z;

    PVec rd;
    DVec rp;
    DVec rs;

    PVec dx;
    DVec ds;
    DVec dz;

    PVec x;

public:
    // Parameters
    Scalar tolerance;
    int    max_iters;

    QPIneqSolver(int n_vars=NVars, int n_const=NIneq) : n(n_vars),m(n_const), s(m), z(m), rd(n), rp(m), rs(m), dx(n), ds(m), dz(m)
        {
            tolerance = defTol<Scalar>();
            max_iters = 250;
            if (NVars == -1) {
                x.resize(n_vars);
            }
            if (NIneq == -1) {
                s.resize(n_const);
                z.resize(n_const);
            }
        }

    ~QPIneqSolver() {}

    void solve(Eigen::Matrix<Scalar,NVars,NVars> &Q, Eigen::Matrix<Scalar,NVars,1> &c,
              Eigen::Matrix<Scalar,NIneq,NVars> &A, Eigen::Matrix<Scalar,NIneq,1> &b,
              Eigen::Matrix<Scalar,NVars,1> &x_out)
    {
        const Scalar eta(0.95);
        const Scalar eps = tolerance;

        // Initialization
        s.setOnes();
        z.setOnes();
        x.setZero();

        // Initial residuals. Uses fact that x=0 here.
        rd = c - A.adjoint()*z;
        rp = s + b;
        rs = (s.array()*z.array());

        const Scalar ms = Scalar(1.0)/(Scalar)m;
        Scalar mu = (Scalar)n/((Scalar)m); // Initial mu based on knowing that s,z are ones.
        Scalar alpha;

        for (int iter=0; iter < max_iters; iter++)
        {
            // Precompute decompositions for this iteration
            Eigen::LLT<PMat> Gbar = (Q + A.adjoint()*((z.array()/s.array()).matrix().asDiagonal())*A).llt();

            for (int ii=0; ii < 2; ii++)
            {
                // Prediction/correction step
                {
                    auto tmp = (rs.array() - z.array()*rp.array())/s.array();
                    dx = -Gbar.solve(rd + A.adjoint()*tmp.matrix());
                    ds = A*dx - rp;
                    dz.array() = -(rs.array() + z.array()*ds.array())/s.array();
                }

                // Compute alph,mu
                alpha = Scalar(1.0);
                for (int jj=0; jj < m; jj++)
                {
                    Scalar a = -z(jj)/dz(jj);
                    alpha    = (a < alpha) && (a > 0) ? a : alpha;
                    a        = -s(jj)/ds(jj);
                    alpha    = (a < alpha) && (a > 0) ? a : alpha;
                }

                if (ii)
                    break; // Don't need to compute any more

                // Centering
                Scalar mu_aff = (s + alpha*ds).dot(z + alpha*dz)*ms;
                Scalar sigma = mu_aff / mu;
                sigma *= sigma*sigma;

                // Corrector residual
                rs.array() += ds.array()*dz.array() - sigma*mu;
            }

            // Step
            alpha *= eta; // rescale step size
            x += alpha*dx;
            s += alpha*ds;
            z += alpha*dz;

            // Update residuals
            rd = Q*x + c - A*z;
            rp = s + A.adjoint()*x - b;
            rs = (s.array()*z.array());

            mu = s.dot(z)*ms;

            // Convergence test
            if ( (mu < eps) &&
                 (rd.norm() < eps) &&
                 (rs.norm() < eps) )
            {
                break;
            }
        }
        x_out = x;
    }
    public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

#if 0
/**
 * General QPs with both equality and inequality constraints.
 *  This doesn't currently work.
 */
template<typename Scalar, int NVars=-1, int NEq=-1, int NIneq=-1>
class QPGenSolver
{
    // Static size for work matrix.
    static constexpr int NWork =
        ((NVars == -1) || (NEq == -1) || (NIneq==-1)) ? -1 : (NVars+NEq+2*NIneq);
    typedef Eigen::Matrix<Scalar,NVars,1> PVec;
    typedef Eigen::Matrix<Scalar,NIneq,1> DVec; // Dual (i.e., Lagrange multiplier) vector
    typedef Eigen::Matrix<Scalar,NEq,1> EVec;   // Dual (i.e., Lagrange multiplier) vector for equality

    typedef Eigen::Matrix<Scalar,NWork,NWork> WorkBuf;
private:

    // Problem size
    const int n;
    const int mi;
    const int me;

    // Work buffers
    DVec s;
    DVec z;
    EVec y;

    PVec rd;
    DVec rp;
    DVec rs;
    EVec ry;

    PVec dx;
    DVec ds;
    DVec dz;
    EVec dy;

    WorkBuf augSystem;
public:
    QPGenSolver(int n_vars=NVars, int n_const_eq=NEq, int n_const_ineq=NIneq)
        : n(n_vars),mi(n_const_ineq),me(n_const_eq),
          s(mi), z(mi), y(me), rd(n), rp(mi), rs(mi),
          ry(me), dx(n), ds(mi), dz(mi), dy(me),
          augSystem(2*mi+me+n,2*mi+me+n)
        {
        }

    ~QPGenSolver() {}

    void solve(Eigen::Matrix<Scalar,NVars,NVars> &Q, Eigen::Matrix<Scalar,NVars,1> &c,
              Eigen::Matrix<Scalar,NIneq,NVars> &A, Eigen::Matrix<Scalar,NIneq,1> &b,
              Eigen::Matrix<Scalar,NEq,NVars> &E, Eigen::Matrix<Scalar,NEq,1> &f,
              Eigen::Matrix<Scalar,NVars,1> &x)
    {

    }
};
#endif

template<typename Scalar, int NVars, int NIneq>
void quadprog(Eigen::Matrix<Scalar,NVars,NVars> &Q, Eigen::Matrix<Scalar,NVars,1> &c,
              Eigen::Matrix<Scalar,NIneq,NVars> &A, Eigen::Matrix<Scalar,NIneq,1> &b,
              Eigen::Matrix<Scalar,NVars,1> &x)
{
    QPIneqSolver<Scalar,NVars,NIneq> qp(c.size(),b.size());
    qp.solve(Q,c,A,b,x);
}

} // End namespace
#endif


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Siargey Kachanovich
 *
 *    Copyright (C) 2019 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

// workaround for the annoying boost message in boost 1.69
#define BOOST_PENDING_INTEGER_LOG2_HPP
#include <boost/integer/integer_log2.hpp>
// end workaround

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "function"
#include <boost/test/unit_test.hpp>
#include <gudhi/Unitary_tests_utils.h>

#include <gudhi/Functions/Function_Sm_in_Rd.h>
#include <gudhi/Functions/Function_affine_plane_in_Rd.h>
#include <gudhi/Functions/Constant_function.h>
#include <gudhi/Functions/Function_chair_in_R3.h>
#include <gudhi/Functions/Function_torus_in_R3.h>
#include <gudhi/Functions/Function_whitney_umbrella_in_R3.h>
#include <gudhi/Functions/Function_lemniscate_revolution_in_R3.h>
#include <gudhi/Functions/Function_iron_in_R3.h>
#include <gudhi/Functions/Function_moment_curve_in_Rd.h>
#include <gudhi/Functions/Embed_in_Rd.h>
#include <gudhi/Functions/Translate.h>
#include <gudhi/Functions/Linear_transformation.h>
#include <gudhi/Functions/Negation.h>
#include <gudhi/Functions/Cartesian_product.h>
#include <gudhi/Functions/PL_approximation.h>

#include <gudhi/Coxeter_triangulation.h>

#include <string>

#include <random>
#include <cstdlib>

using namespace Gudhi::coxeter_triangulation;

template <class Function>
void test_function(const Function& fun) {
  Eigen::VectorXd seed = fun.seed();
  Eigen::VectorXd res_seed = fun(fun.seed());
  BOOST_CHECK(seed.size() == (long int)fun.amb_d());
  BOOST_CHECK(res_seed.size() == (long int)fun.cod_d());
  for (std::size_t i = 0; i < fun.cod_d(); i++) GUDHI_TEST_FLOAT_EQUALITY_CHECK(res_seed(i), 0., 1e-10);
}

BOOST_AUTO_TEST_CASE(function) {
  {
    // the sphere testing part
    std::size_t m = 3, d = 5;
    Eigen::VectorXd center(d);
    center << 2, 1.5, -0.5, 4.5, -1;
    double radius = 5;
    typedef Function_Sm_in_Rd Function_sphere;
    Function_sphere fun_sphere(radius, m, d, center);
    test_function(fun_sphere);
  }
  {
    // the affine plane testing part
    std::size_t m = 0, d = 5;
    Eigen::MatrixXd normal_matrix = Eigen::MatrixXd::Zero(d, d - m);
    for (std::size_t i = 0; i < d - m; ++i) normal_matrix(i, i) = 1;
    typedef Function_affine_plane_in_Rd Function_plane;
    Function_plane fun_plane(normal_matrix);
    test_function(fun_plane);
  }
  {
    // the constant function testing part
    std::size_t k = 2, d = 5;
    auto x = Eigen::VectorXd::Constant(k, 1);
    Constant_function fun_const(d, k, x);
    Eigen::VectorXd res_zero = fun_const(Eigen::VectorXd::Zero(d));
    for (std::size_t i = 0; i < k; ++i) GUDHI_TEST_FLOAT_EQUALITY_CHECK(res_zero(i), x(i), 1e-10);
  }
  {
    // the chair function
    Function_chair_in_R3 fun_chair;
    test_function(fun_chair);
  }
  {
    // the torus function
    Function_torus_in_R3 fun_torus;
    test_function(fun_torus);
  }
  {
    // the whitney umbrella function
    Function_whitney_umbrella_in_R3 fun_umbrella;
    test_function(fun_umbrella);
  }
  {
    // the lemniscate revolution function
    Function_lemniscate_revolution_in_R3 fun_lemniscate;
    test_function(fun_lemniscate);
  }
  {
    // the iron function
    Function_iron_in_R3 fun_iron;
    test_function(fun_iron);
  }
  {
    Function_moment_curve_in_Rd fun_moment_curve(3, 5);
    test_function(fun_moment_curve);
  }
  {
    // function embedding
    Function_iron_in_R3 fun_iron;
    auto fun_embed = make_embedding(fun_iron, 5);
    test_function(fun_iron);

    // function translation
    Eigen::VectorXd off = Eigen::VectorXd::Random(5);
    auto fun_trans = translate(fun_embed, off);
    test_function(fun_trans);

    // function linear transformation
    Eigen::MatrixXd matrix = Eigen::MatrixXd::Random(5, 5);
    BOOST_CHECK(matrix.determinant() != 0.);
    auto fun_lin = make_linear_transformation(fun_trans, matrix);
    test_function(fun_lin);

    // function negative
    auto fun_neg = negation(fun_lin);
    test_function(fun_neg);

    // function product
    typedef Function_Sm_in_Rd Function_sphere;
    Function_sphere fun_sphere(1, 1);
    auto fun_prod = make_product_function(fun_sphere, fun_sphere, fun_sphere);
    test_function(fun_prod);

    // function PL approximation
    Coxeter_triangulation<> cox_tr(6);
    typedef Coxeter_triangulation<>::Vertex_handle Vertex_handle;
    auto fun_pl = make_pl_approximation(fun_prod, cox_tr);
    Vertex_handle v0 = Vertex_handle(cox_tr.dimension(), 0);
    Eigen::VectorXd x0 = cox_tr.cartesian_coordinates(v0);
    Eigen::VectorXd value0 = fun_prod(x0);
    Eigen::VectorXd pl_value0 = fun_pl(x0);
    for (std::size_t i = 0; i < fun_pl.cod_d(); i++) GUDHI_TEST_FLOAT_EQUALITY_CHECK(value0(i), pl_value0(i), 1e-10);
    Vertex_handle v1 = v0;
    v1[0] += 1;
    Eigen::VectorXd x1 = cox_tr.cartesian_coordinates(v1);
    Eigen::VectorXd value1 = fun_prod(x1);
    Eigen::VectorXd pl_value1 = fun_pl(x1);
    for (std::size_t i = 0; i < fun_pl.cod_d(); i++) GUDHI_TEST_FLOAT_EQUALITY_CHECK(value1(i), pl_value1(i), 1e-10);
    Eigen::VectorXd pl_value_mid = fun_pl(0.5 * x0 + 0.5 * x1);
    for (std::size_t i = 0; i < fun_pl.cod_d(); i++)
      GUDHI_TEST_FLOAT_EQUALITY_CHECK(0.5 * value0(i) + 0.5 * value1(i), pl_value_mid(i), 1e-10);
  }
}


/* A namespace that extends Eigen to contain other necessary functions
 *
 * Author        : Jonathan EDEN
 * Created       : 2016
 * Description   : A namepsace that extends the Eigen library
*/

#pragma once
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/SVD>

using namespace Eigen;
// This is a namespace to add some necessary functions into.
namespace EigenExtension{
  /**
   * Skew symmetric matrix for cross product computation
   * @param v 3d Vector
   * @return skew symmetric matrix
   */
  Matrix3f SkewSymmetric(Vector3f v);
  /**
   * Skew symmetric matrix for cross product computation
   * @param v 3d Vector
   * @return skew symmetric matrix
   */
  Matrix3d SkewSymmetric2(Vector3d v);
  // Get the spline coefficients.
  Matrix<float,2,1> GetLinearSplineCoefficients(float q_r_i, float q_r_ip1, double t);
  Matrix<float,4,1> GetCubicSplineCoefficients(float q_r_i, float q_d_r_i,
                                          float q_r_ip1, float q_d_r_ip1, double t);
  Matrix<float,6,1> GetQuinticSplineCoefficients(float q_r_i, float q_d_r_i, float q_dd_r_i,
                                          float q_r_ip1, float q_d_r_ip1, float q_dd_r_ip1, double t);
  // Interpolate the spline
  void LinearSplineInterpolate(float* q_r, float* q_d_r, float* q_dd_r, Matrix<float,2,1> a, double t);
  void CubicSplineInterpolate(float* q_r, float* q_d_r, float* q_dd_r, Matrix<float,4,1> a, double t);
  void QuinticSplineInterpolate(float* q_r, float* q_d_r, float* q_dd_r, Matrix<float,6,1> a, double t);
  // Compute the rotation matrix
  Matrix3f ComputeRotationMatrix(AngleAxisf a);
  // Compute the derivative of a rotation matrix
  Matrix3f ComputeRotationMatrixDeriv(AngleAxisf a, AngleAxisf a_d);
  // Compute the double derivative of a rotation matrix
  Matrix3f ComputeRotationMatrixDoubleDeriv(AngleAxisf a, AngleAxisf a_d, AngleAxisf a_dd);
  /**
   *  Pseudo inverse matrix
   * @param A
   * @return Pseudo inverse matrix
   */
  MatrixXf Pinv(MatrixXf A);
  /**
   *  Pseudo inverse matrix
   * @param A
   * @return Pseudo inverse matrix
   */
  MatrixXd Pinv(MatrixXd A);

  //MatrixXf LeftPseudoInverse(MatrixXf A);
  //MatrixXf RightMatrixPseudoInverse(MatrixXf A);
}


/* @copyright The code is licensed under the MIT License
 *            <https://opensource.org/licenses/MIT>,
 *            Copyright (c) 2020 Christian Eskil Vaugelade Berg
 * @author Christian Eskil Vaugelade Berg
*/
#pragma once

#include <cmath>
#include <Eigen/Dense>

namespace orient::detail {

template<typename Scalar>
std::pair<Scalar, Scalar> asinWD(Scalar x)
{
  return std::make_pair(std::asin(x), 1.0 / std::sqrt(1.0 - x*x));
}

template<typename Scalar>
std::pair<Scalar, Scalar> acosWD(Scalar x)
{
  return std::make_pair(std::acos(x), -1.0 / std::sqrt(1.0 - x*x));
}

template<typename Scalar>
std::tuple<Scalar, Scalar, Scalar> atan2WD(Scalar y, Scalar x)
{
  const auto norm = x*x + y*y;
  const auto Jy = x / norm;
  const auto Jx = - y / norm;
  return std::make_tuple(std::atan2(y, x), Jy, Jx);
}

template<typename Derived, typename Scalar=typename Eigen::DenseBase<Derived>::Scalar>
std::pair<Eigen::Matrix<Scalar,3,3>, Eigen::Matrix<Scalar, 9, 9>> transposeWD(Eigen::MatrixBase<Derived> const& M)
{
  Eigen::Matrix<Scalar, 9,9> J = Eigen::Matrix<Scalar, 9,9>::Identity();
  J.col(1).swap(J.col(3));
  J.col(2).swap(J.col(6));
  J.col(5).swap(J.col(7));
  return std::make_pair(M.transpose(), J);
}

template<typename Derived, typename Scalar=typename Eigen::DenseBase<Derived>::Scalar>
std::pair<Scalar, Eigen::Matrix<Scalar, 1, 9>> traceWD(Eigen::MatrixBase<Derived> const& M)
{
  Eigen::Matrix<Scalar, 1, 9> J;
  J << 1,0,0,0,1,0,0,0,1;
  return std::make_pair(M.trace(), J);
}

}


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file NaiveSO3Tests.cpp
/// \brief Unit tests for the naive implementation of the SO3 Lie Group math.
/// \details Unit tests for the various Lie Group functions will test both special cases,
///          and randomly generated cases.
///
/// \author Sean Anderson
//////////////////////////////////////////////////////////////////////////////////////////////

#include <gtest/gtest.h>

#include <math.h>
#include <iostream>
#include <iomanip>
#include <ios>

#include <Eigen/Dense>
#include <lgmath/CommonMath.hpp>
#include <lgmath/so3/Operations.hpp>

/////////////////////////////////////////////////////////////////////////////////////////////
///
/// UNIT TESTS OF SO(3) MATH
///
/////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General test of SO(3) hat function
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, test3x3HatFunction) {

  // Init
  std::vector<Eigen::Matrix<double,3,1> > trueVecs;
  std::vector<Eigen::Matrix<double,3,3> > trueMats;

  // Add vectors to be tested - we choose a few
  trueVecs.push_back(Eigen::Matrix<double,3,1>( 0.0,  0.0,  0.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>( 1.0,  2.0,  3.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(-1.0, -2.0, -3.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>::Random());

  // Get number of tests
  const unsigned numTests = trueVecs.size();

  // Setup truth matrices
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double,3,3> mat;
    mat <<               0.0,  -trueVecs.at(i)[2],   trueVecs.at(i)[1],
           trueVecs.at(i)[2],                 0.0,  -trueVecs.at(i)[0],
          -trueVecs.at(i)[1],   trueVecs.at(i)[0],                 0.0;
    trueMats.push_back(mat);
  }

  // Test the function
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double,3,3> testMat = lgmath::so3::hat(trueVecs.at(i));
    std::cout << "true: " << trueMats.at(i) << std::endl;
    std::cout << "func: " << testMat << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(trueMats.at(i), testMat, 1e-6));
  }

  // Test identity,  hat(v)^T = -hat(v)
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double,3,3> testMat = lgmath::so3::hat(trueVecs.at(i));
    EXPECT_TRUE(lgmath::common::nearEqual(testMat.transpose(), -testMat, 1e-6));
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Test special cases of exponential functions: vec2rot and rot2vec
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, testVec2RotSpecialCase) {

  // Init
  std::vector<Eigen::Matrix<double,3,1> > trueVecs;
  std::vector<Eigen::Matrix<double,3,3> > trueMats;
  Eigen::Matrix<double,3,3> temp;

  // Identity
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, 0.0, 0.0));
  trueMats.push_back(Eigen::Matrix<double,3,3>::Identity());

  // x-rot by PI
  trueVecs.push_back(Eigen::Matrix<double,3,1>(lgmath::constants::PI, 0.0, 0.0));
  temp <<  1.0,  0.0,  0.0,
           0.0, -1.0,  0.0,
           0.0,  0.0, -1.0;
  trueMats.push_back(temp);

  // y-rot by PI
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, lgmath::constants::PI, 0.0));
  temp << -1.0,  0.0,  0.0,
           0.0,  1.0,  0.0,
           0.0,  0.0, -1.0;
  trueMats.push_back(temp);

  // z-rot by PI
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, 0.0, lgmath::constants::PI));
  temp << -1.0,  0.0,  0.0,
           0.0, -1.0,  0.0,
           0.0,  0.0,  1.0;
  trueMats.push_back(temp);

  // x-rot by -PI
  trueVecs.push_back(Eigen::Matrix<double,3,1>(-lgmath::constants::PI, 0.0, 0.0));
  temp <<  1.0,  0.0,  0.0,
           0.0, -1.0,  0.0,
           0.0,  0.0, -1.0;
  trueMats.push_back(temp);

  // y-rot by -PI
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, -lgmath::constants::PI, 0.0));
  temp << -1.0,  0.0,  0.0,
           0.0,  1.0,  0.0,
           0.0,  0.0, -1.0;
  trueMats.push_back(temp);

  // z-rot by -PI
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, 0.0, -lgmath::constants::PI));
  temp << -1.0,  0.0,  0.0,
           0.0, -1.0,  0.0,
           0.0,  0.0,  1.0;
  trueMats.push_back(temp);

  // x-rot by PI/2
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.5*lgmath::constants::PI, 0.0, 0.0));
  temp <<  1.0,  0.0,  0.0,
           0.0,  0.0, -1.0,
           0.0,  1.0,  0.0;
  trueMats.push_back(temp);

  // y-rot by PI/2
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, 0.5*lgmath::constants::PI, 0.0));
  temp <<  0.0,  0.0,  1.0,
           0.0,  1.0,  0.0,
          -1.0,  0.0,  0.0;
  trueMats.push_back(temp);

  // z-rot by PI/2
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, 0.0, 0.5*lgmath::constants::PI));
  temp <<  0.0, -1.0,  0.0,
           1.0,  0.0,  0.0,
           0.0,  0.0,  1.0;
  trueMats.push_back(temp);

  // Get number of tests
  const unsigned numTests = trueVecs.size();

  // Test vec2rot
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double,3,3> testMat = lgmath::so3::vec2rot(trueVecs.at(i));
    std::cout << "true: " << trueMats.at(i) << std::endl;
    std::cout << "func: " << testMat << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(trueMats.at(i), testMat, 1e-6));
  }

  // Test rot2vec
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double,3,1> testVec = lgmath::so3::rot2vec(trueMats.at(i));
    std::cout << "true: " << trueVecs.at(i) << std::endl;
    std::cout << "func: " << testVec << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqualAxisAngle(trueVecs.at(i), testVec, 1e-6));
  }

}

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General test of exponential functions: vec2rot and rot2vec
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, CompareAnalyticalAndNumericVec2Rot) {

  // Add vectors to be tested
  std::vector<Eigen::Matrix<double,3,1> > trueVecs;
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, 0.0, 0.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(lgmath::constants::PI, 0.0, 0.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, lgmath::constants::PI, 0.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, 0.0, lgmath::constants::PI));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(-lgmath::constants::PI, 0.0, 0.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, -lgmath::constants::PI, 0.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, 0.0, -lgmath::constants::PI));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.5*lgmath::constants::PI, 0.0, 0.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, 0.5*lgmath::constants::PI, 0.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, 0.0, 0.5*lgmath::constants::PI));
  const unsigned numRand = 50;
  for (unsigned i = 0; i < numRand; i++) {
    trueVecs.push_back(Eigen::Matrix<double,3,1>::Random());
  }

  // Get number of tests
  const unsigned numTests = trueVecs.size();

  // Calc matrices
  std::vector<Eigen::Matrix<double,3,3> > analyticRots;
  for (unsigned i = 0; i < numTests; i++) {
    analyticRots.push_back(lgmath::so3::vec2rot(trueVecs.at(i)));
  }

  // Compare analytical and numeric result
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double,3,3> numericRot = lgmath::so3::vec2rot(trueVecs.at(i), 20);
    std::cout << "ana: " << analyticRots.at(i) << std::endl;
    std::cout << "num: " << numericRot << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(analyticRots.at(i), numericRot, 1e-6));
  }

  // Test identity, rot^T = rot^-1
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double,3,3> lhs = analyticRots.at(i).transpose();
    Eigen::Matrix<double,3,3> rhs = analyticRots.at(i).inverse();
    std::cout << "lhs: " << lhs << std::endl;
    std::cout << "rhs: " << rhs << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(lhs, rhs, 1e-6));
  }

  // Test rot2vec
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double,3,1> testVec = lgmath::so3::rot2vec(analyticRots.at(i));
    std::cout << "true: " << trueVecs.at(i) << std::endl;
    std::cout << "func: " << testVec << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqualAxisAngle(trueVecs.at(i), testVec, 1e-6));
  }

}

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General test of exponential jacobians: vec2jac and vec2jacinv
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, CompareAnalyticalJacobiansInversesAndNumericCounterparts) {

  // Add vectors to be tested
  std::vector<Eigen::Matrix<double,3,1> > trueVecs;
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, 0.0, 0.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(lgmath::constants::PI, 0.0, 0.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, lgmath::constants::PI, 0.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, 0.0, lgmath::constants::PI));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(-lgmath::constants::PI, 0.0, 0.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, -lgmath::constants::PI, 0.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, 0.0, -lgmath::constants::PI));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.5*lgmath::constants::PI, 0.0, 0.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, 0.5*lgmath::constants::PI, 0.0));
  trueVecs.push_back(Eigen::Matrix<double,3,1>(0.0, 0.0, 0.5*lgmath::constants::PI));
  const unsigned numRand = 50;
  for (unsigned i = 0; i < numRand; i++) {
    trueVecs.push_back(Eigen::Matrix<double,3,1>::Random());
  }

  // Get number of tests
  const unsigned numTests = trueVecs.size();

  // Calc analytical matrices
  std::vector<Eigen::Matrix<double,3,3> > analyticJacs;
  std::vector<Eigen::Matrix<double,3,3> > analyticJacInvs;
  for (unsigned i = 0; i < numTests; i++) {
    analyticJacs.push_back(lgmath::so3::vec2jac(trueVecs.at(i)));
    analyticJacInvs.push_back(lgmath::so3::vec2jacinv(trueVecs.at(i)));
  }

  // Compare inversed analytical and analytical inverse
  for (unsigned i = 0; i < numTests; i++) {
    std::cout << "ana: " << analyticJacs.at(i) << std::endl;
    std::cout << "num: " << analyticJacInvs.at(i) << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(analyticJacs.at(i).inverse(), analyticJacInvs.at(i), 1e-6));
  }

  // Compare analytical and 'numerical' jacobian
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double,3,3> numericJac = lgmath::so3::vec2jac(trueVecs.at(i), 20);
    std::cout << "ana: " << analyticJacs.at(i) << std::endl;
    std::cout << "num: " << numericJac << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(analyticJacs.at(i), numericJac, 1e-6));
  }

  // Compare analytical and 'numerical' jacobian inverses
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double,3,3> numericJac = lgmath::so3::vec2jacinv(trueVecs.at(i), 20);
    std::cout << "ana: " << analyticJacInvs.at(i) << std::endl;
    std::cout << "num: " << numericJac << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(analyticJacInvs.at(i), numericJac, 1e-6));
  }

  // Test identity, rot(v) = eye(3) + hat(v)*jac(v), through the 'alternate' vec2rot method
  for (unsigned i = 0; i < numTests; i++) {
    Eigen::Matrix<double,3,3> lhs = lgmath::so3::vec2rot(trueVecs.at(i));
    Eigen::Matrix<double,3,3> rhs, jac;
    // the following vec2rot call uses the identity: rot(v) = eye(3) + hat(v)*jac(v)
    lgmath::so3::vec2rot(trueVecs.at(i), &rhs, &jac);
    std::cout << "lhs: " << lhs << std::endl;
    std::cout << "rhs: " << rhs << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(lhs, rhs, 1e-6));
  }
}

int main(int argc, char** argv) {
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

#undef VERSION
#define VERSION 3

using namespace std;
using namespace mtl;

dense_vector<double> inline last_unit_vector(size_t n)
{
    dense_vector<double> v(n, 0.0);
    v[n-1]= 1;
    return v;
}

dense_vector<double> inline unit_vector(size_t k, size_t n)
{
    dense_vector<double> v(n, 0.0);
    v[k]= 1;
    return v;
}

#if VERSION == 1

dense2D<double> inline inverse_upper(dense2D<double> const& A)
{
    const size_t N= num_rows(A);
    assert(num_cols(A) == N); // Matrix must be square

    dense2D<double> Inv(N, N);

    for (size_t k= 0; k < N; ++k) {
	dense_vector<double> e_k(N);
	for (size_t i= 0; i < N; ++i)
	    if (i == k)
		e_k[i]= 1.0;
	    else
		e_k[i]= 0.0;

	dense_vector<double> res_k(N);
	res_k= upper_trisolve(A, e_k);

	for (size_t i= 0; i < N; ++i)
	    Inv[i][k]= res_k[i];
    }
    return Inv;
}

#elif VERSION == 2

dense2D<double> inverse_upper(dense2D<double> const& A)
{
    const size_t N= num_rows(A);
    assert(num_cols(A) == N); // Matrix must be square

    dense2D<double> Inv(N, N);

    for (size_t k= 0; k < N; ++k) {
	dense_vector<double> e_k(N);
	for (size_t i= 0; i < N; ++i)
	    e_k[i]= i == k ? 1.0 : 0.0;

	for (size_t i= 0; i < N; ++i)
	    Inv[i][k]= upper_trisolve(A, e_k)[i];
    }
    return Inv;
}

#elif VERSION == 3

dense2D<double> inverse_upper(dense2D<double> const& A)
{
    const size_t N= num_rows(A);
    assert(num_cols(A) == N); // Matrix must be square

    dense2D<double> Inv(N, N);
    Inv= 0;

    for (size_t k= 0; k < N; ++k) {
	irange r(0, k+1);
	Inv[r][k]= upper_trisolve(A[r][r], unit_vector(k, k+1));
    }
    return Inv;
}

#endif

dense2D<double> inline inverse_lower(dense2D<double> const& A)
{
    dense2D<double> T(trans(A));
    return dense2D<double>(trans(inverse_upper(T)));
}

dense2D<double> inline inverse(dense2D<double> const& A)
{
    assert(num_cols(A) == num_rows(A)); // Matrix must be square

    dense2D<double>          PLU(A);
    dense_vector<size_t>   Pv(num_rows(A));

    lu(PLU, Pv);
    dense2D<double>  I(num_rows(A), num_cols(A));
    I= 1;
    dense2D<double>  PU(upper(PLU)),
        PL(strict_lower(PLU) + I);

    return dense2D<double>(inverse_upper(PU) * inverse_lower(PL) * permutation(Pv));
}

int main(int, char**)
{
    const unsigned size= 3;
    typedef dense2D<double>      Matrix;
    Matrix   A(size, size);
    A=  4, 1, 2,
	1, 5, 3,
	2, 6, 9;

    cout << "A is:\n" << A;

    Matrix LU(A);
    mtl::dense_vector<size_t> Pv(size);
    lu(LU, Pv);

    Matrix P(permutation(Pv));
    cout << "Permutation vector is " << Pv << "\nPermutation matrix is\n" << P;

    cout << "Permuted A is \n" << Matrix(P * A);
    Matrix I(size, size); I= 1;
    //Matrix I(mat::identity(size, size)), L(I + strict_lower(LU)), U(upper(LU));
    Matrix L(I + strict_lower(LU)), U(upper(LU));

    Matrix UI(inverse_upper(U));
    cout << "inverse(U) [permuted] is:\n" << UI << "UI * U is:\n" << Matrix(UI * U);
    assert(one_norm(Matrix(UI * U - I)) < 0.1);

    Matrix LI(inverse_lower(L));
    cout << "inverse(L) [permuted] is:\n" << LI << "LI * L is:\n" << Matrix(LI * L);
    assert(one_norm(Matrix(LI * L - I)) < 0.1);

    Matrix AI(UI * LI * P);
    cout << "inverse(A) [UI * LI * P] is \n" << AI << "A * AI is\n" << Matrix(AI * A);
    assert(one_norm(Matrix(AI * A - I)) < 0.1);

    Matrix A_inverse(inverse(A));
    cout << "inverse(A) is \n" << A_inverse << "A * AI is\n" << Matrix(A_inverse * A);
    assert(one_norm(Matrix(A_inverse * A - I)) < 0.1);

    Matrix A_e(inv(A));
    cout << "inv(A) is \n" << A_e << "\n";

    return 0;
}


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

int main(int argc, char *argv[])
{
   // Initialize matrix for initializer list
   auto A = mat{{1.0,2.0,3.0},{4.0, 5.0,6.0},{7.0, 8.0, 9.0}};
   cout << A << endl;

   // Zero filled matrix and direct element
   // initialization
   auto B = mat(3,3, fill::ones);
   B(1,1) = 10;
   cout << B << endl;

   // Elementwise multiplication
   auto C = A % B;

   auto b = vec{6.0, 60.0, 24.0};

   auto x = solve(C,b);

   cout << x << endl;

   return 0;
}


#ifndef OCC_RAYCASTING_HPP_7K2XI8HT
#define OCC_RAYCASTING_HPP_7K2XI8HT

#include <Eigen/Core>

#include <pcl_util/point_types.hpp>

#include "scrollgrid/grid_types.hpp"
#include "scrollgrid/scrollgrid3.hpp"
#include "scrollgrid/dense_array3.hpp"

#include <math.h>

namespace ca
{

static const int32_t CA_SG_COMPLETELY_FREE = 0;
static const int32_t CA_SG_COMPLETELY_OCCUPIED = 255;  //250
static const int32_t CA_SG_BELIEF_UPDATE_POS = 20; // when hit
static const int32_t CA_SG_BELIEF_UPDATE_NEG = 10; // when pass-through  2

template<class TraceFunctor>
void occupancy_trace(const Vec3Ix& start_pos,
                     const Vec3Ix& end_pos,
                     const TraceFunctor& fun) {
  // beware: vec3ix are int64_t
  int x = start_pos[0],
      y = start_pos[1],
      z = start_pos[2];
  int dx = end_pos[0] - start_pos[0],
      dy = end_pos[1] - start_pos[1],
      dz = end_pos[2] - start_pos[2];
  int sx, sy, sz;
  //X
  if ( dx>0 ) {
    sx = 1;
  } else if ( dx<0 ) {
    sx = -1;
    dx = -dx;
  } else {
    sx = 0;
  }

  //Y
  if ( dy>0 ) {
    sy = 1;
  } else if ( dy<0 ) {
    sy = -1;
    dy = -dy;
  } else {
    sy = 0;
  }

  //Z
  if ( dz>0 ) {
    sz = 1;
  } else if ( dz<0 ) {
    sz = -1;
    dz = -dz;
  } else {
    sz = 0;
  }

  int ax = 2*dx,
      ay = 2*dy,
      az = 2*dz;

  if ( ( dy <= dx ) && ( dz <= dx ) ) {
    for (int decy=ay-dx, decz=az-dx;
         ;
         x+=sx, decy+=ay, decz+=az) {
      //SetP ( grid,x,y,z,end_pos, atMax, count);
      fun(x,y,z,false);
      //Bresenham step
      if ( x==end_pos[0] ) break;
      if ( decy>=0 ) {
        decy-=ax;
        y+=sy;
      }
      if ( decz>=0 ) {
        decz-=ax;
        z+=sz;
      }
    }
  } else if ( ( dx <= dy ) && ( dz <= dy ) ) {
    //dy>=dx,dy
    for (int decx=ax-dy,decz=az-dy;
         ;
         y+=sy,decx+=ax,decz+=az ) {
      // SetP ( grid,x,y,z,end_pos, atMax, count);
      fun(x,y,z,false);
      //Bresenham step
      if ( y==end_pos[1] ) break;
      if ( decx>=0 ) {
        decx-=ay;
        x+=sx;
      }
      if ( decz>=0 ) {
        decz-=ay;
        z+=sz;
      }
    }
  } else if ( ( dx <= dz ) && ( dy <= dz ) ) {
    //dy>=dx,dy
    for (int decx=ax-dz,decy=ay-dz;
         ;
         z+=sz,decx+=ax,decy+=ay ) {
      //SetP ( grid,x,y,z,end_pos, atMax, count);
      fun(x,y,z,false);
      //Bresenham step
      if ( z==end_pos[2] ) break;
      if ( decx>=0 ) {
        decx-=az;
        x+=sx;
      } if ( decy>=0 ) {
        decy-=az;
        y+=sy;
      }
    }
  }
  fun(x,y,z,true);
}

template<class TraceFunctor>
void occupancy_trace_dist(const Vec3Ix& start_pos,
                     const Vec3Ix& end_pos,
                     const TraceFunctor& fun,
		     double distance) {  //TODO add distance
  // beware: vec3ix are int64_t
  int x = start_pos[0],
      y = start_pos[1],
      z = start_pos[2];
  int dx = end_pos[0] - start_pos[0],
      dy = end_pos[1] - start_pos[1],
      dz = end_pos[2] - start_pos[2];
  int sx, sy, sz;
  //X
  if ( dx>0 ) {
    sx = 1;
  } else if ( dx<0 ) {
    sx = -1;
    dx = -dx;
  } else {
    sx = 0;
  }

  //Y
  if ( dy>0 ) {
    sy = 1;
  } else if ( dy<0 ) {
    sy = -1;
    dy = -dy;
  } else {
    sy = 0;
  }

  //Z
  if ( dz>0 ) {
    sz = 1;
  } else if ( dz<0 ) {
    sz = -1;
    dz = -dz;
  } else {
    sz = 0;
  }

  int ax = 2*dx,
      ay = 2*dy,
      az = 2*dz;

  if ( ( dy <= dx ) && ( dz <= dx ) ) {
    for (int decy=ay-dx, decz=az-dx;
         ;
         x+=sx, decy+=ay, decz+=az) {
      //SetP ( grid,x,y,z,end_pos, atMax, count);
//       fun(x,y,z,false,distance);
      distance=sqrt((x-start_pos[0])*(x-start_pos[0])+(y-start_pos[1])*(y-start_pos[1])+(z-start_pos[2])*(z-start_pos[2]));
      fun(x,y,z,false,distance);
      //Bresenham step
      if ( x==end_pos[0] ) break;
      if ( decy>=0 ) {
        decy-=ax;
        y+=sy;
      }
      if ( decz>=0 ) {
        decz-=ax;
        z+=sz;
      }
    }
  } else if ( ( dx <= dy ) && ( dz <= dy ) ) {
    //dy>=dx,dy
    for (int decx=ax-dy,decz=az-dy;
         ;
         y+=sy,decx+=ax,decz+=az ) {
      // SetP ( grid,x,y,z,end_pos, atMax, count);
//       fun(x,y,z,false,distance);
      distance=sqrt((x-start_pos[0])*(x-start_pos[0])+(y-start_pos[1])*(y-start_pos[1])+(z-start_pos[2])*(z-start_pos[2]));
      fun(x,y,z,false,distance);
      //Bresenham step
      if ( y==end_pos[1] ) break;
      if ( decx>=0 ) {
        decx-=ay;
        x+=sx;
      }
      if ( decz>=0 ) {
        decz-=ay;
        z+=sz;
      }
    }
  } else if ( ( dx <= dz ) && ( dy <= dz ) ) {
    //dy>=dx,dy
    for (int decx=ax-dz,decy=ay-dz;
         ;
         z+=sz,decx+=ax,decy+=ay ) {
      //SetP ( grid,x,y,z,end_pos, atMax, count);
//       fun(x,y,z,false,distance);
      distance=sqrt((x-start_pos[0])*(x-start_pos[0])+(y-start_pos[1])*(y-start_pos[1])+(z-start_pos[2])*(z-start_pos[2]));
      fun(x,y,z,false,distance);
      //Bresenham step
      if ( z==end_pos[2] ) break;
      if ( decx>=0 ) {
        decx-=az;
        x+=sx;
      } if ( decy>=0 ) {
        decy-=az;
        y+=sy;
      }
    }
  }
//       fun(x,y,z,true,distance);
      distance=sqrt((x-start_pos[0])*(x-start_pos[0])+(y-start_pos[1])*(y-start_pos[1])+(z-start_pos[2])*(z-start_pos[2]));
      fun(x,y,z,true,distance);
}
/**
 * Update occupancy information along ray.
 */
template<class GridScalar>
void occupancy_trace_simple(const Vec3Ix& start_pos, // in ijk
                            const Vec3Ix& end_pos, // in ijk
                            const ca::ScrollGrid3<GridScalar>& grid3,
                            ca::DenseArray3<uint8_t>& array3)
                            {
  //int ray_ctr = 0;
  // beware: vec3ix are int64_t
  int x = start_pos[0],
      y = start_pos[1],
      z = start_pos[2];
  int dx = end_pos[0] - start_pos[0],
      dy = end_pos[1] - start_pos[1],
      dz = end_pos[2] - start_pos[2];
  int sx, sy, sz;
  //X
  if ( dx>0 ) {
    sx = 1;
  } else if ( dx<0 ) {
    sx = -1;
    dx = -dx;
  } else {
    sx = 0;
  }

  //Y
  if ( dy>0 ) {
    sy = 1;
  } else if ( dy<0 ) {
    sy = -1;
    dy = -dy;
  } else {
    sy = 0;
  }

  //Z
  if ( dz>0 ) {
    sz = 1;
  } else if ( dz<0 ) {
    sz = -1;
    dz = -dz;
  } else {
    sz = 0;
  }

  int ax = 2*dx,
      ay = 2*dy,
      az = 2*dz;

  if ( ( dy <= dx ) && ( dz <= dx ) ) {
    // this is tracing loop. we step through the ray in integer coordinates
    // until we reach the end
    for (int decy=ay-dx, decz=az-dx;
         ;
         x+=sx, decy+=ay, decz+=az) {

      // here we are in the middle of tracing the
      // ray. here we are passing through.
      mem_ix_t mem_ix = grid3.grid_to_mem(x, y, z);

      // by definition here we are passing through
      // TODO correct & efficent boundary check
      int32_t new_value = static_cast<int32_t>(array3[mem_ix])-CA_SG_BELIEF_UPDATE_NEG;
      array3[mem_ix] = static_cast<uint8_t>(std::max(CA_SG_COMPLETELY_FREE, new_value));

      //Bresenham step
      if ( x==end_pos[0] ) break;
      if ( decy>=0 ) {
        decy-=ax;
        y+=sy;
      }
      if ( decz>=0 ) {
        decz-=ax;
        z+=sz;
      }
    }
  } else if ( ( dx <= dy ) && ( dz <= dy ) ) {
    //dy>=dx,dy
    for (int decx=ax-dy,decz=az-dy;
         ;
         y+=sy,decx+=ax,decz+=az ) {

      mem_ix_t mem_ix = grid3.grid_to_mem(x, y, z);
      int32_t new_value = static_cast<int32_t>(array3[mem_ix])-CA_SG_BELIEF_UPDATE_NEG;
      array3[mem_ix] = static_cast<uint8_t>(std::max(CA_SG_COMPLETELY_FREE, new_value));

      //Bresenham step
      if ( y==end_pos[1] ) break;
      if ( decx>=0 ) {
        decx-=ay;
        x+=sx;
      }
      if ( decz>=0 ) {
        decz-=ay;
        z+=sz;
      }
    }
  } else if ( ( dx <= dz ) && ( dy <= dz ) ) {
    //dy>=dx,dy
    for (int decx=ax-dz,decy=ay-dz;
         ;
         z+=sz,decx+=ax,decy+=ay ) {

      mem_ix_t mem_ix = grid3.grid_to_mem(x, y, z);
      int32_t new_value = static_cast<int32_t>(array3[mem_ix])-CA_SG_BELIEF_UPDATE_NEG;
      array3[mem_ix] = static_cast<uint8_t>(std::max(CA_SG_COMPLETELY_FREE, new_value));

      //array3[mem_ix] = ray_ctr++;
      //Bresenham step
      if ( z==end_pos[2] ) break;
      if ( decx>=0 ) {
        decx-=az;
        x+=sx;
      } if ( decy>=0 ) {
        decy-=az;
        y+=sy;
      }
    }
  }

  // here we are at the end of the ray.
  // here we update according to xyz

  mem_ix_t mem_ix = grid3.grid_to_mem(x, y, z);
  int32_t new_value = static_cast<int32_t>(array3[mem_ix])+CA_SG_BELIEF_UPDATE_POS;
  array3[mem_ix] = static_cast<uint8_t>(std::min(CA_SG_COMPLETELY_OCCUPIED, new_value));

}
} /* ca
 */

#endif /* end of include guard: OCC_RAYCASTING_HPP_7K2XI8HT */


#define MIN(a,b) a < b ? a : b
#define MAX(a,b) a > b ? a : b

#include <mpi.h>
#include <cassert>
#include <vector>
#include <random>
#include <iostream>
#include <boost/mpi/datatype.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

using namespace boost::numeric::ublas;

template <typename T>
matrix<T>* gen_matrix(size_t rows = 1024, size_t cols = 1024,int min = 0, int max = 3){
  std::random_device seeder;
  boost::random::mt19937 gen(seeder());
  boost::random::uniform_int_distribution<> dist(min, max);

  matrix<T>* m = new matrix<T> (rows, cols);
  for (unsigned i = 0; i < m->size1 (); ++ i)
    for (unsigned j = 0; j < m->size2 (); ++ j)
        m->at_element(i, j) = dist(gen);;
  return m;
}

template <typename T>
void vm_mul_row_wise(std::vector<T>* sub_vectors, matrix<T>* m, std::vector<T>* sub_out){
  const int m_rows = m->size1();
  const int m_cols = m->size2();
  const int v_cols = m->size1();
  const int v_rows = sub_vectors->size() / v_cols;

  for(int i=0; i < v_rows; i++){
    for(int j=0; j < m_cols; j++){
      T temp = 0;
      for(int k=0; k < m_rows; k++){
        temp += sub_vectors->at(i * v_cols + k) * m->at_element(k, j);
      }
      sub_out->at(i*v_cols + j) = temp;
    }
  }
}

template <typename T>
bool check_equality(matrix<T>* m1, matrix<T>* m2){
  for(int i=0; i < m1->size1(); i++){
    for(int j=0; j < m1->size2(); j++){
      if(m1->at_element(i,j) != m2->at_element(i,j)){
      	return false;
      }
    }
  }
  return true;
}

void matrix_mul(const int rows=1024, const int cols=1024){

  int world_rank;
  MPI_Comm_rank(MPI_COMM_WORLD, &world_rank);
  int world_size;
  MPI_Comm_size(MPI_COMM_WORLD, &world_size);

  auto mpi_type = MPI_INT;
  const int root = 0;

  matrix<int>* m1 = NULL;
  matrix<int>* m2 = NULL;
  // matrix<int> m_origin_out;
  if (world_rank == root) {
    m1 = gen_matrix<int>(rows, cols);
    m2 = gen_matrix<int>(cols, rows);
    // m_origin_out = matrix<int>(prod(*m1, *m2));
  }else{
    m2 = new matrix<int>(rows, cols);
    // m_origin_out = matrix<int>(rows, cols);
  }
  MPI_Bcast(m2->data().begin(), rows * cols, mpi_type, root, MPI_COMM_WORLD);
  // MPI_Bcast(m_origin_out.data().begin(), rows * rows, mpi_type, root, MPI_COMM_WORLD);

  if( rows % world_size != 0){
    MPI_Abort(MPI_COMM_WORLD, -1);
  }
  int recv_count =  rows / world_size * cols;

  std::vector<int>* sub_vectors = new std::vector<int>(recv_count);
  assert(sub_vectors != NULL);

  if(world_rank == root){
    MPI_Scatter( m1->data().begin(), recv_count, mpi_type, sub_vectors->data(), recv_count, mpi_type, root, MPI_COMM_WORLD);
  } else{
    MPI_Scatter( NULL, recv_count, mpi_type, sub_vectors->data(), recv_count, mpi_type, root, MPI_COMM_WORLD);
  }

  // v_rows * cols ,  cols * rows,  v_rows * rows
  std::vector<int>* sub_out = new std::vector<int>(recv_count);
  matrix<int>* m_out = new matrix<int>(rows, rows);
  assert(m_out != NULL);

  vm_mul_row_wise<int>(sub_vectors, m2, sub_out);

  MPI_Allgather(sub_out->data(), recv_count, mpi_type, m_out->data().begin(), recv_count, mpi_type, MPI_COMM_WORLD);

  std::cout << *m_out << std::endl;
  // auto is_same = check_equality(m_out, &m_origin_out);
  // std::cout <<  "Rank:" << world_rank  << ",The two results are same:" << is_same << std::endl;
  // MPI_Barrier(MPI_COMM_WORLD);

  delete sub_out;
  delete m_out;
  delete m2;
  if (world_rank == root) {
    delete m1;
  }
}

template <typename T>
int get_max(vector<T>* v){
  T max = v->at(0);
  for(int i=0; i<v->size(); i++){
    max = (max < v->at(i)) ? v->at(i) : max;
  }
  return max;
}

// (W−F+2P)/S+1
// here just implement max pooling
void matrix_pool(const int rows=1024, const int cols=1024, const int k_h = 4, const int k_w = 4,
		const int p_h = 0, const int p_w = 0, const int stride_h = 4, const int stride_w = 4){

  int world_rank;
  MPI_Comm_rank(MPI_COMM_WORLD, &world_rank);
  int world_size;
  MPI_Comm_size(MPI_COMM_WORLD, &world_size);

  auto mpi_type = MPI_INT;
  const int root = 0;

  matrix<int>* M = NULL;
  if( world_rank == root){
    M = gen_matrix<int>(rows, cols);
  }else{
    M = new matrix<int>(rows, cols);
  }
  MPI_Bcast(M->data().begin(), rows*cols, mpi_type, root, MPI_COMM_WORLD);

  if( rows % k_h != 0 || cols % k_w != 0){
    MPI_Abort(MPI_COMM_WORLD, -1);
  }

  const int new_rows = (rows - k_h + 2 * p_h) / stride_h + 1;
  const int new_cols = (cols - k_w + 2 * p_w) / stride_w + 1;

  matrix<int>* N = new matrix<int>(new_rows, new_cols);
  int num_elements_per_proc = new_rows * new_cols / world_size;
  std::vector<int>* sub_vec = new std::vector<int>(num_elements_per_proc);
  MPI_Scatter(N->data().begin(), num_elements_per_proc, mpi_type, sub_vec->data(), num_elements_per_proc, mpi_type, root, MPI_COMM_WORLD);

  for(int i=0; i < num_elements_per_proc; i++){
    int pool_index = i*world_size + world_rank;

    int nr = pool_index / new_cols;
    int nc = pool_index % new_cols;

    int hstart = nr * stride_h - p_h;
    int wstart = nc * stride_w - p_w;
    int hend = MIN(hstart + k_h, rows);
    int wend = MIN(wstart + k_w, cols);
    hstart = MAX(hstart, 0);
    wstart = MAX(wstart, 0);

    int temp_max = INT_MIN;
    for(int h=hstart; h < hend; h++){
      for(int w=wstart; w < wend; w++){
        int index = h * cols + w;
        int bottom_data = M->at_element(h, w);
        if(bottom_data > temp_max ){
          temp_max = bottom_data;
        }
      }
    }
    sub_vec->at(i) = temp_max;
  }

  MPI_Allgather(sub_vec->data(), num_elements_per_proc, mpi_type, N->data().begin(), num_elements_per_proc,mpi_type, MPI_COMM_WORLD);
  std::cout<< *M << std::endl;
  std::cout<< *N << std::endl;
}

// (W−F+2P)/S+1
// just implement naive conv
// TODO: im2col
void matrix_conv(const int rows=1024, const int cols=1024, const int k_h = 4, const int k_w = 4,
		const int p_h = 0, const int p_w = 0, const int stride_h = 4, const int stride_w = 4){

  int world_rank;
  MPI_Comm_rank(MPI_COMM_WORLD, &world_rank);
  int world_size;
  MPI_Comm_size(MPI_COMM_WORLD, &world_size);

  auto mpi_type = MPI_INT;
  const int root = 0;

  matrix<int>* M = NULL;
  matrix<int>* K = NULL;
  if( world_rank == root){
    M = gen_matrix<int>(rows, cols);
    K = gen_matrix<int>(k_h, k_w);
  }else{
    M = new matrix<int>(rows, cols);
    K = new matrix<int>(k_h, k_w);
  }
  MPI_Bcast(M->data().begin(), rows*cols, mpi_type, root, MPI_COMM_WORLD);
  MPI_Bcast(K->data().begin(), k_h * k_w, mpi_type, root, MPI_COMM_WORLD);

  if( rows % k_h != 0 || cols % k_w != 0){
    MPI_Abort(MPI_COMM_WORLD, -1);
  }

  const int new_rows = (rows - k_h + 2 * p_h) / stride_h + 1;
  const int new_cols = (cols - k_w + 2 * p_w) / stride_w + 1;

  matrix<int>* N = new matrix<int>(new_rows, new_cols);
  int num_elements_per_proc = new_rows * new_cols / world_size;
  std::vector<int>* sub_vec = new std::vector<int>(num_elements_per_proc);
  MPI_Scatter(N->data().begin(), num_elements_per_proc, mpi_type, sub_vec->data(), num_elements_per_proc, mpi_type, root, MPI_COMM_WORLD);

  for(int i=0; i < num_elements_per_proc; i++){
    int pool_index = i*world_size + world_rank;

    int nr = pool_index / new_cols;
    int nc = pool_index % new_cols;

    int hstart = nr * stride_h - p_h;
    int wstart = nc * stride_w - p_w;
    int hend = MIN(hstart + k_h, rows);
    int wend = MIN(wstart + k_w, cols);
    hstart = MAX(hstart, 0);
    wstart = MAX(wstart, 0);

    int temp_prod = 0;
    for(int h=hstart; h < hend; h++){
      for(int w=wstart; w < wend; w++){
        int index = h * cols + w;
        int bottom_data = M->at_element(h, w);
        int weight = K->at_element(h-hstart, w-wstart);
        temp_prod += bottom_data * weight;
      }
    }
    sub_vec->at(i) = temp_prod;
  }

  MPI_Allgather(sub_vec->data(), num_elements_per_proc, mpi_type, N->data().begin(), num_elements_per_proc,mpi_type, MPI_COMM_WORLD);
  std::cout<< *M << std::endl;
  std::cout<< *K << std::endl;
  std::cout<< *N << std::endl;

}

int main(int argc, char** argv) {

  MPI_Init(NULL, NULL);

  matrix_mul(1024, 1024);
  matrix_pool(1024,1024);
  matrix_conv(1024, 1024);

  MPI_Finalize();
}


#include "QuarticPolynomial.h"

#include <Eigen/LU>
#include <cmath>

using namespace Eigen;

QuarticPolynomial::QuarticPolynomial(double xs, double vxs, double axs,
        double vxe, double axe, double t):
        a0(xs), a1(vxs) {
    a2 = axs / 2.0;
    Matrix2d A;
    Vector2d B;
    A << 3 * pow(t, 2), 4 * pow(t, 3), 6 * t, 12 * pow(t, 2);
    B << vxe - a1 - 2 * a2 * t, axe - 2 * a2;
    Matrix2d A_inv = A.inverse();
    Vector2d x = A_inv * B;
    a3 = x[0];
    a4 = x[1];
}

double QuarticPolynomial::calc_point(double t) {
    return a0 + a1 * t + a2 * pow(t, 2) + a3 * pow(t, 3) + a4 * pow(t, 4);
}

double QuarticPolynomial::calc_first_derivative(double t) {
    return a1 + 2 * a2 * t + 3 * a3 * pow(t, 2) + 4 * a4 * pow(t, 3);
}

double QuarticPolynomial::calc_second_derivative(double t) {
    return 2 * a2 + 6 * a3 * t + 12 * a4 * pow(t, 2);
}

double QuarticPolynomial::calc_third_derivative(double t) {
    return 6 * a3 + 24 * a4 * t;
}


#include "PoseEstimator.h"
#include "PoseOptimizer.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>

#include <opencv2/core/eigen.hpp>

namespace MVSO
{
	void solveICP(std::vector<cv::Point3f>& points3D_t0, std::vector<cv::Point3f>& points3D_t1, cv::Mat& R, cv::Mat& t)
	{
		cv::Point3f p1, p2;     // center of mass
		int N = points3D_t0.size();
		for (int i = 0; i < N; i++)
		{
			p1 += points3D_t0[i];
			p2 += points3D_t1[i];
		}
		p1 = cv::Point3f(cv::Vec3f(p1) / N);
		p2 = cv::Point3f(cv::Vec3f(p2) / N);
		std::vector<cv::Point3f>     q1(N), q2(N); // remove the center
		for (int i = 0; i < N; i++)
		{
			q1[i] = points3D_t0[i] - p1;
			q2[i] = points3D_t1[i] - p2;
		}

		// compute q1*q2^T
		Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
		for (int i = 0; i < N; i++)
		{
			W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
		}

		// SVD on W
		Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
		Eigen::Matrix3d U = svd.matrixU();
		Eigen::Matrix3d V = svd.matrixV();

		if (U.determinant() * V.determinant() < 0)
		{
			for (int x = 0; x < 3; ++x)
			{
				U(x, 2) *= -1;
			}
		}

		Eigen::Matrix3d R_ = U * (V.transpose());
		Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

		cv::eigen2cv(R_, R);
		cv::eigen2cv(t_, t);
	}

	void solveICPRansac(std::vector<cv::Point3f>& pts1, std::vector<cv::Point3f>& pts2,
		cv::Mat& R, cv::Mat& t,std::vector<int>& inliers,
		int iterationCount, float reprojectionError)
	{
		cv::Mat bestR = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
		cv::Mat bestT = (cv::Mat_<double>(3, 1) << 0, 0, 0);
		std::vector<int> bestInliers;
		cv::Mat ptsMat1(pts1);
		cv::Mat ptsMat2(pts2);
		for (int it = 0; it < iterationCount; it++)
		{
			std::vector<cv::Point3f> subset1;
			std::vector<cv::Point3f> subset2;
			for (int i = 0; i < 5; i++)
			{
				int id = rand() % pts1.size();

				subset1.push_back(pts1[id]);
				subset2.push_back(pts2[id]);
			}

			cv::Mat tmpR, tmpT;
			solveICP(subset1, subset2, tmpR, tmpT);

			std::vector<int> inliers;
			for (int i = 0; i < pts1.size(); i++)
			{
				cv::Point3f& p1 = pts1[i];
				cv::Point3f& p2 = pts2[i];
				cv::Point3f ep2;

				ep2.x = tmpR.at<double>(0, 0) * p1.x + tmpR.at<double>(0, 1) * p1.y + tmpR.at<double>(0, 2) * p1.z + tmpT.at<double>(0);
				ep2.y = tmpR.at<double>(1, 0) * p1.x + tmpR.at<double>(1, 1) * p1.y + tmpR.at<double>(1, 2) * p1.z + tmpT.at<double>(1);
				ep2.z = tmpR.at<double>(2, 0) * p1.x + tmpR.at<double>(2, 1) * p1.y + tmpR.at<double>(2, 2) * p1.z + tmpT.at<double>(2);

				float err = sqrt((p2.x - ep2.x)*(p2.x - ep2.x) + (p2.y - ep2.y)*(p2.y - ep2.y) + (p2.z - ep2.z));
				if (err < reprojectionError)
				{
					inliers.push_back(i);
				}
			}
			if (inliers.size() > bestInliers.size())
			{
				bestInliers = inliers;
				bestR = tmpR;
				bestT = tmpT;
			}

		}
		R = bestR;
		t = bestT;
		inliers = bestInliers;
	}

	MVSO::PoseEstimator::PoseEstimator(CameraModel & camera): camera_(camera)
	{
	}

	cv::Mat PoseEstimator::estimatePose(std::vector<cv::Point2f>& pointsLeft_t0, std::vector<cv::Point2f>& pointsLeft_t1, std::vector<cv::Point3f>& points3D_t0)
	{
		// Calculate frame to frame transformation
		cv::Mat pose;
		cv::Mat rotation, translation;

		// -----------------------------------------------------------
		// Rotation(R) estimation using Nister's Five Points Algorithm
		// -----------------------------------------------------------
		double focal = camera_.fx_;
		cv::Point2d principle_point(camera_.cx_, camera_.cy_);

		//recovering the pose and the essential cv::matrix
		cv::Mat E, mask;
		cv::Mat translation_mono = cv::Mat::zeros(3, 1, CV_64F);
		E = cv::findEssentialMat(pointsLeft_t1, pointsLeft_t0, focal, principle_point, cv::RANSAC, 0.999, 1.0, mask);
		cv::recoverPose(E, pointsLeft_t1, pointsLeft_t0, rotation, translation_mono, focal, principle_point, mask);
		// std::cout << "recoverPose rotation: " << rotation << std::endl;

		// ------------------------------------------------
		// Translation (t) estimation by use solvePnPRansac
		// ------------------------------------------------
		cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);
		cv::Mat inliers;
		cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);

		int iterationsCount = 100;        // number of Ransac iterations.
		float reprojectionError = 1.0;    // maximum allowed distance to consider it an inlier.
		float confidence = 0.98;          // RANSAC successful confidence.
		bool useExtrinsicGuess = false;
		int flags = cv::SOLVEPNP_EPNP;

		//cv::Rodrigues(rotation, rvec);
		cv::solvePnPRansac(points3D_t0, pointsLeft_t1, camera_.intrinsicMat_, distCoeffs, rvec, translation,
			useExtrinsicGuess, iterationsCount, reprojectionError, confidence,
			inliers, flags);

		std::vector<cv::Point3f> points3d;
		std::vector<cv::Point2f> points2d;
		std::vector<double> weights;

		for (int i = 0; i < inliers.rows; i++)
		{
			int id = inliers.at<int>(i, 0);

			cv::Point3f p3d = points3D_t0[id];
			cv::Point2f p0 = pointsLeft_t0[id], p1 = pointsLeft_t1[id];
			points3d.push_back(p3d);
			points2d.push_back(p1);

			double w = sqrt((p0.x - p1.x)*(p0.x - p1.x) + (p0.y - p1.y)*(p0.y - p1.y));
			//w *= 1/(1+exp(abs(p3d.z - 5)));
			if (w > 7)
				w = 7;
			else if (w < 1.0)
				w = 1.0;
			weights.push_back(exp(-w));
		}

		PoseOptimizer optimizer(camera_);
		//cv::Rodrigues(rvec, rotation);
		//optimizer.optimizePose(points3d, points2d, rotation, translation);
		optimizer.optimizePose(points3d, points2d, weights,rotation, translation);

		//cv::Rodrigues(rvec, rotation);
		rotation = rotation.t();
		translation = -translation;
		// std::cout << "inliers size: " << inliers.size() << std::endl;
		cv::hconcat(rotation, translation, pose);
		return pose;
	}

	cv::Mat PoseEstimator::estimatePose(std::vector<cv::Point2f>& points_t0, std::vector<cv::Point2f>& points_t1, std::vector<cv::Point3f>& points3D_t0, std::vector<cv::Point3f>& points3D_t1)
	{

		cv::Mat R, t;
		std::vector<int> inliers;
		solveICPRansac(points3D_t0, points3D_t1, R, t, inliers, 100, 1.0);

		std::vector<cv::Point3f> pts1, pts2;
		for (int n : inliers)
		{
			pts1.push_back(points3D_t0[n]);
			pts2.push_back(points3D_t1[n]);
		}

		PoseOptimizer optimizer(camera_);
		optimizer.optimizePose(pts1, pts2, R, t);
		cv::Mat pose;
		cv::hconcat(R, t, pose);
		return pose;

	}

	PoseEstimator::~PoseEstimator()
	{
	}

}


#include <Eigen/Eigen>
#include <iostream>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix3i m = Matrix3i::Random();
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Here is the upper-triangular matrix extracted from m:" << endl
     << Matrix3i(m.triangularView<Eigen::Upper>()) << endl;
cout << "Here is the strictly-upper-triangular matrix extracted from m:" << endl
     << Matrix3i(m.triangularView<Eigen::StrictlyUpper>()) << endl;
cout << "Here is the unit-lower-triangular matrix extracted from m:" << endl
     << Matrix3i(m.triangularView<Eigen::UnitLower>()) << endl;
// FIXME need to implement output for triangularViews (Bug 885)

  return 0;
}


#ifndef SPLINE_HPP
#define SPLINE_HPP

#include <array>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseQR>

using namespace std;
using namespace Eigen;

/**
 * A 1D polynomial of type f(t) = a + b t + c t^2 + d t^3 + ... + z t^(order)
 * that is bounded to the range t in [0; 1].
 */
template<unsigned int Order>
struct UnitBoundedPolynomial1
{
    static_assert(Order % 2 == 1, "Order must be odd");
    static_assert(Order > 0);

    /**
     * The number of derivates (including 0-th derivative)
     * required to fit this polynomial to start and end point
     */
    static constexpr const int RequiredValues = (Order + 1) / 2;

    double coeffs[Order+1];

    /**
     * Returns point at t on polynomial
     * @param  t in [0;1]
     * @return   double
     */
    double interpolate(double t) const;

    /**
     * Returns first derivative of polynomial at t
     * @param  t in [0;1]
     * @return   double
     */
    double derivative(double t) const;

    /**
     * Returns second derivative of polynomial at t
     * @param  t in [0;1]
     * @return   double
     */
    double derivative2(double t) const;

    /**
     * Fits the polynomial to go through start and end point
     * with specified derivatives.
     */
    static UnitBoundedPolynomial1<Order> fit(const Matrix<double, RequiredValues, 1> a, const Matrix<double, RequiredValues, 1> b);
};

/**
 * A multi dimensional polynomial function. Aggregate of multiple independent 1-dimensional polynomials.
 */
template<unsigned int Order, unsigned int Dims>
struct UnitBoundedPolynomial
{
    static_assert(Order > 0);
    static_assert(Dims > 0);

    typedef Matrix<double, Dims, 1> VectorNd;
    typedef UnitBoundedPolynomial1<Order> Poly1;

    UnitBoundedPolynomial1<Order> _dims[Dims];
    double length;

    VectorNd interpolate(double t) const;
    VectorNd derivative(double t) const;

    /**
     * Walks on the polynomial. Calls fn many times with points on the function.
     * Resolution is specified using deltatau.
     * @param deltatau  Resolution with which to walk
     * @param fn        Callback function
     * @param payload   Gets passed to callback
     * @param a         Optional, start point, can be used for extrapolation
     * @param b         Optional, end point, can be used for extrapolation
     */
    void walk(const double deltatau, void (*fn)(VectorNd, double, const UnitBoundedPolynomial<Order, Dims>&, void *), void *payload = nullptr, double a = 0.0, double b = 1.0) const;

    /**
     * Calculates a spline.
     * @param  A Matrix of point a with A[:, 0] = point, A[:, 1] = 1st derivative, A[:, 2] = 2nd derivative
     * @param  B Matrix of point b with B[:, 0] = point, B[:, 1] = 1st derivative, B[:, 2] = 2nd derivative
     * @return   Spline
     */
    static UnitBoundedPolynomial<Order, Dims> fit(Matrix<double, Dims, Poly1::RequiredValues> A,
                                                  Matrix<double, Dims, Poly1::RequiredValues> B);

private:
    void calculateLength();
};

template<unsigned int Order, unsigned int Dims>
class SplineSolver;

/**
 * A hermite spline.
 * Order and dimensionality can vary.
 * Currently only quintic and cubic splines are supported.
 */
template<unsigned int Order, unsigned int Dims>
struct HermiteSpline
{
    static_assert(Order > 0);
    static_assert(Dims > 0);

    using Solver = SplineSolver<Order, Dims>;

    typedef Matrix<double, Dims, 1> VectorNd;
    typedef Matrix<double, Dims, Dynamic> MatrixNXd;

    typedef UnitBoundedPolynomial1<Order> Polynomial1;
    typedef UnitBoundedPolynomial<Order, Dims> Polynomial;

    vector<Polynomial> children;
    double length;

    HermiteSpline() : length(0)
    {
    }

    VectorNd interpolate(double s);

    void walk(const double deltatau, void (*fn)(VectorNd, HermiteSpline<Order, Dims>&, double, Polynomial&, void *), void *payload = nullptr);
    void add(Polynomial sp);

    /**
     * Calculates the hermite spline.
     * @param values    An array of required values (i.e. point, derivative, 2nd derivative, ..) x dimensions x number of points.
     * @return HermiteSpline
     */
    static HermiteSpline<Order, Dims> fit(array<MatrixNXd, Polynomial1::RequiredValues> values);
};

template<unsigned int Order, unsigned int Dims>
class BaseSplineSolver
{
public:

    typedef typename Eigen::Matrix<double, Dims, 1> VectorNd;
    typedef typename Eigen::Matrix<double, Dims, Dynamic> MatrixNXd;
    typedef typename MatrixNXd::RowXpr RowXpr;

    typedef UnitBoundedPolynomial1<Order> Polynomial1;
    typedef UnitBoundedPolynomial<Order, Dims> Polynomial;

    typedef HermiteSpline<Order, Dims> Spline;

    HermiteSpline<Order, Dims> solve(vector<VectorNd> points,
                                     Matrix<double, Dims, Polynomial1::RequiredValues - 1> start,
                                     Matrix<double, Dims, Polynomial1::RequiredValues - 1> end);

protected:

    virtual bool find_params_1d(RowXpr params[Polynomial1::RequiredValues]) = 0;

};

/**
 * The spline solver class implements methods
 * to compute a spline given N+1 points
 * plus start and end point slopes.
 */
template<unsigned int Order, unsigned int Dims>
class SplineSolver : public BaseSplineSolver<Order, Dims>
{};

// Quintic solver

template<unsigned int Dims>
class SplineSolver<5, Dims> : public BaseSplineSolver<5, Dims>
{
public:

    using RowXpr = typename BaseSplineSolver<5, Dims>::RowXpr;

protected:

    SparseMatrix<double> A;
    SparseLU<SparseMatrix<double>,  COLAMDOrdering<int>> solver;

    bool find_params_1d(RowXpr params[3]);
    bool build_solver(const int N, const int M);

};

// Cubic solver

template<unsigned int Dims>
class SplineSolver<3, Dims> : public BaseSplineSolver<3, Dims>
{
public:

    using RowXpr = typename BaseSplineSolver<3, Dims>::RowXpr;

protected:

    SparseMatrix<double> A;
    SparseLU<SparseMatrix<double>,  COLAMDOrdering<int>> solver;

    bool find_params_1d(RowXpr params[2]);
    bool build_solver(const int N, const int M);

};

template <unsigned int Dims>
using CubicHermiteSpline = HermiteSpline<3, Dims>;

template <unsigned int Dims>
using QuinticHermiteSpline = HermiteSpline<5, Dims>;

#endif