#include<iostream>
#include<fstream>
#include<vector>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/sparsity_pattern.h>

using namespace dealii;

// Computing matrix A times vector b, and using the A[0] to store the result and return it.
std::vector<double> multiply(std::vector<std::vector<double>> A, std::vector<double> b)
{
  int m=A.size();
  int n=b.size();
  for (int i=0;i<m;i++)
    {
      double temp=0;
      for(int j=0;j<n;j++)
	{
	  temp+=A[i][j]*b[j];
	}
      A[0][i]=temp;
      //std::cout<<temp;
      //std::cout<<"The result of "<<i+1<<"th iteration\n";

    }

  return A[0];
}

// Computing u'*v;
double multiply(std::vector<double> u, std::vector<double> v)
{
  if(u.size()!=v.size())
    {
      std::cout<<"Multiply() ERROR: u'*v error! Please Check the size of u and v!";
    }
  else
    {
      double sum=0;
      for(int i=0;i<u.size();i++)
	{
	  sum+=u[i]*v[i];
	}

      return sum;
    }
}

// Design for return transpose matrix of A;
std::vector<std::vector<double>> transpose(std::vector<std::vector<double>> A)
{
  int n=A.size();
  int m=A[0].size();
    if (n < m)
      {
        for (int i = 0; i < n; i++)
	  {
            for (int j = 0; j < i; j++)
	      {
                double temp = A[j][i];
                A[j][i] = A[i][j];
                A[i][j] = temp;
	      }
	  }
        for (int i = n; i < m; i++)
	  {
            std::vector<double> temp;
            for (int j = 0; j < n; j++)
	      {
                temp.push_back(A[j][i]);
	      }
            A.push_back(temp);
	  }
        for (int i = 0; i < n; i++)
	  {
            for (int j = n; j < m; j++)
	      {
                A[i].pop_back();
	      }
	  }
      }
    else if(m<n)
      {
        for (int i = 0; i < m; i++)
	  {
            for (int j = 0; j < i; j++)
	      {
                double temp = A[j][i];
                A[j][i] = A[i][j];
		A[i][j] = temp;
	      }
	  }
        for (int i = 0; i < m; i++)
	  {
            for (int j = m; j < n; j++)
	      {
                A[i].push_back(A[j][i]);
	      }
	  }

        for (int i = m; i < n; i++)
	  {
            A.pop_back();
	  }

      }
    else
      {
	for(int i=0;i<n;i++)
	  {
	    for(int j=0;j<i;j++)
	      {
		double temp=A[j][i];
		A[j][i]=A[i][j];
		A[i][j]=temp;
	      }
	  }
      }

    return A;

}

//This function computes the matrix A times matrix B, and its result will be stored in A.
// It can be improved if reducing the use of the temp matrix to store the original values which
// is used in computing.
void multiply(std::vector<std::vector<double>> &A, std::vector<std::vector<double>> B)
{
    int n = A.size();
    int m = B.size();
    int col = B[0].size();
    std::vector<double> temp(m);
    std::vector<std::vector<double>> A0(n, temp);

    if (col < n)
      {
        for (int i = 0; i < n; i++)
	  {
            for (int j = 0; j < col; j++)
	      {
                double tempValue = 0;
                for (int t = 0; t < m; t++)
		  {
                    tempValue += A[i][t] * B[t][j];
		  }
                A0[i][j] = tempValue;
	      }
            for (int j = col; j < m; j++)
	      {
                A0[i].pop_back();
	      }
	  }

        A = A0;
      }
    else
      {
        for (int i = 0; i <n; i++)
	  {
            for (int j = 0; j < m; j++)
	      {
                double tempValue = 0;
                for (int t = 0; t < m; t++)
		  {
                    tempValue += A[i][t] * B[t][j];
		  }
                A0[i][j] = tempValue;
	      }
            for (int j = m; j < col; j++)
	      {
                double tempValue = 0;
                for (int t = 0; t < m; t++)
		  {
                    tempValue += A[i][t] * B[t][j];
		  }
                A0[i].push_back(tempValue);
	      }
	  }
        A = A0;
      }
}

// this function will turn the matrix Q into the nxn identity matrix;
void identitymatrix(std::vector<std::vector<double>> &Q, int n)
{
  Q.clear();
  std::vector<double> temp(n,0);
  for (int i=0;i<n;i++)
    {
      Q.push_back(temp);
      Q[i][i]=1;
    }
}

// This function computes the infi-norm of vector x;
double infi_Norm(std::vector<double> x)
{
  double norm=0;
  for (int i=0;i<x.size();i++)
    {
      if (norm<abs(x[i]))
	{
	  norm=abs(x[i]);
	}
    }
  return norm;
}

//////
// Multiplication of SparseMatrix A and vector x0;
std::vector<double> multiply(const SparseMatrix<double> &A, std::vector<double> x0)
{
  std::vector<double> x(x0.size(),0);
  //  std::cout<<"Flag 1!!\n";
  //std::cout<<A.n()<<"\n";

  if (A.n()!=x0.size())
    {
      std::cout<<"Function multiply() ERROR: The sizes of matrix and vector are not same! Please check it!\n";
    }
  else
    {
      // std::cout<<"TEST POINT 2 \n";
      for(int k=0;k<A.m();k++)
	{
	  //std::cout<<"this is the "<<k<<"th iterations \n";
	  SparseMatrix<double>::const_iterator i=A.begin(k);

	  while(i!=A.end(k))
	    {
	      x[k]+=i->value()*x0[i->column()];
	      ++i;
	    }
	}
      return x;
    }
}

//void Householder(std::vector<std::vector<double>> *A, std::vector<std::vector<double>> *P)
//void Householder(std::vector<std::vector<double>> *A, std::vector<std::vector<double>> *P)

// This function computes the householder process of the symmetric definite matrix, it return
// two matrices. The tridiagonal matrix coordinating with A and the hermitian unitary matrix P.
void Householder(std::vector<std::vector<double>> &A, std::vector<std::vector<double>> &P)
{
  int n=A.size();
  std::vector<std::vector<double>> Pk;

  for(int k=0;k<n-2;k++)
    {
      identitymatrix(Pk, n);
      double q=0;
      double alpha=0;
      // Computing the value of q in the Householder function;
      for(int i=k+1;i<n;i++)
	{
	  q+=A[i][k]*(A[i][k]);
	}

      // Computing the value of alpha
      if(A[k+1][k]==0)
	{
	  alpha=-sqrt(q);
	}
      else
	{
	  alpha=-sqrt(q)*A[k+1][k]/abs(A[k+1][k]);
	  // std::cout<<"the abs A[k+1][k] is "<<A[k+1][k]<<std::endl;
	}

      double RSQ=alpha*alpha-alpha*A[k+1][k]; // RSQ = 2r^2;

      //std::cout<<"Check Point 2, This is RSQ:::"<<RSQ<<"\n";

      std::vector<double> v(n-k,0), u(n-k,0), z(n-k,0);
      v[0]=0;
      v[1]=A[k+1][k]-alpha;

      //std::cout<<"Check Point 3::: "<<v[1]<<" \n";
      // w=(1/sqrt(2*RSQ)*v=1/2r*v);
      for(int j=2;j<v.size();j++)
	{
	  v[j]=A[k+j][k];
	}

      // std::cout<<"CheckPPPPPoint555 \n";

      for(int j=0;j<n-k;j++)
	{
	  for(int i=1;i<n-k;i++)
	    {
	      u[j]+=A[k+j][k+i]*v[i];
	    }
	  u[j]/=RSQ;
	}

      //std::cout<<"Check POOOIONIOHIHO\n";

      double PROD=0;
      for(int i=1;i<n-k;i++)
	{
	  PROD+=v[i]*u[i];
	}

      //std::cout<<"CheckPoint 4!!!!!!!!!\n";

      for(int j=0;j<n-k;j++)
	{
	  z[j]=u[j]-PROD/(2*RSQ)*v[j];
	}

      for(int l=k+1;l<n-1;l++)
	{
	  for(int j=l+1;j<n;j++)
	    {
	      A[j][l]=A[j][l]-v[l-k]*z[j-k]-v[j-k]*z[l-k];
	      A[l][j]=A[j][l];
	    }
	  A[l][l]=A[l][l]-2*v[l-k]*z[l-k];
	}

      A[n-1][n-1]=A[n-1][n-1]-2*v[n-k-1]*z[n-k-1];

      for(int j=k+2;j<n;j++)
	{
	  A[k][j]=0;
	  A[j][k]=0;
	}

      A[k+1][k]=A[k+1][k]-v[1]*z[0];
      A[k][k+1]=A[k+1][k];

      for (int j=k+1;j<n;j++)
	{
	  std::vector<double> temp(j-k,0);
	  for(int i=k+1;i<=j;i++)
	    {
	      for(int t=k+1;t<n;t++)
		{
		  temp[i-k-1]+=v[i-k]*v[t-k]*Pk[t][j]/(2*RSQ);
		}
	    }

	  //temp=multiply(W,Pj);// to get the jth column of the W*P;
	  for (int i=k+1;i<=j;i++)
	    {
	      Pk[i][j]-=2*temp[i-k-1];
	      Pk[j][i]=Pk[i][j];
	    }
	}
      multiply(P,Pk);

    }

}

///////
// This function computes the QR factorization of the tridiagonal matrix A.
// Input: tridiagonal matrix A, identity matrix Q
// Output: uptriangle matrix A_, the orthonormal matrix Q; A=Q*A_;
//
// tips: Maybe I can improve the function by replace the matrix A by two vectors an bn-1;
// an contains the diagonal entries of A; bn-1 contains the sub-diagonal entries of A;

void QR(std::vector<std::vector<double>> &A, std::vector<std::vector<double>> &Q)
{
  int n=A.size();

  // obtain the diagonal and subdiagonal entries of matrix A;
  /*
  std::vector<double>an(n),bn(n-1);
  for (int i=0;i<A.size()-1;i++)
    {
      an[i]=A[i][i];
      bn[i]=A[i+1][i];
    }
  an[n-1]=A[n-1][n-1];
  */

  for (int i=0;i<A.size()-1;i++)
    {
      double theta=0;
      double tempa1=A[i][i],tempa2=A[i+1][i+1],tempb=A[i+1][i],tempc=A[i][i+1];
      theta=atan(A[i+1][i]/A[i][i]);
      double c=cos(theta), s=sin(theta);

      A[i][i]=c*tempa1+s*tempb;
      A[i][i+1]=c*tempc+s*tempa2;
      A[i+1][i+1]=-s*tempc+c*tempa2;
      A[i+1][i]=-s*tempa1+c*tempb;
      if (i<A.size()-2)
	{
	  A[i][i+2]=s*A[i+1][i+2];
	  A[i+1][i+2]=c*A[i+1][i+2];
	}

      // update the orthogonal matrix Q by Q_k=Q_k-1*Q;

      for(int k=0;k<n;k++)
	{
	  double tmp1=Q[k][i],tmp2=Q[k][i+1];
	  Q[k][i]=Q[k][i]*c+Q[k][i+1]*s;
	  Q[k][i+1]=tmp1*(-s)+tmp2*c;
	}

    }

}

// This function computes the eigenvalue of the matrix A by QR methods, and it will also return the
// matrix Q during the process;
void QRSolver(std::vector<std::vector<double>> &A, std::vector<std::vector<double>> &Q, double tol=0.001)
{
  int n=A.size();
  // using householder transform matrix A into the tridiagonal matrix
  // store it in A and store the Householder unitary matrix in Q; A=QA_Q;
  //Householder(A,Q);

  // Qk as the initial matrix for every QR factorization, multiplying it together to get the
  // final unitary matrix;
  std::vector<std::vector<double>> Qk;

  // Get the subdiagonal entries of the matrix A and verify if its norm smaller than the tolerance;
  // If it is small enough that means we diagonalize the matrix A and the diagonal entries are
  // eigenvalues of A.
  std::vector<double> b(n-1,1);
  for(int i=0;i<n-1;i++)
    {
      b.push_back(A[i+1][i]);
    }

  int num=0;
  while (infi_Norm(b)>tol&&num<1000)
    {
      num++;

      b.clear();

      Householder(A,Q);

      identitymatrix(Qk,n);

      QR(A,Qk);

      // Compute the num-th iteration, get the Qk in this step;
      multiply(Q,Qk);
      // compute R*Q beacuse I store the R(computed above) into A, So I directly use multiply
      // function to get A*Q into A=RQ.
      multiply(A,Qk);

      //update the entries of b, i.e. the subdiagonal entries of matrix A=RQ;
        for(int i=0;i<n-1;i++)
	  {
	    b.push_back(A[i+1][i]);
	  }
    }
}

class MinTrace
{
 public:
  MinTrace();
  MinTrace(SparseMatrix<double> A, SparseMatrix<double> M, int p=1);

  // get the random initial matrix V which satisifies that: V'*M*V=I_p;
  std::vector<std::vector<double>>rand_V();

  // compute the orthonormal vectors corresponding to M from the original vectors U={u_1,u_2,...,u_p};
  std::vector<std::vector<double>> M_GS(std::vector<std::vector<double>> U);

  // Compute the solution vector of the equation of X_k[i] and PAP*d_i=PA*X_k[i];
  std::vector<double>CG(std::vector<double> X_ki);
  // get the p smallest eigenvalues of A correponding with M;
  std::vector<double> min_trace(int max_iter, double eps);

 private:
  SparseMatrix<double> A;
  SparseMatrix<double> M;
  int p; // The number of the eigenvalues to be solved;
};

MinTrace::MinTrace()
{
  int p=3;
  SparsityPattern sparsity_pattern(10,10,{2,3,3,3,3,3,3,3,3,2});
  SparsityPattern sp2(10,10,1);
  //sparsity_pattern.add(1,2);
  sparsity_pattern.add(0,1);
  sparsity_pattern.add(9,8);
  for (int i=1;i<9;i++)
    {
      sparsity_pattern.add(i,i+1);
      sparsity_pattern.add(i,i-1);
    }

  A.reinit(sparsity_pattern);
  M.reinit(sp2);

  for (int k=0;k<A.m();k++)
    {
      SparseMatrix<double>::iterator i=A.begin(k);
      i->value()=2;
      while(i!=A.end(k))
	{
	  i->value()+=1;
	  ++i;
	}
    }

  SparseMatrix<double>::iterator i=M.begin();
  double num=0;
  while(i!=M.end())
    {
      num=num+1;
      i->value()=num;
      ++i;
    }
}

// ATTENTION!!! There is a difficulty that SparseMatrix can not be copied directly!
MinTrace::MinTrace(SparseMatrix<double> A0, SparseMatrix<double> M0, int p0)
{

};

std::vector<std::vector<double>> MinTrace::M_GS(std::vector<std::vector<double>> U)
{
  std::vector<std::vector<double>> V;
  V=transpose(U);
}

std::vector<std::vector<double>> MinTrace::rand_V()
{
  std::cout<<"CheckPoint1\n";
  std::vector<double> x(M.n(),0);
  for (int i=0;i<x.size();i++)
    {
      std::cout<<x[i]<<" ";
    }
  std::cout<<"\n";
  std::cout<<"CheckPoint666 \n";
  std::vector<std::vector<double>> V(p,x);
  std::cout<<"CheckPoint12314113\n";

  V[0][0]=1;
  V[1][1]=sqrt(2.0)/2;
  V[2][2]=sqrt(3.0)/3;
  std::cout<<"231wqrdasd24121321321321adsadq2q1312321sada\n";
  return V;
}

std::vector<double> MinTrace::min_trace(int max_iter=10, double eps=1e-03)
{
  std::cout<<M.n()<<"   it is a test!\n";

  std::vector<double> x(p); // define the eigenvector;
  std::vector<std::vector<double>> V; // V is an n x p matrix and stored as V[p][n];

  V=rand_V();
  std::cout<<"wewqewqewqewqeqw\n";
  //  for(int k=0;k<max_iter;k++)
    for(int k=0;k<1;k++)
    {
      std::cout<<"CheckPoint222\n";
      std::vector<std::vector<double>> VMV(p,x),tmpVMV(p); // VMV is a p x p matrix;
      std::cout<<"AAAAAAAAAAAAAAAAAAAAAAAA\n";
      for(int i=0;i<p;i++)
	{
	  tmpVMV[i]=multiply(M,V[i]);
	  std::cout<<"CheckPoint111111111\n";
	}

      std::cout<<"CheckPoint2\n";
      for(int i=0;i<p;i++)
	{
	  for(int j=0;j<p;j++)
	    {
	      double tmp=0;
	      for(int l=0;l<M.n();l++)
		{
		  tmp+=V[i][l]*tmpVMV[j][l];
		}
	      VMV[i][j]=tmp;
	    }
	}

      for(int i=0;i<p;i++)
	{
	  for(int j=0;j<p;j++)
	    {
	      std::cout<<VMV[i][j]<<" ";
	    }
	  std::cout<<"\n";
	}
    }

   return x;
}

int main()
{
  SparseMatrix<double> A, M;
  // std::ofstream sparsematrix1 ("original_matrix.1");
  //A.print(sparsematrix1);
  std::vector<unsigned int> row_length(10,10);
  unsigned int t=3;
  SparsityPattern sparsity_pattern(10,10,{2,3,3,3,3,3,3,3,3,2});
  SparsityPattern sp2(10,10,1);
  //sparsity_pattern.add(1,2);
  sparsity_pattern.add(0,1);
  sparsity_pattern.add(9,8);
  for (int i=1;i<9;i++)
    {
      sparsity_pattern.add(i,i+1);
      sparsity_pattern.add(i,i-1);
    }

  A.reinit(sparsity_pattern);
  M.reinit(sp2);

  // In this way, I can construct a SparseMatrix to be the test data for the algorithm.
  for (int k=0;k<A.m();k++)
    {
      SparseMatrix<double>::iterator i=A.begin(k);
      i->value()=2;
      while(i!=A.end(k))
	{
	  i->value()+=1;
	  ++i;
	}
    }

  SparseMatrix<double>::iterator i=M.begin();
  double num=0;
  while(i!=M.end())
    {
      num=num+1;
      i->value()=num;
      ++i;
    }

  std::ofstream out ("sparsity_pattern.1");
  sparsity_pattern.print_gnuplot(out);

  std::ofstream sparsematrix ("sparse_matrix.1");
  A.print(sparsematrix);

  std::ofstream sparsematrix2 ("sparse_matrix.2");
  M.print(sparsematrix2);

  std::vector<double> x(10,1);
  x=multiply(M,x);
  for(int i=0;i<x.size();i++)
    {
      std::cout<<x[i]<<" ";
    }
 // std::cout<<"\n"<<1e-03<<"\n";

  MinTrace solve;

//  solve.min_trace();
  //sparsity_pattern.print();
  return 0;
}


#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


/* 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);
}

}


// 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 */


#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()
	{
	}

}


#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


#include "ImplicitIntegrator.h"
#include "Deformation.hpp"
#include "ProtoConverter.hpp"
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <Eigen/SVD>
#include <Eigen/IterativeLinearSolvers>
#include <Mathematics/SymmetricEigensolver3x3.h>

#include <iostream>
#include <glm/gtc/constants.hpp>

using Eigen::MatrixXd;

namespace Deformation
{
    using FloatT = double;
    using Mat3 = Eigen::Matrix<FloatT, 3, 3>;
    using Mat3x4 = Eigen::Matrix<FloatT, 3, 4>;
    using Mat9x12 = Eigen::Matrix<FloatT, 9, 12>;
    using Mat9 = Eigen::Matrix<FloatT, 9, 9>;
    using Mat12 = Eigen::Matrix<FloatT, 12, 12>;
    using Vec3 = Eigen::Matrix<FloatT, 3, 1>;
    using Vec9 = Eigen::Matrix<FloatT, 9, 1>;
    using Vec12 = Eigen::Matrix<FloatT, 12, 1>;

    Mat3 Calc_Ds(const Deformation::Tetrahedra& tet, const std::vector<Vertex>& vertices, bool restState)
    {
        Mat3 ds = Mat3::Zero();

        for (int row = 0; row < 3; row++)
        {
            for (int col = 1; col < 4; col++)
            {
                if (restState)
					ds(row, col - 1) = vertices[tet.mIndices[col]].mMaterialCoordinates[row] - vertices[tet.mIndices[0]].mMaterialCoordinates[row];
                else
                    ds(row, col - 1) = vertices[tet.mIndices[col]].mPosition[row] - vertices[tet.mIndices[0]].mPosition[row];
            }
        }

        return ds;
    }

    Mat3 Calc_DmInv(const Deformation::Tetrahedra& tet, const std::vector<Vertex>& vertices)
    {
        auto mat = Calc_Ds(tet, vertices, true);
        if (std::abs(mat.determinant()) < 1e-10f)
            throw std::exception("Small");
        return mat.inverse();
    }

    Mat3 Calc_F(const Mat3& ds, const Mat3& dmInv)
    {
        return ds * dmInv;
    }

    Mat3 Calc_E(const Mat3& f)
    {
        return 0.5f * (f * f.transpose() - Mat3::Identity());
    }

    Mat3 Calc_dPsiDf(const Mat3& f, const Mat3& e)
    {
        FloatT youngsModulus = 8 * 10e8f;
        FloatT poissonRatio = 0.45f;
        FloatT lamba = youngsModulus * poissonRatio / ((1.0f + poissonRatio) * (1.0f - 2.0f * poissonRatio));
        FloatT mu = youngsModulus / (2.0f + 2.0f * poissonRatio);
        return mu * f * e + lamba * e.trace() * f;
    }

    Vec9 Reshape3x3(const Mat3& m)
    {
        Vec9 vec = Vec9::Zero();
        vec(0, 0) = m(0, 0);
        vec(1, 0) = m(1, 0);
        vec(2, 0) = m(2, 0);

        vec(3, 0) = m(0, 1);
        vec(4, 0) = m(1, 1);
        vec(5, 0) = m(2, 1);

        vec(6, 0) = m(0, 2);
        vec(7, 0) = m(1, 2);
        vec(8, 0) = m(2, 2);

        return vec;
    }

    Vec12 Reshape3x4(const Mat3x4& m)
    {
        Vec12 vec = Vec12::Zero();
        vec(0, 0) = m(0, 0);
        vec(1, 0) = m(1, 0);
        vec(2, 0) = m(2, 0);

        vec(3, 0) = m(0, 1);
        vec(4, 0) = m(1, 1);
        vec(5, 0) = m(2, 1);

        vec(6, 0) = m(0, 2);
        vec(7, 0) = m(1, 2);
        vec(8, 0) = m(2, 2);

        vec(9, 0) = m(0, 3);
        vec(10, 0) = m(1, 3);
        vec(11, 0) = m(2, 3);
        return vec;
    }

    Mat9x12 Calc_dFdx(const Mat3& dmInv)
    {
        auto m = dmInv(0, 0);
        auto n = dmInv(0, 1);
        auto o = dmInv(0, 2);
        auto p = dmInv(1, 0);
        auto q = dmInv(1, 1);
        auto r = dmInv(1, 2);
        auto s = dmInv(2, 0);
        auto t = dmInv(2, 1);
        auto u = dmInv(2, 2);

        auto t1 = -m - p - s;
        auto t2 = -n - q - t;
        auto t3 = -o - r - u;

        Mat9x12 dFdx = Mat9x12::Zero();

        dFdx(0, 0) = t1;
        dFdx(0, 3) = m;
        dFdx(0, 6) = p;
        dFdx(0, 9) = s;
        dFdx(1, 1) = t1;
        dFdx(1, 4) = m;
        dFdx(1, 7) = p;
        dFdx(1, 10) = s;

        dFdx(2, 2) = t1;
        dFdx(2, 5) = m;
        dFdx(2, 8) = p;
        dFdx(2, 11) = s;
        dFdx(3, 0) = t2;
        dFdx(3, 3) = n;
        dFdx(3, 6) = q;
        dFdx(3, 9) = t;
        dFdx(4, 1) = t2;
        dFdx(4, 4) = n;

        dFdx(4, 7) = q;
        dFdx(4, 10) = t;
        dFdx(5, 2) = t2;
        dFdx(5, 5) = n;
        dFdx(5, 8) = q;
        dFdx(5, 11) = t;
        dFdx(6, 0) = t3;
        dFdx(6, 3) = o;
        dFdx(6, 6) = r;
        dFdx(6, 9) = u;
        dFdx(7, 1) = t3;
        dFdx(7, 4) = o;

        dFdx(7, 7) = r;
        dFdx(7, 10) = u;
        dFdx(8, 2) = t3;
        dFdx(8, 5) = o;
        dFdx(8, 8) = r;
        dFdx(8, 11) = u;

        return dFdx;
    }

    Vec9 Calc_g_i(const Mat3& f)
    {
        Vec9 g_i = Vec9::Zero();
        g_i(0, 0) = 2 * f(0, 0);
        g_i(1, 0) = 2 * f(1, 0);
        g_i(2, 0) = 2 * f(2, 0);

        g_i(3, 0) = 2 * f(0, 1);
        g_i(4, 0) = 2 * f(1, 1);
        g_i(5, 0) = 2 * f(2, 1);

        g_i(6, 0) = 2 * f(0, 2);
        g_i(7, 0) = 2 * f(1, 2);
        g_i(8, 0) = 2 * f(2, 2);
        return g_i;
    }

    Mat3 Calc_dj_df(const Mat3& f)
    {
        Mat3 dj_df = Mat3::Zero();
        dj_df.col(0) = f.col(1).cross(f.col(2));
        dj_df.col(1) = f.col(2).cross(f.col(0));
        dj_df.col(2) = f.col(0).cross(f.col(1));

        //dj_df.block(0, 0, 3, 1) = f.col(1).cross(f.col(2));
        //dj_df.block(0, 1, 3, 1) = f.col(2).cross(f.col(0));
        //dj_df.block(0, 2, 3, 1) = f.col(0).cross(f.col(1));
        return dj_df;
    }

    FloatT Calc_i_c(const Mat3& f)
    {
        auto frob = f.norm();
        return frob * frob;
    }

    Mat9 Calc_h_i()
    {
        return 2 * Mat9::Identity();
    }

    Mat3 Calc_f_hat(const Vec3& f_i)
    {
        Mat3 f_hat = Mat3::Zero();
        // col 0
        f_hat(1, 0) = f_i(2);
        f_hat(2, 0) = -f_i(1);

        // col 1
        f_hat(0, 1) = -f_i(2);
        f_hat(2, 1) = f_i(0);

        // col 2
        f_hat(0, 2) = f_i(1);
        f_hat(1, 2) = -f_i(0);

        return f_hat;
    }

    Mat9 Calc_h_j(const Mat3& f)
    {
        Mat9 h_j = Mat9::Zero();
        auto f0_hat = Calc_f_hat(f.col(0));
        auto f1_hat = Calc_f_hat(f.col(1));
        auto f2_hat = Calc_f_hat(f.col(2));

        // col 0
        h_j.block(3, 0, 3, 3) = f2_hat;
        h_j.block(6, 0, 3, 3) = -f1_hat;

        // col 1
        h_j.block(0, 3, 3, 3) = -f2_hat;
        h_j.block(6, 3, 3, 3) = f0_hat;

        // col 2
        h_j.block(0, 6, 3, 3) = f1_hat;
        h_j.block(3, 6, 3, 3) = -f0_hat;

        return h_j;
    }

    Mat9 Calc_D(const Mat3& f)
    {
        Mat9 d = Mat9::Zero();
        const auto& f0 = f.col(0);
        const auto& f1 = f.col(1);
        const auto& f2 = f.col(2);

        d.block(0, 0, 3, 3) = f0 * f0.transpose();
        d.block(3, 0, 3, 3) = f0 * f1.transpose();
        d.block(6, 0, 3, 3) = f0 * f2.transpose();

        d.block(0, 3, 3, 3) = f1 * f0.transpose();
        d.block(3, 3, 3, 3) = f1 * f1.transpose();
        d.block(6, 3, 3, 3) = f1 * f2.transpose();

        d.block(0, 6, 3, 3) = f2 * f0.transpose();
        d.block(3, 6, 3, 3) = f2 * f1.transpose();
        d.block(6, 6, 3, 3) = f2 * f2.transpose();

        return d;
    }

    Mat9 KroneckerProduct(const Mat3& a, const Mat3& b)
    {
        Mat9 product = Mat9::Zero();
        product.block(0, 0, 3, 3) = a(0, 0) * b;
        product.block(3, 0, 3, 3) = a(1, 0) * b;
        product.block(6, 0, 3, 3) = a(2, 0) * b;

        product.block(0, 3, 3, 3) = a(0, 1) * b;
        product.block(3, 3, 3, 3) = a(1, 1) * b;
        product.block(6, 3, 3, 3) = a(2, 1) * b;

        product.block(0, 6, 3, 3) = a(0, 2) * b;
        product.block(3, 6, 3, 3) = a(1, 2) * b;
        product.block(6, 6, 3, 3) = a(2, 2) * b;

        return product;
    }

    Mat9 Calc_h_2(const Mat3& f, const Mat9& d)
    {
        return 4 * (KroneckerProduct(Mat3::Identity(), f * f.transpose()) + KroneckerProduct(f * f.transpose(), Mat3::Identity()) + d);
    }

    Mat9 Calc_vec_dPsi2_dF2(const Vec9& g_i, FloatT i_c, const Mat9& h_i, const Mat9& h_2, FloatT mu, FloatT lambda)
    {
        return lambda / 4.0f * (g_i * g_i.transpose()) + (-mu / 2.0f + lambda / 4.0f * (i_c - 3.0f)) * h_i + mu / 4.0f * h_2;
    }

    Mat12 Calc_dfdx(const Mat9x12& dFdx, const Mat9 & vec_dPsi2_dF2, FloatT restVol)
    {
        return -restVol * dFdx.transpose() * vec_dPsi2_dF2 * dFdx;
    }

    Mat3 Get_Q(int i, const std::array<FloatT, 3>& epsilon, const std::array<FloatT, 3>& sigma, const Mat3& u, const Mat3& v_transpose)
    {
        Mat3 d = Mat3::Zero();
        d(0, 0) = sigma[0] * sigma[2] + sigma[1] * epsilon[i];
        d(1, 1) = sigma[1] * sigma[2] + sigma[0] * epsilon[i];
		d(2, 2) = epsilon[i] * epsilon[i] - sigma[2] * sigma[2];
        return 1.0f / d.norm() * u * d * v_transpose;
    }

    Mat3 Get_Q(const Mat3 & U, const Mat3 & V, int i)
    {
        //Mat3 q = Mat3::Zero();

        //if (i == 3)
        //{
        //    q(0, 1) = -1;
        //    q(1, 0) = 1;
        //}
        //else if (i == 4)
        //{
        //    q(1, 2) = 1;
        //    q(2, 1) = -1;
        //}
        //else if (i == 5)
        //{
        //    q(0, 2) = 1;
        //    q(2, 0) = -1;
        //}
        //else if (i == 6)
        //{
        //    q(0, 1) = 1;
        //    q(1, 0) = 1;
        //}
        //else if (i == 7)
        //{
        //    q(1, 2) = 1;
        //    q(2, 1) = 1;
        //}
        //else if (i == 8)
        //{
        //    q(0, 2) = 1;
        //    q(2, 0) = 1;
        //}

        //return 1 / sqrt(2.0f) * u * q * v_transpose;

        static const FloatT scale = 1.0 / std::sqrt(2.0);
        const Mat3 sV = scale * V;

        using M3 = Eigen::Matrix<FloatT, 3, 3, Eigen::ColMajor>;

        M3 A;
        A << sV(0, 2) * U(0, 1), sV(1, 2)* U(0, 1), sV(2, 2)* U(0, 1),
            sV(0, 2)* U(1, 1), sV(1, 2)* U(1, 1), sV(2, 2)* U(1, 1),
            sV(0, 2)* U(2, 1), sV(1, 2)* U(2, 1), sV(2, 2)* U(2, 1);

        M3 B;
        B << sV(0, 1) * U(0, 2), sV(1, 1)* U(0, 2), sV(2, 1)* U(0, 2),
            sV(0, 1)* U(1, 2), sV(1, 1)* U(1, 2), sV(2, 1)* U(1, 2),
            sV(0, 1)* U(2, 2), sV(1, 1)* U(2, 2), sV(2, 1)* U(2, 2);

        M3 C;
        C << sV(0, 2) * U(0, 0), sV(1, 2)* U(0, 0), sV(2, 2)* U(0, 0),
            sV(0, 2)* U(1, 0), sV(1, 2)* U(1, 0), sV(2, 2)* U(1, 0),
            sV(0, 2)* U(2, 0), sV(1, 2)* U(2, 0), sV(2, 2)* U(2, 0);

        M3 D;
        D << sV(0, 0) * U(0, 2), sV(1, 0)* U(0, 2), sV(2, 0)* U(0, 2),
            sV(0, 0)* U(1, 2), sV(1, 0)* U(1, 2), sV(2, 0)* U(1, 2),
            sV(0, 0)* U(2, 2), sV(1, 0)* U(2, 2), sV(2, 0)* U(2, 2);

        M3 E;
        E << sV(0, 1) * U(0, 0), sV(1, 1)* U(0, 0), sV(2, 1)* U(0, 0),
            sV(0, 1)* U(1, 0), sV(1, 1)* U(1, 0), sV(2, 1)* U(1, 0),
            sV(0, 1)* U(2, 0), sV(1, 1)* U(2, 0), sV(2, 1)* U(2, 0);

        M3 F;
        F << sV(0, 0) * U(0, 1), sV(1, 0)* U(0, 1), sV(2, 0)* U(0, 1),
            sV(0, 0)* U(1, 1), sV(1, 0)* U(1, 1), sV(2, 0)* U(1, 1),
            sV(0, 0)* U(2, 1), sV(1, 0)* U(2, 1), sV(2, 0)* U(2, 1);

        if (i == 3)
            return B - A;
        else if (i == 4)
            return D - C;
        else if (i == 5)
            return F - E;
        else if (i == 6)
            return A + B;
        else if (i == 7)
            return C + D;
        else if (i == 8)
            return E + F;
    }

    FloatT Calc_Epsilon(int i, FloatT I2, FloatT I3)
    {
        return 2 * sqrtf(I2 / 3.0f) * cosf(1.0f / 3.0f * (acosf(3*I3/I2*sqrtf(3/I2)) + 2*glm::pi<FloatT>()*(i-1)));
    }

    void BuildTwistAndFlipEigenvectors(const Mat3& U, const Mat3& V, Mat9& Q)
    {
        static const FloatT scale = 1.0 / std::sqrt(2.0);
        const Mat3 sV = scale * V;

        using M3 = Eigen::Matrix<FloatT, 3, 3, Eigen::ColMajor>;

        M3 A;
        A << sV(0, 2) * U(0, 1), sV(1, 2)* U(0, 1), sV(2, 2)* U(0, 1),
            sV(0, 2)* U(1, 1), sV(1, 2)* U(1, 1), sV(2, 2)* U(1, 1),
            sV(0, 2)* U(2, 1), sV(1, 2)* U(2, 1), sV(2, 2)* U(2, 1);

        M3 B;
        B << sV(0, 1) * U(0, 2), sV(1, 1)* U(0, 2), sV(2, 1)* U(0, 2),
            sV(0, 1)* U(1, 2), sV(1, 1)* U(1, 2), sV(2, 1)* U(1, 2),
            sV(0, 1)* U(2, 2), sV(1, 1)* U(2, 2), sV(2, 1)* U(2, 2);

        M3 C;
        C << sV(0, 2) * U(0, 0), sV(1, 2)* U(0, 0), sV(2, 2)* U(0, 0),
            sV(0, 2)* U(1, 0), sV(1, 2)* U(1, 0), sV(2, 2)* U(1, 0),
            sV(0, 2)* U(2, 0), sV(1, 2)* U(2, 0), sV(2, 2)* U(2, 0);

        M3 D;
        D << sV(0, 0) * U(0, 2), sV(1, 0)* U(0, 2), sV(2, 0)* U(0, 2),
            sV(0, 0)* U(1, 2), sV(1, 0)* U(1, 2), sV(2, 0)* U(1, 2),
            sV(0, 0)* U(2, 2), sV(1, 0)* U(2, 2), sV(2, 0)* U(2, 2);

        M3 E;
        E << sV(0, 1) * U(0, 0), sV(1, 1)* U(0, 0), sV(2, 1)* U(0, 0),
            sV(0, 1)* U(1, 0), sV(1, 1)* U(1, 0), sV(2, 1)* U(1, 0),
            sV(0, 1)* U(2, 0), sV(1, 1)* U(2, 0), sV(2, 1)* U(2, 0);

        M3 F;
        F << sV(0, 0) * U(0, 1), sV(1, 0)* U(0, 1), sV(2, 0)* U(0, 1),
            sV(0, 0)* U(1, 1), sV(1, 0)* U(1, 1), sV(2, 0)* U(1, 1),
            sV(0, 0)* U(2, 1), sV(1, 0)* U(2, 1), sV(2, 0)* U(2, 1);

        // Twist eigenvectors
        //std::cout << "eigenvectors before mapping" << std::endl;
        //std::cout << Q << std::endl;
        //std::cout << "B-A" << std::endl;
        //std::cout << B - A << std::endl;
        Eigen::Map<M3>(Q.data()) = B - A;
        //std::cout << "eigenvectors after single mapping" << std::endl;
        //std::cout << Q << std::endl;
        Eigen::Map<M3>(Q.data() + 9) = D - C;
        Eigen::Map<M3>(Q.data() + 18) = F - E;

        // Flip eigenvectors
        Eigen::Map<M3>(Q.data() + 27) = A + B;
        Eigen::Map<M3>(Q.data() + 36) = C + D;
        Eigen::Map<M3>(Q.data() + 45) = E + F;
    }

    Mat9 CalcProjectedHessian(FloatT mu, FloatT lambda, const Mat3& F, const Mat3& U, const Mat3& V, const Vec3& S)
    {
        Vec9 eigenvalues;
        Mat9 eigenvectors;

        const FloatT J = F.determinant();

        // Compute the twist and flip eigenvalues
        {
            // Twist eigenvalues
            eigenvalues.segment<3>(0) = S;
            // Flip eigenvalues
            eigenvalues.segment<3>(3) = -S;
            const FloatT evScale = lambda * (J - 1.0) - mu;
            eigenvalues.segment<6>(0) *= evScale;
            eigenvalues.segment<6>(0).array() += mu;
        }

        // Compute the twist and flip eigenvectors
        BuildTwistAndFlipEigenvectors(U, V, eigenvectors);

        // Compute the remaining three eigenvalues and eigenvectors
        {
            Mat3 A;
            const FloatT s0s0 = S(0) * S(0);
            const FloatT s1s1 = S(1) * S(1);
            const FloatT s2s2 = S(2) * S(2);
            A(0, 0) = mu + lambda * s1s1 * s2s2;
            A(1, 1) = mu + lambda * s0s0 * s2s2;
            A(2, 2) = mu + lambda * s0s0 * s1s1;
            const FloatT evScale = lambda * (2.0 * J - 1.0) - mu;
            A(0, 1) = evScale * S(2);
            A(1, 0) = A(0, 1);
            A(0, 2) = evScale * S(1);
            A(2, 0) = A(0, 2);
            A(1, 2) = evScale * S(0);
            A(2, 1) = A(1, 2);

            //std::cout << "A" << std::endl;
            //std::cout << A << std::endl;

            const Eigen::SelfAdjointEigenSolver<Mat3> Aeigs(A);
            eigenvalues.segment<3>(6) = Aeigs.eigenvalues();

   //         std::cout << "u" << std::endl;
   //         std::cout << U << std::endl;

   //         std::cout << "v" << std::endl;
   //         std::cout << V << std::endl;

   //         std::cout << "A eigs" << std::endl;
   //         std::cout << Aeigs.eigenvalues() << std::endl;
   //         std::cout << "Unrotated eig vectors" << std::endl;
			//std::cout << Aeigs.eigenvectors() << std::endl;

   //         std::cout << "Rotated eigen 0 matrix" << std::endl;
   //         std::cout << U * Aeigs.eigenvectors().col(0).asDiagonal() * V.transpose() << std::endl;

   //         std::cout << "eigenvectors before mapping" << std::endl;
   //         std::cout << eigenvectors << std::endl;
            Eigen::Map<Mat3>(eigenvectors.data() + 54) = U * Aeigs.eigenvectors().col(0).asDiagonal() * V.transpose();
            //std::cout << "eigenvectors after single mapping" << std::endl;
            //std::cout << eigenvectors << std::endl;
            Eigen::Map<Mat3>(eigenvectors.data() + 63) = U * Aeigs.eigenvectors().col(1).asDiagonal() * V.transpose();
            Eigen::Map<Mat3>(eigenvectors.data() + 72) = U * Aeigs.eigenvectors().col(2).asDiagonal() * V.transpose();
        }

        // Clamp the eigenvalues
        for (int i = 0; i < 9; i++)
        {
            //std::cout << "lambda_" << i << " " << eigenvalues(i) << std::endl;

            if (eigenvalues(i) < 0.0)
            {
                eigenvalues(i) = 0.0;
                //eigenvalues(i) = -eigenvalues(i);

            }
        }
        //std::cout << "eigenvalues" << std::endl;
        //std::cout << eigenvalues << std::endl;

        //std::cout << "eigenvectors" << std::endl;
        //std::cout << eigenvectors << std::endl;

        return eigenvectors * eigenvalues.asDiagonal() * eigenvectors.transpose();
    }

    Mat3x4 CalcBm(const std::array<size_t, 4>& vertIndices, const std::vector<Vertex>& vertices)
    {
        std::vector<Vec3> vertices_converted;
        glm::ivec4 tetIndices({ vertIndices[0], vertIndices[1], vertIndices[2], vertIndices[3] });

        for (int i = 0; i < 4; i++)
        {
            const auto& pos = vertices[tetIndices[i]].mMaterialCoordinates;
            vertices_converted.push_back(Vec3(pos.x, pos.y, pos.z));
        }

        // TODO: Eliminate this class, it is just used to get the area normal
        class Triangle final
        {
        public:
            Triangle(const Vec3& v0, const Vec3& v1, const Vec3& v2)
                : _x0(v0)
                , _x1(v1)
                , _x2(v2)
            {}

            // TODO: Make this a constructor
            static Triangle GenerateFace(const int faceNum, const glm::ivec4& tet, const std::vector<Vec3>& verts)
            {
                assert(faceNum >= 0); assert(faceNum < 4);
                if (faceNum == 0)
                {
                    return Triangle(verts[static_cast<unsigned long>(tet[0])], verts[static_cast<unsigned long>(tet[1])], verts[static_cast<unsigned long>(tet[3])]);
                }
                else if (faceNum == 1)
                {
                    return Triangle(verts[static_cast<unsigned long>(tet[0])], verts[static_cast<unsigned long>(tet[2])], verts[static_cast<unsigned long>(tet[1])]);
                }
                else if (faceNum == 2)
                {
                    return Triangle(verts[static_cast<unsigned long>(tet[3])], verts[static_cast<unsigned long>(tet[2])], verts[static_cast<unsigned long>(tet[0])]);
                }
                else if (faceNum == 3)
                {
                    return Triangle(verts[static_cast<unsigned long>(tet[1])], verts[static_cast<unsigned long>(tet[2])], verts[static_cast<unsigned long>(tet[3])]);
                }
                else
                {
                    std::cerr << "Error, impossible code path hit." << std::endl;
                    std::exit(EXIT_FAILURE);
                }
            }

            // TODO: Roll these into one computation
            Vec3 normal() const
            {
                return ((_x1 - _x0).cross(_x2 - _x0)).normalized();
            }
            FloatT area() const
            {
                return 0.5 * ((_x1 - _x0).cross(_x2 - _x0)).norm();
            }
        private:
            const Vec3& _x0;
            const Vec3& _x1;
            const Vec3& _x2;
        };

        Mat3x4 Bm;
		const Triangle tri0 = Triangle::GenerateFace(0, tetIndices, vertices_converted);
		const Triangle tri1 = Triangle::GenerateFace(1, tetIndices, vertices_converted);
		const Triangle tri2 = Triangle::GenerateFace(2, tetIndices, vertices_converted);
		const Triangle tri3 = Triangle::GenerateFace(3, tetIndices, vertices_converted);

		// Calculate the area vectors
		// v0 is incident on faces (0,1,2)
		Bm.col(0) = tri0.normal() * tri0.area() + tri1.normal() * tri1.area() + tri2.normal() * tri2.area();
		// v1 is incident on faces (0,1,3)
		Bm.col(1) = tri0.normal() * tri0.area() + tri1.normal() * tri1.area() + tri3.normal() * tri3.area();
		// v2 is incident on faces (1,2,3)
		Bm.col(2) = tri1.normal() * tri1.area() + tri2.normal() * tri2.area() + tri3.normal() * tri3.area();
		// v3 is incident on faces (0,2,3)
		Bm.col(3) = tri0.normal() * tri0.area() + tri2.normal() * tri2.area() + tri3.normal() * tri3.area();
		Bm /= -3.0;

        return Bm;
    }

    Eigen::VectorXf ComputePerturbedForce(const TetraGroup& group, const Eigen::VectorXf& deltaX)
    {
        size_t numVertices = group.mVertices.size();
        Eigen::VectorXf globalForce = Eigen::VectorXf::Zero(3 * numVertices);

        for (const auto& pair : group.mIdToTetrahedra)
        {
            const auto& tet = pair.second;

            auto ds = Calc_Ds(tet, group.mVertices, false);
            auto dmInv = Calc_DmInv(tet, group.mVertices);
            auto f = Calc_F(ds, dmInv);

            auto j = f.determinant();
            auto dj_df = Calc_dj_df(f);
            auto g_j = Reshape3x3(dj_df);
            Mat3 dPsi_dF = group.mMu * f + (group.mLambda * (j - 1.0f) - group.mMu) * dj_df;

            auto dF_dx = Calc_dFdx(dmInv);
            auto dPsi_dx = dF_dx.transpose() * Reshape3x3(dPsi_dF);
            Vec12 force = -tet.mRestVolume * dPsi_dx; // 12x1 atm

            Eigen::JacobiSVD<Mat3, Eigen::NoQRPreconditioner> svd_f(f, Eigen::ComputeFullU | Eigen::ComputeFullV);
            auto sigmas = svd_f.singularValues();
            auto u = svd_f.matrixU();
            auto v = svd_f.matrixV();

            if (u.determinant() < 0.0)
            {
                u.col(0) *= -1.0;
                sigmas(0) *= -1.0;
            }
            if (v.determinant() < 0.0)
            {
                v.col(0) *= -1.0;
                sigmas(0) *= -1.0;
            }

            // Contribution to globalB
            for (size_t i = 0; i < 4; i++)
            {
                auto vert_i = tet.mIndices[i];
                globalForce(3 * vert_i + 0) += force(3 * i + 0);
                globalForce(3 * vert_i + 1) += force(3 * i + 1);
                globalForce(3 * vert_i + 2) += force(3 * i + 2);
            }
        }

        return globalForce;
    }

    bool ImplicitUpdate(TetraGroup& group, float timestep, bool saveFrame, IronGames::SimulationFrame* frame, float deltaVThreshold)
    {
        size_t numVertices = group.mVertices.size();
        Eigen::SparseMatrix<float> globalA(3* numVertices, 3* numVertices);
        Eigen::VectorXf globalB = Eigen::VectorXf::Zero(3 * numVertices);
		Eigen::VectorXf globalX = Eigen::VectorXf::Zero(3 * numVertices);
        Vec12 nodeVelocities = Vec12::Zero();
        Vec12 localB = Vec12::Zero();

        Eigen::VectorXf globalForce = Eigen::VectorXf::Zero(3 * numVertices);
        Eigen::VectorXf globalVelocities = Eigen::VectorXf::Zero(3 * numVertices);
        Eigen::SparseMatrix<float> globalDfDx(3 * numVertices, 3 * numVertices);
        Eigen::SparseMatrix<float> massMatrix(3 * numVertices, 3 * numVertices);

        //std::cout << "timestep " << timestep << std::endl;

        for (size_t i = 0; i < group.mVertices.size(); i++)
        {
            globalA.coeffRef(3 * i, 3 * i) = group.mVertices[i].mMass;
            globalA.coeffRef(3 * i + 1, 3 * i + 1) = group.mVertices[i].mMass;
			globalA.coeffRef(3 * i + 2, 3 * i + 2) = group.mVertices[i].mMass;
        }

        for (const auto& pair : group.mIdToTetrahedra)
        {
            const auto& tet = pair.second;

            auto ds = Calc_Ds(tet, group.mVertices, false);
            //std::cout << "ds" << std::endl;
            //std::cout << ds << std::endl;
            auto dmInv = Calc_DmInv(tet, group.mVertices);
            //std::cout << "dmInv" << std::endl;
            //std::cout << dmInv << std::endl;
            auto f = Calc_F(ds, dmInv);
            //std::cout << "f" << std::endl;
            //std::cout << f << std::endl;

            // Stable Neo-hookean potential
			auto j = f.determinant();
			auto dj_df = Calc_dj_df(f); // SEEMS OK
            //std::cout << "dj_df" << std::endl;
            //std::cout << dj_df << std::endl;
			auto g_j = Reshape3x3(dj_df);
            //auto dPsi_dF = group.mMu * Reshape3x3(f) + group.mLambda * (j - 1.0f - group.mMu/group.mLambda) * g_j;
            Mat3 dPsi_dF = group.mMu * f + (group.mLambda * (j - 1.0f) - group.mMu) * dj_df;
            //auto elementForce = dPsi_dF * CalcBm(tet.mIndices, group.mVertices);

			auto dF_dx = Calc_dFdx(dmInv);
            //std::cout << "dfdx" << std::endl;
            //std::cout << dF_dx << std::endl;
			auto dPsi_dx = dF_dx.transpose() * Reshape3x3(dPsi_dF);
			Vec12 force = -tet.mRestVolume * dPsi_dx; // 12x1 atm

            //std::cout << "Force" << std::endl;
            //std::cout << force << std::endl;
            //force = -Reshape3x4(elementForce); // 12x1 atm

			auto h_j = Calc_h_j(f);
            //std::cout << "h_j" << std::endl;
            //std::cout << h_j << std::endl;
            //std::cout << "g_j * g_j^T" << std::endl;
            //std::cout << (g_j * g_j.transpose()) << std::endl;
			//Mat9 dPsi2_dF2 = group.mMu * Mat9::Identity() + group.mLambda * (j - 1.0f - group.mMu / group.mLambda) * h_j + group.mLambda * g_j * g_j.transpose();

            Eigen::JacobiSVD<Mat3, Eigen::NoQRPreconditioner> svd_f(f, Eigen::ComputeFullU | Eigen::ComputeFullV);
            auto sigmas = svd_f.singularValues();
            auto u = svd_f.matrixU();
            auto v = svd_f.matrixV();

            if (u.determinant() < 0.0)
            {
                u.col(0) *= -1.0;
                sigmas(0) *= -1.0;
            }
            if (v.determinant() < 0.0)
            {
                v.col(0) *= -1.0;
                sigmas(0) *= -1.0;
            }

            auto dPsi2_dF2 = CalcProjectedHessian(group.mMu, group.mLambda, f, u, v, sigmas);

			auto dfdx = Calc_dfdx(dF_dx, dPsi2_dF2, tet.mRestVolume);

            //std::cout << "dfdx" << std::endl;
            //std::cout << dfdx << std::endl;

            // Contribution to globalA
            // 12 x 12 matrix df/dx
            // the force on one of the 4 vertices is a 3-vector
            // the derivative of that with respect to the spatial coordinates of all four node spatial coordinates should give me a 3x12 matrix (?)
            // then we have that for each of the nodes
            // and we get a 12x12
            // so a single element tells me how the force in coordinate (x,y,z) on node i changes with respect to the (x,y,z) position of node j, makes sense
            // and a 3x3 block tells me how the force on node i changes with respect to the position of node j
            // so it's probably easier to consider the global matrix as n x n where elements are 3x3 rather than 3n x 3n with scalar elements.
            for (size_t i = 0; i < 4; i++)
            {
                auto vert_i = tet.mIndices[i];

                for (size_t j = 0; j < 4; j++)
                {
                    auto vert_j = tet.mIndices[j];

                    // we want to set a 3x3 block in globalA here from (3*vertId to 3*vertId + 2, 3*otherVertId to 3*otherVertId + 2)
                    // and we should draw from (3*i to 3*i + 2, 3*j to 3*j + 2)
                    for (size_t sub_i = 0; sub_i < 3; sub_i++)
                    {
                        for (size_t sub_j = 0; sub_j < 3; sub_j++)
                        {
                            globalA.coeffRef(3 * vert_i + sub_i, 3 * vert_j + sub_j) -= timestep * timestep * dfdx(3 * i + sub_i, 3 * j + sub_j);
                            globalDfDx.coeffRef(3 * vert_i + sub_i, 3 * vert_j + sub_j) += dfdx(3 * i + sub_i, 3 * j + sub_j);
                        }
                    }
                }
            }

            for (size_t i = 0; i < 4; i++)
            {
                nodeVelocities(3 * i + 0) = group.mVertices[tet.mIndices[i]].mVelocity.x;
                nodeVelocities(3 * i + 1) = group.mVertices[tet.mIndices[i]].mVelocity.y;
                nodeVelocities(3 * i + 2) = group.mVertices[tet.mIndices[i]].mVelocity.z;
            }

            localB = timestep * force + timestep * timestep * dfdx * nodeVelocities;

            if (isnan(localB.norm()))
                throw std::exception("nan detected in local forces");

            // Contribution to globalB
            for (size_t i = 0; i < 4; i++)
            {
                auto vert_i = tet.mIndices[i];
                globalB(3 * vert_i + 0) += localB(3 * i + 0);
                globalB(3 * vert_i + 1) += localB(3 * i + 1);
                globalB(3 * vert_i + 2) += localB(3 * i + 2);

                globalForce(3 * vert_i + 0) += force(3 * i + 0);
                globalForce(3 * vert_i + 1) += force(3 * i + 1);
                globalForce(3 * vert_i + 2) += force(3 * i + 2);
            }
        }

        // Apply additional per-node forces
        for (int i = 0; i < group.mVertices.size(); i++)
        {
            // Gravity
            globalB(3 * i + 1) += -9.8 * timestep * group.mVertices[i].mMass;

            // Collision forces
            //globalB(3 * i + 0) += group.mVertices[i].mForce.x;
            //globalB(3 * i + 1) += group.mVertices[i].mForce.y;
            //globalB(3 * i + 2) += group.mVertices[i].mForce.z;
        }

        if (isnan(globalA.norm()))
            throw std::exception("Nan detected.");

        if (isnan(globalB.norm()))
            throw std::exception("Nan detected.");

        //{
        //    auto denseGlobalA = globalA.toDense();

        //    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> solver(denseGlobalA);
        //    auto eigs = solver.eigenvalues();
        //    auto minVal = 1000.0f;
        //    for (int i = 0; i < eigs.size(); i++)
        //        minVal = std::min(minVal, eigs[i]);

        //    if (minVal < 0)
        //    {
        //        std::cout << "Timestep : " << timestep << std::endl;
        //        std::cout << "Negative eig" << std::endl;
        //    }
        //}

        //std::cout << "globalA" << std::endl;
        //std::cout << globalA << std::endl;

        //std::cout << "globalB" << std::endl;
        //std::cout << globalB << std::endl;

        // PCG Solver
        {
            Eigen::ConjugateGradient<Eigen::SparseMatrix<float>, Eigen::Lower | Eigen::Upper> solver;
            solver.compute(globalA);

            globalX = solver.solve(globalB);
            if (isnan(solver.error()))
                throw std::exception("Solver failed.");

            //std::cout << "globalX" << std::endl;
            //std::cout << globalX << std::endl;
            //std::cout << "globalX inf norm: " << globalX.lpNorm<Eigen::Infinity>() << std::endl;
            //if (globalX.lpNorm<Eigen::Infinity>() > deltaVThreshold)
            //    return false;
            // lets see, make the threshold value here 1

            std::string solverInfo;
            if (solver.info() == 0)
                solverInfo = std::string("Success");
            else if (solver.info() == 1)
                solverInfo = std::string("Numerical issue");
            else if (solver.info() == 2)
                solverInfo = std::string("No convergence");
            else if (solver.info() == 3)
                solverInfo = std::string("Invalid input");

            if (solver.info() != 0)
				std::cout << "Solver info : " << solverInfo << std::endl;
        }

        // Check error against nonlinear equation
        {
            for (int i = 0; i < numVertices; i++)
            {
                globalVelocities(3 * i + 0) = group.mVertices[i].mVelocity[0];
                globalVelocities(3 * i + 1) = group.mVertices[i].mVelocity[1];
                globalVelocities(3 * i + 2) = group.mVertices[i].mVelocity[2];
                massMatrix.coeffRef(3 * i, 3 * i) = 1.0/group.mVertices[i].mMass;
                massMatrix.coeffRef(3 * i + 1, 3 * i + 1) = 1.0/group.mVertices[i].mMass;
                massMatrix.coeffRef(3 * i + 2, 3 * i + 2) = 1.0/group.mVertices[i].mMass;
            }

            // dv = h M^-1 (f0 + df/dx * dx)
            auto dx = timestep * (globalVelocities + globalX);
            auto perturbedForce = ComputePerturbedForce(group, dx);
            auto residual = timestep * massMatrix * perturbedForce - globalX;
            // this isn't the nonlinear eq. unfortunately, we'd need to evaluate the force with new position
            // to get that
            std::cout << "timestep: " << timestep << std::endl;
            std::cout << "infinity error: " << residual.lpNorm<Eigen::Infinity>() << std::endl;
            std::cout << "l2 error: " << residual.norm() << std::endl;
            if (residual.lpNorm<Eigen::Infinity>() > 10.0)
            {
                return false;
            }
        }

        // SparseLU Solver
        //{
        //    Eigen::SparseLU<Eigen::SparseMatrix<float>, Eigen::COLAMDOrdering<int> >   solver;
        //    solver.analyzePattern(globalA);
        //    solver.factorize(globalA);
        //    globalX = solver.solve(globalB);

        //    if (isnan(globalX.norm()))
        //        throw std::exception("Solver failed.");
        //}

        // Integrate
        for (int i = 0; i < group.mVertices.size(); i++)
        {
            auto& vertex = group.mVertices[i];
            auto deltaV = glm::dvec3(globalX(3 * i + 0), globalX(3 * i + 1), globalX(3 * i + 2));

            if (saveFrame)
            {
                auto& vert = *frame->mutable_vertices(i);
                *vert.mutable_force() = ProtoConverter::Convert(deltaV);
            }

            vertex.mVelocity += deltaV;
            vertex.mPosition += (double)timestep * vertex.mVelocity;
        }

        return true;

        // things to try:
        // - 'solveWithGuess' and providing the previous timestep's change in velocities as the guess?
        // - other preconditioners, solvers
        // - the doc's have globalX = solver.solve(globalB) twice, so maybe it tries for a specified number of
        //   iterations each time and refines the solution.. ?
    }
}

////////////////////////////////////////////////////////////////////////////////