#include "KMC/integrals.hpp"
#include "KMC/lookup_table.hpp"
#include "catch.hpp"

#include <cmath>
#include <cstdio>
#include <iostream>
#include <string>

#include <boost/math/quadrature/gauss_kronrod.hpp>

constexpr double ABSTOL = 1e-3;
constexpr double RELTOL = 1e-2;

constexpr double REVERSEFAC = 10;
/**
 * EXPLAIN:
 * Reverse lookup on regions where the CDF is flat gives large error
 * But this is a rare event in simulations because this requires
 * the U01 rng to be very close to either 0 or 1
 * WARNING:
 * When spring is stiff and distPerp is large,
 * the reverse lookup error is very large
 * because function values are tiny and very flat.
 */

inline double absError(double a, double b) { return fabs(a - b); }

inline double relError(double a, double b) {
    return b < 1e-8 ? absError(a, b) : fabs((a - b) / b);
}

inline bool errorPass(double a, double b, double fac = 1) {
    return absError(a, b) < fac * ABSTOL || relError(a, b) < fac * RELTOL;
}

TEST_CASE("Lookup table test SOFT spring ", "[lookup]") {
    constexpr double errTol = 1e-2;
    LookupTable LUT;

    const double D = 0.024;
    const double alpha = 0.1 / (2 * 0.00411);
    const double freelength = 0.05;
    const double M = alpha * D * D;
    const double ell0 = freelength / D;
    LUT.Init(alpha, freelength, D);

    double distPerp = 0;
    distPerp = 0.2;
    // ("distPerp = 0.2 > D+ell0, single peaked")
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(relError(LUT.Lookup(distPerp, sbound * D),
                       D * integral(distPerp / D, 0, sbound, M, ell0)) <
              errTol);
    }
    // ("distPerp = 0.1 > D+ell0, single peaked")
    distPerp = 0.1;
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(relError(LUT.Lookup(distPerp, sbound * D),
                       D * integral(distPerp / D, 0, sbound, M, ell0)) <
              errTol);
    }
    // ("distPerp = 0.06 < D+ell0, double peaked")
    distPerp = 0.06;
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(relError(LUT.Lookup(distPerp, sbound * D),
                       D * integral(distPerp / D, 0, sbound, M, ell0)) <
              errTol);
    }
}

TEST_CASE("Lookup table test MEDIUM spring ", "[lookup]") {
    LookupTable LUT;

    const double D = 0.024;
    const double alpha = 1.0 / (2 * 0.00411);
    const double freelength = 0.05;
    const double M = alpha * D * D;
    const double ell0 = freelength / D;
    LUT.Init(alpha, freelength, D);

    double distPerp = 0;
    distPerp = 0.2;
    // ("distPerp = 0.2 > D+ell0, single peaked")
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(errorPass(LUT.Lookup(distPerp, sbound * D),
                        D * integral(distPerp / D, 0, sbound, M, ell0)));
    }
    // ("distPerp = 0.1 > D+ell0, single peaked")
    distPerp = 0.1;
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(errorPass(LUT.Lookup(distPerp, sbound * D),
                        D * integral(distPerp / D, 0, sbound, M, ell0)));
    }
    // ("distPerp = 0.06 < D+ell0, double peaked")
    distPerp = 0.06;
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(errorPass(LUT.Lookup(distPerp, sbound * D),
                        D * integral(distPerp / D, 0, sbound, M, ell0)));
    }
}

TEST_CASE("Lookup table test STIFF spring ", "[lookup]") {
    LookupTable LUT;

    const double D = 0.024;
    const double alpha = 10.0 / (2 * 0.00411);
    const double freelength = 0.05;
    const double M = alpha * D * D;
    const double ell0 = freelength / D;
    LUT.Init(alpha, freelength, D);

    double distPerp = 0;
    distPerp = 0.2;
    // ("distPerp = 0.2 > D+ell0, single peaked")
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(errorPass(LUT.Lookup(distPerp, sbound * D),
                        D * integral(distPerp / D, 0, sbound, M, ell0)));
    }
    // ("distPerp = 0.1 > D+ell0, single peaked")
    distPerp = 0.1;
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(errorPass(LUT.Lookup(distPerp, sbound * D),
                        D * integral(distPerp / D, 0, sbound, M, ell0)));
    }
    // ("distPerp = 0.06 < D+ell0, double peaked")
    distPerp = 0.06;
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(errorPass(LUT.Lookup(distPerp, sbound * D),
                        D * integral(distPerp / D, 0, sbound, M, ell0)));
    }
}

TEST_CASE("Lookup table test manual medium spring REL error", "[lookup]") {
    // integrated by mathematica
    LookupTable LUT;
    const double D = 0.024;
    constexpr double errTol = RELTOL;

    double distPerp = 0;
    LUT.Init(1.0 / (2 * 0.00411), 0.05, D);

    distPerp = 0.2;
    // ("distPerp = 0.2 > D+ell0, single peaked")
    CHECK(relError(LUT.Lookup(distPerp, 0.5 * D) / D, 0.0722077) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 1.0 * D) / D, 0.142839) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 1.5 * D) / D, 0.210412) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 2.0 * D) / D, 0.273623) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 3.0 * D) / D, 0.383039) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 4.0 * D) / D, 0.466375) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 5.0 * D) / D, 0.523889) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 6.0 * D) / D, 0.55967) < errTol);

    distPerp = 0.08;
    // "distPerp = 0.08 > D+ell0, single peaked"/D,
    CHECK(relError(LUT.Lookup(distPerp, 0.5 * D) / D, 0.497588) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 1.0 * D) / D, 0.993608) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 1.5 * D) / D, 1.48554) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 2.0 * D) / D, 1.96929) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 3.0 * D) / D, 2.88784) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 4.0 * D) / D, 3.6925) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 5.0 * D) / D, 4.3332) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 6.0 * D) / D, 4.78986) < errTol);

    distPerp = 0.06;
    // "distPerp = 0.06 < D+ell0, double peaked"/D,
    CHECK(relError(LUT.Lookup(distPerp, 0.5 * D) / D, 0.488864) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 1.0 * D) / D, 0.981139) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 1.5 * D) / D, 1.47815) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 2.0 * D) / D, 1.97788) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 3.0 * D) / D, 2.96052) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 4.0 * D) / D, 3.85857) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 5.0 * D) / D, 4.59864) < errTol);
    CHECK(relError(LUT.Lookup(distPerp, 6.0 * D) / D, 5.14058) < errTol);
}

TEST_CASE("REVERSE Lookup table test manual medium spring REL error",
          "[REVERSE lookup]") {
    // integrated by mathematica
    LookupTable LUT;
    const double D = 0.024;

    double distPerp = 0;
    LUT.Init(1.0 / (2 * 0.00411), 0.05, D);

    double tol = RELTOL * REVERSEFAC;

    distPerp = 0.1;
    // ("distPerp = 0.1 > D+ell0, single peaked")
    CHECK(relError(LUT.ReverseLookup(distPerp, D * 0) / D, 0.0) < tol);
    CHECK(relError(LUT.ReverseLookup(distPerp, D * 0.0460519) / D, 0.05) < tol);
    CHECK(relError(LUT.ReverseLookup(distPerp, D * 0.322128) / D, 0.35) < tol);
    CHECK(relError(LUT.ReverseLookup(distPerp, D * 0.459824) / D, 0.5) < tol);
    CHECK(relError(LUT.ReverseLookup(distPerp, D * 0.915356) / D, 1.0) < tol);
    CHECK(relError(LUT.ReverseLookup(distPerp, D * 1.36196) / D, 1.5) < tol);
    CHECK(relError(LUT.ReverseLookup(distPerp, D * 1.79446) / D, 2.0) < tol);
    CHECK(relError(LUT.ReverseLookup(distPerp, D * 2.20718) / D, 2.5) < tol);
    CHECK(relError(LUT.ReverseLookup(distPerp, D * 3.27015) / D, 4.0) < tol);
    CHECK(relError(LUT.ReverseLookup(distPerp, D * 3.79115) / D, 5.0) < tol);
    CHECK(relError(LUT.ReverseLookup(distPerp, D * 4.15242) / D, 6.0) < tol);
    // CHECK(relError(LUT.ReverseLookup(distPerp / D, 4.37561), 7.0) < tol);
}

TEST_CASE("REVERSE Lookup table test soft spring ", "[REVERSE lookup]") {
    LookupTable LUT;

    const double D = 0.024;
    const double alpha = 0.1 / (2 * 0.00411);
    const double freelength = 0.05;
    const double M = alpha * D * D;
    const double ell0 = freelength / D;
    LUT.Init(alpha, freelength, D);

    double distPerp = 0;
    distPerp = 0.2;
    // ("distPerp = 0.2 > D+ell0, single peaked")
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 3; sbound += 0.2) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        CHECK(errorPass(LUT.ReverseLookup(distPerp, val * D), sbound * D,
                        REVERSEFAC));
    }
    // ("distPerp = 0.1 > D+ell0, single peaked")
    distPerp = 0.1;
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 3; sbound += 0.2) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        CHECK(errorPass(LUT.ReverseLookup(distPerp, val * D), sbound * D,
                        REVERSEFAC));
    }
    // ("distPerp = 0.06 < D+ell0, double peaked")
    distPerp = 0.06;
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 3; sbound += 0.2) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        CHECK(errorPass(LUT.ReverseLookup(distPerp, val * D), sbound * D,
                        REVERSEFAC));
    }
}

TEST_CASE("REVERSE Lookup table test medium spring ", "[REVERSE lookup]") {
    LookupTable LUT;

    const double D = 0.024;
    const double alpha = 1.0 / (2 * 0.00411);
    const double freelength = 0.05;
    const double M = alpha * D * D;
    const double ell0 = freelength / D;
    LUT.Init(alpha, freelength, D);

    double distPerp = 0;
    distPerp = 0.2;
    // ("distPerp = 0.2 > D+ell0, single peaked")
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 4; sbound += 0.2) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        CHECK(errorPass(LUT.ReverseLookup(distPerp, val * D), sbound * D,
                        REVERSEFAC));
    }
    // ("distPerp = 0.1 > D+ell0, single peaked")
    distPerp = 0.1;
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 3; sbound += 0.2) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        CHECK(errorPass(LUT.ReverseLookup(distPerp, val * D), sbound * D,
                        REVERSEFAC));
    }
    // ("distPerp = 0.06 < D+ell0, double peaked")
    distPerp = 0.06;
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 3; sbound += 0.2) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        CHECK(errorPass(LUT.ReverseLookup(distPerp, val * D), sbound * D,
                        REVERSEFAC));
    }
}

TEST_CASE("REVERSE Lookup table test stiff spring ", "[REVERSE lookup]") {
    LookupTable LUT;

    const double D = 0.024;
    const double alpha = 10.0 / (2 * 0.00411);
    const double freelength = 0.05;
    const double M = alpha * D * D;
    const double ell0 = freelength / D;
    LUT.Init(alpha, freelength, D);

    double distPerp = 0;
    distPerp = 0.2;
    // ("distPerp = 0.2 > D+ell0, single peaked")
    // WARNING: This reverse lookup fails
    // for (double sbound = 0; sbound < 3; sbound += 0.1) {
    //     double val = integral(distPerp / D, 0, sbound, M, ell0);
    //     CHECK(errorPass(LUT.ReverseLookup(distPerp / D, val), sbound,
    //                     REVERSEFAC));
    // }
    // ("distPerp = 0.1 > D+ell0, single peaked")
    distPerp = 0.1;
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 4; sbound += 0.1) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        CHECK(errorPass(LUT.ReverseLookup(distPerp, val * D), sbound * D,
                        REVERSEFAC));
    }
    // ("distPerp = 0.06 < D+ell0, double peaked")
    distPerp = 0.06;
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 4; sbound += 0.1) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        CHECK(errorPass(LUT.ReverseLookup(distPerp, val * D), sbound * D,
                        REVERSEFAC));
    }
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/xfeatures2d.hpp>
#include <opencv2/xfeatures2d/nonfree.hpp>
#include <opencv2/opencv.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>
#include <sophus/se3.hpp>
#include <Eigen/Core>

#include <chrono>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <cmath>
#include<math.h>
using namespace std;
using namespace cv;
//AVERAGE
double Average(vector<double> v)
{      double sum=0;
       for(int i=0;i<v.size();i++)
               sum+=v[i];
       return sum/v.size();
}
//DEVIATION
double Deviation(vector<double> v, double ave)
{
       double E=0;
       for(int i=0;i<v.size();i++){
               E+=(v[i] - ave)*(v[i] - ave);
       }
       return sqrt(E/v.size());
}
void writeResults( const string& filename, const vector<string>& timestamps, const vector<Mat>& Rt )
{
    CV_Assert( timestamps.size() == Rt.size() );

    ofstream file( filename.c_str() );
    if( !file.is_open() )
        return;

    cout.precision(4);
    for( size_t i = 0; i < Rt.size(); i++ )
    {
        const Mat& Rt_curr = Rt[i];
        if( Rt_curr.empty() )
            continue;

        CV_Assert( Rt_curr.type() == CV_64FC1 );

        Mat R = Rt_curr(Rect(0,0,3,3)), rvec;
        Rodrigues(R, rvec);
        double alpha = norm( rvec );
        if(alpha > DBL_MIN)
            rvec = rvec / alpha;

        double cos_alpha2 = std::cos(0.5 * alpha);
        double sin_alpha2 = std::sin(0.5 * alpha);

        rvec *= sin_alpha2;

        CV_Assert( rvec.type() == CV_64FC1 );
        // timestamp tx ty tz qx qy qz qw
        file << timestamps[i] << " " << fixed
             << Rt_curr.at<double>(0,3) << " " << Rt_curr.at<double>(1,3) << " " << Rt_curr.at<double>(2,3) << " "
             << rvec.at<double>(0) << " " << rvec.at<double>(1) << " " << rvec.at<double>(2) << " " << cos_alpha2 << endl;

    }
    file.close();
}

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

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

// BA by g2o
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

Mat bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose,
  const string& filename,
  const vector<string>& timestamps
);

// BA by gauss-newton
Mat bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose,
  int mode
);
double calc_residual(
   const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  Sophus::SE3d &pose,
  const Mat &K,
  vector<double>& residuals
);
int main(int argc, char **argv) {
	if(argc != 4){
        cout << "Format: file_with_rgb_depth_pairs trajectory_file odometry_name [Rgbd or ICP or RgbdICP or FastICP]" << endl;
        return -1;
   }

   vector<string> timestamps;
   vector<Mat> Rts, Rts_ba;
   Rts.push_back(Mat::eye(4,4,CV_64FC1));
   Rts_ba.push_back(Mat::eye(4,4,CV_64FC1));
   const string filename = argv[1];
   ifstream file( filename.c_str() );
   if( !file.is_open() )
      return -1;
   char dlmrt = '/';
   size_t pos = filename.rfind(dlmrt);
   string dirname = pos == string::npos ? "" : filename.substr(0, pos) + dlmrt;

   const int timestampLength = 17;
   const int rgbPathLehgth = 17+8;
   const int depthPathLehgth = 17+10;

   float fx = 517.3f, // default
         fy = 516.5f,
         cx = 318.6f,
         cy = 255.3f;
   // string value;
   // string inFileStr;
   // string myArray[10000];
   // int j = 0;

   // int arrSize = sizeof(myArray)/sizeof(myArray[0]);
   // cout << arrSize << endl;

   // cout << "first " << myArray[0] << endl;

   string datas[793];
   string str1;
   std::getline(file, str1);
	datas[0] = str1;
   string timestap3 = str1.substr(0, timestampLength);
   timestamps.push_back(timestap3);
   for(int i = 1; !file.eof(); i++)
   {
      string str;
      std::getline(file, str);
      datas[i] = str;
      if(str.empty()) break;
      if(str.at(0) == '#') continue; /* comment */
      cout << " previous image: " << datas[i-1] << "\n" << " current image "<< str << endl;
      Mat image, depth, image1, depth1, image2, depth2;
      if(i > 2) {
         string rgbFilename2 = datas[i-2].substr(timestampLength + 1, rgbPathLehgth );
         string timestap2 = datas[i-2].substr(0, timestampLength);
         string depthFilename2 = datas[i-2].substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );

         image2 = imread(dirname + rgbFilename2);
         depth2 = imread(dirname + depthFilename2, -1);
      }

	   string rgbFilename1 = datas[i-1].substr(timestampLength + 1, rgbPathLehgth );
      string timestap1 = datas[i-1].substr(0, timestampLength);
      string depthFilename1 = datas[i-1].substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
      image1 = imread(dirname + rgbFilename1);
      depth1 = imread(dirname + depthFilename1, -1);

	   string rgbFilename = str.substr(timestampLength + 1, rgbPathLehgth );
      string timestap = str.substr(0, timestampLength);
      string depthFilename = str.substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
      image = imread(dirname + rgbFilename);
      depth = imread(dirname + depthFilename, -1);
      // cout << "height " << image.rows << " width " << image.cols << endl; // 480 * 640
      // cout << "prev prev " << datas[i-2] << " previous image: " << datas[i-1] << " current image "<< str << endl;
      CV_Assert(!image.empty());
      CV_Assert(!depth.empty());
      CV_Assert(!image1.empty());
      CV_Assert(!depth1.empty());
      CV_Assert(depth.type() == CV_16UC1);
      CV_Assert(depth1.type() == CV_16UC1);

      if(i > 2){
         CV_Assert(!image2.empty());
         CV_Assert(!depth2.empty());
         CV_Assert(depth2.type() == CV_16UC1);
      }
      //   //-- 读取图像
      //   Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
      //   Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
      //   assert(img_1.data && img_2.data && "Can not load images!");

      std::vector<KeyPoint> keypoints_1, keypoints_2, key1, key2;
      vector<DMatch> matches;
      Ptr<FeatureDetector> detector = ORB::create();
      detector->detect(image1, key1);
      detector->detect(image, key2);
      // if(key1.size() == 0 || key2.size() == 0){
      //    find_feature_matches_another(image1, image, keypoints_1, keypoints_2, matches, image2);
      //    cout << "第二個: " <<  "一共找到了" << matches.size() << "组匹配点" << endl;
      // }
      // else{
         find_feature_matches(image1, image, keypoints_1, keypoints_2, matches, image2);
         // find_feature_matches(image1, image, keypoints_1, keypoints_2, matches, image2);
         cout << "第一個: " <<"一共找到了" << matches.size() << "组匹配点" << endl;
      // }

      // 建立3D点
      //Mat d1 = imread(depth1, IMREAD_UNCHANGED);       // 深度图为16位无符号数，单通道图像
      Mat K = (Mat_<double>(3, 3) << 517.3f, 0, 318.6f, 0, 516.5f, 255.3f, 0, 0, 1);
      vector<Point3f> pts_3d;
      vector<Point2f> pts_2d;
      std::vector<KeyPoint> keys1, keys2;
      std::vector<cv::Point2f> points1, points2;
      int index = 1;
      for (DMatch m:matches) {
         // if (index <keypoints_2.size()){
            ushort d = depth1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
            // cout << "depth " << d << endl;
            if (d == 0){   // bad depth
               continue;
               // d = 1;
            }
            // cout << "this is matches: " <<  index << endl;
            float dd = d / 5000.0;

            // cout << "keypoints_1[m.queryIdx].pt " << keypoints_1[m.queryIdx].pt << " keypoints_2[m.trainIdx].pt " << keypoints_2[m.trainIdx].pt << " depth " << d << endl;
            Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
            keys1.push_back(keypoints_1[m.queryIdx]);
            keys2.push_back(keypoints_2[m.trainIdx]);
            points1.push_back(keypoints_1[m.queryIdx].pt);
            points2.push_back(keypoints_2[m.trainIdx].pt);
            // cout << "p1.x " << p1.x << " p1.y " << p1.y << " depth " << dd << endl;

            pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
            pts_2d.push_back(keypoints_2[m.trainIdx].pt);
            index += 1;
         // }
      }
      bool b = false;
      // if(i ==373) {b = true;}
      cout << "3d-2d pairs: " << pts_3d.size() << " " << pts_2d.size() << " "  << i <<  endl;
      keypoints_1.clear();
      keypoints_1 = keys1;
      keypoints_2.clear();
      keypoints_2 = keys2;
      chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
      Mat r, t, inliers;
      solvePnPRansac(pts_3d, pts_2d, K, Mat(), r, t, b, 1000, 6.0, 0.99, inliers, SOLVEPNP_ITERATIVE);
      // cout << inliers << inliers.size()  << inliers.at<int>(1,0)  << inliers.at<int>(3,0)<< endl;
      for (int i=0; i<inliers.rows; i++){
         cout << "inliers -> keypoints_1: " << keypoints_1[inliers.at<int>(i,0)].pt << "inliers -> keypoints_2: " << keypoints_2[inliers.at<int>(i,0)].pt << endl;
      }
      // cout << "inliers "<< inliers << endl;
      cout<<"pnp OK = "<<b<<", inliers point num = "<<inliers.rows<<endl;
      vector<Scalar> colors;
      RNG rng;
      for(int j = 0; j < 100; j++){
         int r = rng.uniform(0, 256);
         int g = rng.uniform(0, 256);
         int b = rng.uniform(0, 256);
         colors.push_back(Scalar(r,g,b));
      }
      for(int j=0; j<inliers.rows; j++){
         circle(image1, keys1[inliers.at<int>(j,0)].pt, 5, colors[j], -1);
         circle(image, keys2[inliers.at<int>(j,0)].pt, 5, colors[j], -1);
      }

      Mat combined_img;
      hconcat(image1,image,combined_img);
      imwrite("./images/" + to_string(i) + ".jpg", combined_img);
      // imwrite("./images/" + to_string(i) + ".jpg", image);
      Mat R;
      cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵
      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 << "R=" << endl << R << endl;
      cout << "t=" << endl << t << endl;
      timestamps.push_back( timestap );
      Mat output;
      hconcat(R, t, output);

      Mat Rt = Mat::eye(4,4,CV_64FC1);
      Rt.at<double>(0,0) = R.at<double>(0,0);
      Rt.at<double>(0,1) = R.at<double>(0,1);
      Rt.at<double>(0,2) = R.at<double>(0,2);
      Rt.at<double>(1,0) = R.at<double>(1,0);
      Rt.at<double>(1,1) = R.at<double>(1,1);
      Rt.at<double>(1,2) = R.at<double>(1,2);
      Rt.at<double>(2,0) = R.at<double>(2,0);
      Rt.at<double>(2,1) = R.at<double>(2,1);
      Rt.at<double>(2,2) = R.at<double>(2,2);
      Rt.at<double>(0,3) = t.at<double>(0,0);
      Rt.at<double>(1,3) = t.at<double>(0,1);
      Rt.at<double>(2,3) = t.at<double>(0,2);

      Mat& prevRt = *Rts.rbegin();
      cout << "prevRt " << prevRt << endl;
      cout << "Rt " << Rt << endl;

      // for (int l=0; l<2; l++){
      //    cout << pts_3d[l].x << endl;
      // }
      // for (int i=0; i<inliers.rows; i++){
      //    cout << "inliers -> keypoints_1: " << keypoints_1[inliers.at<int>(i,0)].pt << "inliers -> keypoints_2: " << keypoints_2[inliers.at<int>(i,0)].pt << endl;
      // }
      for (int i=0; i<inliers.rows; i++){
         Mat m( 4,1, CV_64FC1);
         m.at<double>(0,0) = keypoints_1[inliers.at<int>(i,0)].pt.x;
         m.at<double>(1,0) = keypoints_1[inliers.at<int>(i,0)].pt.y;
         m.at<double>(2,0) = pts_3d[i].z;
         m.at<double>(3,0) = 1;
         // cout << "original " << m.t() << " projected " << (prevRt * Rt * m).t() << "  " << "actual " << keypoints_2[inliers.at<int>(i,0)].pt<< endl;
      }
      Rts.push_back(prevRt * Rt);
      Mat Rt_ba;
      VecVector3d pts_3d_eigen;
      VecVector2d pts_2d_eigen;
      for (size_t i = 0; i < pts_3d.size(); ++i) {
         // cout << "vector3d " << Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z) << "vector2d " << Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y) << endl;
         pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
         pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
      }
      // for (size_t i = 0; i < pts_3d.size(); ++i) {
      //    for (size_t j = 0; j < 3; ++j) {
      //       cout << " eigen3d " << pts_3d_eigen[i][j] << cout << " eigen2d "  <<*pts_2d_eigen[i][j];
      //    }
      // }
      // cout << "calling bundle adjustment by g2o" << endl;
      // Sophus::SE3d pose_g2o;
      // t1 = chrono::steady_clock::now();
      // Rt_ba = bundleAdjustmentG2O(pts_3d_eigen, pts_2d_eigen, K, pose_g2o, argv[2], timestamps);
      // Mat& prevRtba = *Rts_ba.rbegin();
      // Rts_ba.push_back(prevRtba * Rt_ba);
      // t2 = chrono::steady_clock::now();
      // time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
      // cout << "solve pnp by g2o cost time: " << time_used.count() << " seconds." << endl;
     cout << "calling bundle adjustment by gauss newton" << endl;
     Sophus::SE3d pose_gn;
     t1 = chrono::steady_clock::now();
     int mode = 0; // 0=huber
     Mat Rt_baGauss = bundleAdjustmentGaussNewton(pts_3d_eigen, pts_2d_eigen, K, pose_gn, mode);
      Mat& prevRtbaGauss = *Rts_ba.rbegin();
      cout << "prevRt " << prevRtbaGauss << endl;
      cout << "Rt " << RtGauss << endl;
      Rts_ba.push_back(prevRtbaGauss * Rt_baGauss);
     t2 = chrono::steady_clock::now();
     time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
     cout << "solve pnp by gauss newton cost time: " << time_used.count() << " seconds." << endl;

   }
   // writeResults(argv[2], timestamps, Rts);
   writeResults(argv[2], timestamps, Rts_ba);

  return 0;
}
void find_feature_matches_another(const Mat &img_1, const Mat &img_2,
                           std::vector<KeyPoint> &keypoints_1,
			                  std::vector<KeyPoint> &keypoints_2,
                           std::vector<DMatch> &matches,
                           const Mat &img_3) {
   Mat descriptors_1, descriptors_2, descriptors_3, descriptors_4;
   Ptr<FeatureDetector> detector = AgastFeatureDetector::create();
   Ptr<DescriptorExtractor> descriptor = AgastFeatureDetector::create();
   Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
   detector->detect(img_1, keypoints_1);
   detector->detect(img_2, keypoints_2);
   cout << "keypoints_1.size() "<< keypoints_1.size() << " keypoints_2.size() " << keypoints_2.size() << endl;
   // if(keypoints_1.size() != 0 && keypoints_2.size() != 0){
      for (int i=0; i< keypoints_1.size(); i++){
         // cout << "keypoint1 " << keypoints_1[i].pt << "keypoint2 " << keypoints_2[i].pt << endl;
      }
      descriptor->compute(img_1, keypoints_1, descriptors_1);
      descriptor->compute(img_2, keypoints_2, descriptors_2);
   // }
   int eee = descriptors_1.empty();
   int ddd = descriptors_2.empty();

   vector<DMatch> match;
   // Mat de1, de2;
   // if(ddd == true || eee == true){
   //    de1 = descriptors_3;
   //    de2 = descriptors_4;
   // }else {
   //    de1 = descriptors_1;
   //    de2 = descriptors_2;
   // }
   // matcher->match(de1, de2, match);
   matcher->match(descriptors_1, descriptors_2, match);
   double min_dist = 10000, max_dist = 0;
     for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
     if (dist < min_dist) min_dist = dist;
     if (dist > max_dist) max_dist = dist;
   }
   printf("-- Max dist : %f \n", max_dist);
   printf("-- Min dist : %f \n", min_dist);
   for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 10.0)) {
       matches.push_back(match[i]);
     }
   }
 }
 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,
                           const Mat &img_3) {

   Mat descriptors_1, descriptors_2, descriptors_3, descriptors_4;
   Ptr<FeatureDetector> detector = ORB::create();
   Ptr<DescriptorExtractor> descriptor = ORB::create();
   // Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
   Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("FlannBased");
   // cv::FlannBasedMatcher matcher = cv::FlannBasedMatcher(cv::makePtr<cv::flann::LshIndexParams>(12, 20, 2));
   detector->detect(img_1, keypoints_1);
   detector->detect(img_2, keypoints_2);
   cout << "keypoints_1.size() "<< keypoints_1.size() << " keypoints_2.size() " << keypoints_2.size() << endl;
   // if(keypoints_1.size() != 0 && keypoints_2.size() != 0){

      descriptor->compute(img_1, keypoints_1, descriptors_1);
      descriptor->compute(img_2, keypoints_2, descriptors_2);
   // }
   // cout << "des1 " << descriptors_1  << "des2 " << descriptors_2 << endl;
   int eee = descriptors_1.empty();
   int ddd = descriptors_2.empty();
   // for (int i=0; i< keypoints_1.size(); i++){
   //    cout << "keypoint1 " << keypoints_1[i].pt << "keypoint2 " << keypoints_2[i].pt << endl;
   // }
   // cout << "ddd " << ddd << " eee " << eee << endl;
   // if(ddd == true || eee == true){
   //    keypoints_1.clear();
   //    keypoints_2.clear();
   //    detector->detect( img_3, keypoints_1 );
   //    detector->detect( img_2, keypoints_2 );
   //    cout << "keypoints_3.size() "<< keypoints_1.size() << " keypoints_3.size() " << keypoints_2.size() << endl;
   //    for (int i=0; i< keypoints_1.size(); i++){
   //       cout << "keypoint3 " << keypoints_1[i].pt << "keypoint4 " << keypoints_2[i].pt << endl;
   //    }
   //    descriptor->compute( img_3, keypoints_1, descriptors_3 );
   //    descriptor->compute( img_2, keypoints_2, descriptors_4 );
   //    cout << "des3 " << descriptors_3  << "des4 " << descriptors_4 << endl;

   // }
   vector<DMatch> match;
   // BFMatcher matcher ( NORM_HAMMING );
   // Mat de1, de2;

   // if(ddd == true || eee == true){
   //    de1 = descriptors_3;
   //    de2 = descriptors_4;
   // }else {
   //    de1 = descriptors_1;
   //    de2 = descriptors_2;
   // }
   // matcher->match(de1, de2, match);

   // matcher->match(descriptors_1, descriptors_2, match);
   vector<vector<DMatch>> knn_matches;
   descriptors_1.convertTo(descriptors_1, CV_32F);
   descriptors_2.convertTo(descriptors_2, CV_32F);
   matcher->knnMatch( descriptors_1, descriptors_2, knn_matches, 2);
   // descriptors_1.convertTo(descriptors_1, CV_8UC1);
   // descriptors_2.convertTo(descriptors_2, CV_8UC1);
   // matcher.knnMatch( descriptors_1, descriptors_2, knn_matches, 2);
   // for (size_t k = 0; k < knn_matches.size(); k++)
   //  {
   //      for (size_t i = 0; i < knn_matches[k].size(); i++)
   //      {
   //          // const cv::DMatch& match = knn_matches[k][i];
   //          cout << knn_matches[i][k].trainIdx << " ";
   //      }
   //      cout << endl;
   //  }
    const float ratio_thresh = 0.7f;
   //  std::vector<DMatch> good_matches;
    for (size_t i = 0; i < knn_matches.size(); i++)
    {
        if (knn_matches[i][0].distance < ratio_thresh * knn_matches[i][1].distance)
        {
            matches.push_back(knn_matches[i][0]);
        }
    }
   // double min_dist = 10000, max_dist = 0;
   // for (int i = 0; i < descriptors_1.rows; i++) {
   //    double dist = match[i].distance;
   //    if (dist < min_dist) min_dist = dist;
   //    if (dist > max_dist) max_dist = dist;
   // }
   // cout << match.size() << endl;
   // printf("-- Max dist : %f \n", max_dist);
   // printf("-- Min dist : %f \n", min_dist);
   // for (int i = 0; i < match.size(); i++) {
   //  if (match[i].distance <= max(2 * min_dist, 10.0)) {
   //    // if (matches[i].distance < 2 * min_dist) {
   //       matches.push_back(match[i]);
   //   }
   // }

 }

 Point2d pixel2cam(const Point2d &p, const Mat &K) {
   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)
     );
 }
double calc_residual(
   const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  Sophus::SE3d &pose,
  const Mat &K,
  vector<double>& residuals
){
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);
  vector<double> res_std;
  for (int i=0; i<points_3d.size(); i++){
     Eigen::Vector3d pc = pose * points_3d[i];
     Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);
     Eigen::Vector2d error = points_2d[i] - proj;
     residuals.push_back(error.squaredNorm());
      if (isnan(pc[2]) == false) {
         res_std.push_back(error.squaredNorm());
      }
  }
  double avg = Average(res_std);
  double std = Deviation(res_std,avg);
  return std;
}
Mat bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose,
  int mode) {
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  const int iterations = 100;
  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);
   vector<double> residuals;
   double res_std = calc_residual(points_3d, points_2d, pose, K, residuals);
   // for(int i=0; i < residuals.size(); i++) {
   //    cout << residuals.at(i) << endl;
   // }
   cout << "deviation: " << res_std << endl;
   double huber_k = 1.345 * res_std;
   vector<double> weight;
   if(mode == 0){
      for (int j=0; j<residuals.size(); j++){
         if(residuals[j] <= huber_k){
            weight.push_back(1.0);
         }else {
            weight.push_back(huber_k/residuals[j]);
         }
      }
   }else {
      for (int j=0; j<residuals.size(); j++){
         weight.push_back(0.0);
      }
   }

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

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

      Eigen::Vector2d e = points_2d[i] - proj;

      cost += e.squaredNorm();
      Eigen::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 * weight[i]);
      b += -J.transpose() * (e * weight[i]);
    }

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

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

    if (iter > 0 && cost >= lastCost) {
      // cost increase, update is not good
      cout << "cost: " << cost << ", last cost: " << lastCost << endl;
      break;
    }

    // update your estimation
    pose = Sophus::SE3d::exp(dx) * pose;
    lastCost = cost;

    cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;
    if (dx.norm() < 1e-6) {
      // converge
      break;
    }

  }
   Mat Rt = Mat::eye(4,4,CV_64FC1);
      Rt.at<double>(0,0) = pose.matrix()(0);
      Rt.at<double>(1,0) = pose.matrix()(1);
      Rt.at<double>(2,0) = pose.matrix()(2);
      Rt.at<double>(3,0) = pose.matrix()(3);
      Rt.at<double>(0,1) = pose.matrix()(4);
      Rt.at<double>(1,1) = pose.matrix()(5);
      Rt.at<double>(2,1) = pose.matrix()(6);
      Rt.at<double>(3,1) = pose.matrix()(7);
      Rt.at<double>(0,2) = pose.matrix()(8);
      Rt.at<double>(1,2) = pose.matrix()(9);
      Rt.at<double>(2,2) = pose.matrix()(10);
      Rt.at<double>(3,2) = pose.matrix()(11);
      Rt.at<double>(0,3) = pose.matrix()(12);
      Rt.at<double>(1,3) = pose.matrix()(13);
      Rt.at<double>(2,3) = pose.matrix()(14);
      Rt.at<double>(3,3) = pose.matrix()(15);

      cout << "pose by g-n: \n" << pose.matrix() << endl;

      return Rt;
}

/// vertex and edges used in g2o ba
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
   public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

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

   /// left multiplication on SE3
   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, Eigen::Vector2d, VertexPose> {
   public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

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

   virtual void computeError() override {
      const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
      Sophus::SE3d T = v->estimate();
      Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
      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();
      Eigen::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;
};

Mat bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose,
  const string& filename,
  const vector<string>& timestamps) {

  // 构建图优化，先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  // pose is 6, landmark is 3
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
  g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

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

  // K
  Eigen::Matrix3d K_eigen;
  K_eigen <<
          K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
    K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
    K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);

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

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.setVerbose(true);
  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;
  cout << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;
//   cout << "pose matrix "  << vertex_pose->estimate().matrix()(0) << vertex_pose->estimate().matrix()(1) << vertex_pose->estimate().matrix()(4) << endl;

  Mat Rt = Mat::eye(4,4,CV_64FC1);
  Rt.at<double>(0,0) = vertex_pose->estimate().matrix()(0);
  Rt.at<double>(1,0) = vertex_pose->estimate().matrix()(1);
  Rt.at<double>(2,0) = vertex_pose->estimate().matrix()(2);
  Rt.at<double>(3,0) = vertex_pose->estimate().matrix()(3);
  Rt.at<double>(0,1) = vertex_pose->estimate().matrix()(4);
  Rt.at<double>(1,1) = vertex_pose->estimate().matrix()(5);
  Rt.at<double>(2,1) = vertex_pose->estimate().matrix()(6);
  Rt.at<double>(3,1) = vertex_pose->estimate().matrix()(7);
  Rt.at<double>(0,2) = vertex_pose->estimate().matrix()(8);
  Rt.at<double>(1,2) = vertex_pose->estimate().matrix()(9);
  Rt.at<double>(2,2) = vertex_pose->estimate().matrix()(10);
  Rt.at<double>(3,2) = vertex_pose->estimate().matrix()(11);
  Rt.at<double>(0,3) = vertex_pose->estimate().matrix()(12);
  Rt.at<double>(1,3) = vertex_pose->estimate().matrix()(13);
  Rt.at<double>(2,3) = vertex_pose->estimate().matrix()(14);
  Rt.at<double>(3,3) = vertex_pose->estimate().matrix()(15);

  pose = vertex_pose->estimate();
  return Rt;
}


// Compute and store ridgelet coefficients in linearized form
// Input: fsolution*h5 file(s) computed with bte_omp_ftcg executable

#include <hdf5.h>
#include <omp.h>
#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <cmath>
#include <cstdio>
#include <iostream>
#include <memory>
#include <regex>
#include <stdexcept>
#include <string>
#include <tuple>

#include "base/eigen2hdf.hpp"
#include "base/exceptions.hpp"
#include "brt_config.h"
#include "fft/fft2_r2c.hpp"
#include "ridgelet/rc_linearize.hpp"
#include "ridgelet/ridgelet_cell_array.hpp"
#include "ridgelet/ridgelet_frame.hpp"
#include "ridgelet/rt.hpp"
#include "spectral/quadrature/qhermitew.hpp"

typedef FFTr2c<PlannerR2C> fft_t;
// typedef FFTr2c<PlannerR2COD> fft_t;

unsigned int PLANNER_STRATEGY = FFTW_MEASURE;

typedef RT<double, RidgeletFrame, fft_t> RT_t;
typedef RT_t::rt_coeff_t rt_coeff_t;
typedef RidgeletCellArray<rt_coeff_t> rca_t;

namespace po = boost::program_options;

std::string out_fname = "rt_coeffs.h5";

int get_frame(const std::string& fname)
{
  std::regex my_regex(".*solution_vector([0-9]*)");
  std::smatch match;
  bool found = std::regex_search(fname, match, my_regex);
  if (found) {
    return atoi(match[1].str().c_str());
  } else {
    throw std::runtime_error("could not find frame number");
  }
}

unsigned int get_ncoeffs(const RidgeletFrame& rf)
{
  unsigned int nc = 0;
  for (auto& lam : rf.lambdas()) {
    auto t = tgrid_dim(lam, rf);
    unsigned int TX = std::get<0>(t) * std::get<1>(t);
    nc += TX;
  }
  return nc;
}

bool is_power_of_two(int x)
{
  return (x>0) && !(x & (x-1));
}

int main(int argc, char* argv[])
{
  std::cout << "SOURCE::INFO::GIT_BRANCHNAME " << GIT_BNAME << "\n";
  std::cout << "SOURCE::INFO::GIT_SHA1       " << GIT_SHA1 << "\n";

  // disable buffering of printf
  std::setbuf(stdout, NULL);

  po::options_description options("options");
  options.add_options()
      ("help", "produce help message")
      ("dst", po::value<std::string>(), "write output to dst");

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, options), vm);
    po::notify(vm);
  } catch (std::exception& e) {
    if (vm.count("help"))
      std::cout << options << "\n";
    else
      std::cout << e.what() << "\n";
    return 1;
  }
  if (vm.count("help")) {
    std::cout << options << "\n";
    return 0;
  }

  if (vm.count("dst")) {
    out_fname = vm["dst"].as<std::string>();
  }
  std::cerr << "writing output to " << out_fname << std::endl;

  typedef Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> array_t;
  // read input files from stdin and check if they exist
  std::string input_line;
  std::vector<std::string> fnames;
  while (std::cin) {
    std::getline(std::cin, input_line);
    // check for empty (e.g. terminating line)
    std::regex my_regex("^([[:space:]]*|)$");
    std::smatch my_match;
    std::regex_search(input_line, my_match, my_regex);
    // cout << "match size: " << my_match.size() << "\n";
    if (my_match.size() > 0) {
      break;
    } else {
      if (boost::filesystem::is_regular_file(input_line) ||
          boost::filesystem::is_symlink(input_line))
        fnames.push_back(input_line);
      else
        throw std::runtime_error("File " + input_line + " does not exist. Exiting.");
    }
  };

  std::cout << "Found " << fnames.size() << " input files\n";

  // load files
  std::shared_ptr<RidgeletFrame> rf_ptr;
  std::shared_ptr<RCLinearize> rcl_ptr;

  hid_t h5f_out = H5Fcreate(out_fname.c_str(), H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);

  for (auto& fname : fnames) {
    int frame = get_frame(fname);
    array_t coeffs;
    hid_t h5f = H5Fopen(fname.c_str(), H5F_ACC_RDONLY, H5P_DEFAULT);
    eigen2hdf::load(h5f, "C", coeffs);
    int N = coeffs.rows();
    int L = coeffs.cols();
    int K = std::sqrt(N);
    ASSERT(K * K == N);
    int Lx = std::sqrt(L);
    std::cout << "Lx: " << Lx << "\n";
    ASSERT(Lx * Lx == L);
    ASSERT(is_power_of_two(Lx));
    H5Fclose(h5f);

    if (!rf_ptr) {
      int J = std::log2(2 * Lx) - 2;
      std::cout << "J: "  << J << "\n";
      rf_ptr = std::make_shared<RidgeletFrame>(J, J, 1, 1);
      rcl_ptr = std::make_shared<RCLinearize>(*rf_ptr);
      fft_t fft;
      init_fftw(fft, PLANNER_STRATEGY, *rf_ptr);
    }
    fft_t fft;
    RT_t rt(*rf_ptr);

    typedef RT_t::complex_array_t complex_array_t;

    // store Ridgelet coefficients for each direction in the following array:
    typedef Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> out_array_t;

    int ncoeffs = get_ncoeffs(*rf_ptr);
    out_array_t RTCOEFFS(K * K, ncoeffs);

#pragma omp parallel
    {
      complex_array_t Fhh(Lx, Lx);
      complex_array_t Fh(2 * Lx, 2 * Lx);
      rca_t f_rc(*rf_ptr);

#pragma omp for schedule(dynamic) collapse(2)
      for (int j1 = 0; j1 < K; j1++) {
        for (int j2 = 0; j2 < K; j2++) {
          // read F..
          int idv = j2 * K + j1;
          Eigen::Map<array_t> F(coeffs.data() + L * idv, Lx, Lx);
          fft.ft(Fhh, F, false);
          Fh.setZero();
          ftcut(Fh, Lx, Lx) = Fhh;
          hf_zero(Fh);
          rt.rt(f_rc.coeffs(), Fh);

          std::vector<double> rclin = rcl_ptr->linearize(f_rc.coeffs());

          // valgrind complains about the following
          // std::copy(rclin.begin(), rclin.end(), RTCOEFFS.row(idv).data());
          Eigen::Map<Eigen::VectorXd> vrclin(rclin.data(), rclin.size());
          RTCOEFFS.row(idv) = vrclin;
        }  // end for j2
      }    // end for j1
    }      // end omp parallel

    // save to hdf
    eigen2hdf::save(h5f_out, std::to_string(frame), RTCOEFFS);

  }  // end for files
  H5Fclose(h5f_out);

  return 0;
}


#include <gnuplot-iostream/gnuplot-iostream.h>
#include <boost/tuple/tuple.hpp>
#include <utility>
#include <vector>
#include <string>
#include "Graph.hpp"
#include "Planet.hpp"
using namespace std;

/*****************************************************************************

GRAPHING PROGRAM! To graph both the system and the log-log plot

*****************************************************************************/

void graphSystem(vector<vec3D> paths[], int planets, int asts, int base, double scale, string fname){
	Gnuplot gp;

	double sz = scale*149597870700; // Scale from au to meters

	gp << setprecision(3);
	gp << "set xrange [" << -sz << ":" << sz << "]\n";
	gp << "set yrange [" << -sz << ":" << sz << "]\n";
	gp << "set zrange [" << -sz << ":" << sz << "]\n";
	gp << "set xlabel \"x\"\n";
	gp << "set ylabel \"y\"\n";
	gp << "set zlabel \"z\"\n";
	gp << "set term png size 1920,1080 font \"FreeSerif,12\"\n";
	gp << "set title \"ZOLAR ZYZDEM\"\n";
	gp << "set output \"" << fname << "\"\n";
	gp << "set hidden3d\n";
	gp << "set view 60, 60, 1, 1.5\n";
	gp << "splot ";

	for(int i = 0; i < planets+asts; i++){	// Plot planets in black and asteroids in red
		gp << "'-' with lines lc rgb \"";
		if(i >= planets){
			gp << "red";
		} else {
			gp << "black";
		}
		gp << "\" notitle, ";
	}
	gp << "\n";

	for(int i = 0; i < planets+asts; i++){			// Data must be rearranged into vectors of doubles, removed from vec3D tuple
		vector<double> x;
		vector<double> y;
		vector<double> z;

		for(int j = 0; j < paths[i].size(); j++){
			vec3D v = paths[i][j];
			x.push_back(v.x-paths[base][j].x);		// Adjust coordinates to be centered around the base (the sun)
			y.push_back(v.y-paths[base][j].y);
			z.push_back(v.z-paths[base][j].z);
		}

		gp.send1d(boost::make_tuple(x,y,z));
	}
}

void graphLogLog(vector<double> lSMA, vector<double> lPeriod){
	Gnuplot gp;

	auto line = getLeastSquares(lSMA,lPeriod);			// Get the least squares parameters (slope, intercept) from the Calcs.cpp file

	gp << "set xrange [20:35]\n";
	gp << "set yrange [0:30]\n";
	gp << "set xlabel \"log(SMA)\"\n";
	gp << "set ylabel \"log(period)\"\n";
	gp << "set term png size 720,480 font \"FreeSerif,12\"\n";
	gp << "set title \"Log-Log plot of Semi-Major Axis vs. Period\"\n";
	gp << "set output \"ll.png\"\n";
	gp << "f(x) = " << line.first << "*x + " << line.second << "\n"; // Defines a function for gnuplot to evaluate
	gp << "plot '-' with lines lc rgb \"black\" notitle, f(x) with lines title 'Trendline y = " << line.first << "*x + " << line.second << "'\n";
	gp.send1d(boost::make_tuple(lSMA,lPeriod));
}


// Copyright 2021 Yu-Kai Lin. All rights reserved.
// Use of this source code is governed by a BSD-style
// license that can be found in the LICENSE file.

#pragma once

#include "kcp/common.hpp"

#include <Eigen/Core>

#define _USE_MATH_DEFINES
#include <cmath>
#include <map>
#include <memory>
#include <stdexcept>
#include <vector>

#define deg2red(deg) ((deg)*M_PI / 180)
#define red2deg(red) ((red)*180 / M_PI)

#define MAX(A, B) ((A) > (B) ? A : B)
#define MIN(A, B) ((A) < (B) ? A : B)

#define l2Norm2D(x, y) sqrt(pow(x, 2) + pow(y, 2))
#define l2Norm3D(x, y, z) sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2))
#define _GET_L2NORM_MACRO(_1, _2, _3, NAME, ...) NAME
#define l2Norm(...)                                  \
  _GET_L2NORM_MACRO(__VA_ARGS__, l2Norm3D, l2Norm2D) \
  (__VA_ARGS__)

#define CYCLIC_INDEX(i, start, end) ((i < start) ? (end - (start - (i))) : ((i > end) ? start + (i - (end)) : i))

namespace kcp {

/**
 * @brief Get the set of k-closest-points correspondences with kd-tree.
 *
 * @param src The source point cloud.
 * @param dst The target point cloud.
 * @param src_feature The source feature cloud used to compute distances.
 * @param dst_feature The target feature cloud used to compute distances.
 * @param k The number of closest points for each source point.
 * @return Shared pointer to the set of correspondences.
 */
std::shared_ptr<Correspondences>
get_kcp_correspondences(const Eigen::MatrixX3d& src,
                        const Eigen::MatrixX3d& dst,
                        const Eigen::MatrixXd& src_feature,
                        const Eigen::MatrixXd& dst_feature,
                        size_t k);

};  // namespace kcp


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>

#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/Polygon_mesh_processing/IO/polygon_mesh_io.h>

#include <boost/iterator/function_output_iterator.hpp>
#include <boost/property_map/property_map.hpp>

#include <iostream>
#include <fstream>
#include <map>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::Point_3                                     Point;
typedef Kernel::Compare_dihedral_angle_3                    Compare_dihedral_angle_3;

typedef CGAL::Surface_mesh<Point>                           Mesh;

namespace PMP = CGAL::Polygon_mesh_processing;

template <typename G>
struct Constraint : public boost::put_get_helper<bool,Constraint<G> >
{
  typedef typename boost::graph_traits<G>::edge_descriptor edge_descriptor;
  typedef boost::readable_property_map_tag      category;
  typedef bool                                  value_type;
  typedef bool                                  reference;
  typedef edge_descriptor                       key_type;

  Constraint()
    :g_(NULL)
  {}

  Constraint(G& g, double bound)
    : g_(&g), bound_(bound)
  {}

  bool operator[](edge_descriptor e) const
  {
    const G& g = *g_;
    return compare_(g.point(source(e, g)),
                    g.point(target(e, g)),
                    g.point(target(next(halfedge(e, g), g), g)),
                    g.point(target(next(opposite(halfedge(e, g), g), g), g)),
                   bound_) == CGAL::SMALLER;
  }

  const G* g_;
  Compare_dihedral_angle_3 compare_;
  double bound_;
};

template <typename PM>
struct Put_true
{
  Put_true(const PM pm)
    :pm(pm)
  {}

  template <typename T>
  void operator()(const T& t)
  {
    put(pm, t, true);
  }

  PM pm;
};

int main(int argc, char* argv[])
{
  const std::string filename = (argc > 1) ? argv[1] : CGAL::data_file_path("meshes/blobby_3cc.off");

  Mesh mesh;
  if(!PMP::IO::read_polygon_mesh(filename, mesh))
  {
    std::cerr << "Invalid input." << std::endl;
    return 1;
  }

  typedef boost::graph_traits<Mesh>::face_descriptor face_descriptor;
  const double bound = std::cos(0.75 * CGAL_PI);

  std::vector<face_descriptor> cc;
  face_descriptor fd = *faces(mesh).first;
  PMP::connected_component(fd,
      mesh,
      std::back_inserter(cc));

  std::cerr << "Connected components without edge constraints" << std::endl;
  std::cerr << cc.size() << " faces in the CC of " << fd << std::endl;

  // Instead of writing the faces into a container, you can set a face property to true
  typedef Mesh::Property_map<face_descriptor, bool> F_select_map;
  F_select_map fselect_map =
    mesh.add_property_map<face_descriptor, bool>("f:select", false).first;
  PMP::connected_component(fd,
      mesh,
      boost::make_function_output_iterator(Put_true<F_select_map>(fselect_map)));

  std::cerr << "\nConnected components with edge constraints (dihedral angle < 3/4 pi)" << std::endl;
  Mesh::Property_map<face_descriptor, std::size_t> fccmap =
    mesh.add_property_map<face_descriptor, std::size_t>("f:CC").first;
  std::size_t num = PMP::connected_components(mesh,
      fccmap,
      PMP::parameters::edge_is_constrained_map(Constraint<Mesh>(mesh, bound)));

  std::cerr << "- The graph has " << num << " connected components (face connectivity)" << std::endl;
  typedef std::map<std::size_t/*index of CC*/, unsigned int/*nb*/> Components_size;
  Components_size nb_per_cc;
  for(face_descriptor f : faces(mesh)){
    nb_per_cc[ fccmap[f] ]++;
  }
  for(const Components_size::value_type& cc : nb_per_cc){
    std::cout << "\t CC #" << cc.first
              << " is made of " << cc.second << " faces" << std::endl;
  }

  std::cerr << "- We keep only components which have at least 4 faces" << std::endl;
  PMP::keep_large_connected_components(mesh,
      4,
      PMP::parameters::edge_is_constrained_map(Constraint<Mesh>(mesh, bound)));

  std::cerr << "- We keep the two largest components" << std::endl;
  PMP::keep_largest_connected_components(mesh,
      2,
      PMP::parameters::edge_is_constrained_map(Constraint<Mesh>(mesh, bound)));

  return 0;
}


#pragma once

#include <Eigen/Dense>

/**
 * Abstract class that should be inherited from to create specific
 * kalman filters
 *
 * Must initialize:
 *  x_k1_k1, x_k_k1, x_k_k,
 *  P_k1_k1, P_k_k1, P_k_k,
 *  F_k, B_k, H_k,
 *  Q_k, R_k
 *
 * Every predict, must update:
 *  u_k
 *
 * Every PredictWithUpdate, must update:
 *  u_k, z_k
 *
 * The most recently updated values
 *  x_k_k, P_k_k
 *
 * Taken from https://en.wikipedia.org/wiki/Kalman_filter
 *
 * Conversion between code notation and wiki notation is...
 * x_k1_k1 is X_(k-1, k-1)
 * x_k_k is X_(k, k)
 * etc
 */
class KalmanFilter {
public:
    /**
     * Creates a general kalman filter with the given sizes
     * Use a child class to setup the specific state matricies
     * Assumes 1 input
     *
     * @param stateSize The size of the state vector
     * @param observationSize The size of the observation vector
     */
    KalmanFilter(unsigned int stateSize, unsigned int observationSize) :
        x_k1_k1(stateSize), x_k_k1(stateSize), x_k_k(stateSize),
        u_k(1), z_k(observationSize),
        y_k_k1(observationSize), y_k_k(observationSize),
        P_k1_k1(stateSize, stateSize), P_k_k1(stateSize, stateSize), P_k_k(stateSize, stateSize),
        S_k(observationSize, observationSize), K_k(stateSize, observationSize),
        F_k(stateSize, stateSize), B_k(stateSize, 1), H_k(observationSize, stateSize),
        Q_k(stateSize, stateSize), R_k(observationSize, observationSize),
        I(Eigen::MatrixXd::Identity(stateSize, stateSize)) {}

    /**
     * Predicts without update
     */
    void predict();

    /**
     * Predicts with update
     * z_k must be set with the observation
     */
    void predictWithUpdate();

protected:
    Eigen::VectorXd x_k1_k1;
    Eigen::VectorXd x_k_k1;
    Eigen::VectorXd x_k_k;

    Eigen::VectorXd u_k;
    Eigen::VectorXd z_k;

    Eigen::VectorXd y_k_k1;
    Eigen::VectorXd y_k_k;

    Eigen::MatrixXd P_k1_k1;
    Eigen::MatrixXd P_k_k1;
    Eigen::MatrixXd P_k_k;

    Eigen::MatrixXd S_k;
    Eigen::MatrixXd K_k;

    Eigen::MatrixXd F_k;
    Eigen::MatrixXd B_k;
    Eigen::MatrixXd H_k;

    Eigen::MatrixXd Q_k;
    Eigen::MatrixXd R_k;

    Eigen::MatrixXd I;
};


/* test_array_derivatives.cpp - Test derivatives of array expressions

    Copyright (C) 2017 European Centre for Medium-Range Weather Forecasts

    Author: Robin Hogan <r.j.hogan@ecmwf.int>

  Copying and distribution of this file, with or without modification,
  are permitted in any medium without royalty provided the copyright
  notice and this notice are preserved.  This file is offered as-is,
  without any warranty.

*/

#include <adept_arrays.h>

// Arbitrary algorithm converting array of general type A to scalar of
// type S, which may be active or passive
template <class A, class S>
void algorithm(const A& x, S& y) {
  using namespace adept;
  A tmp;
  intVector index(2);
  index << 1, 0;
  tmp = atan2((exp(x) * x), spread<0>(x(index,1),2)) / x(0,0);
  y = sum(tmp);
}

int
main(int argc, const char** argv) {
  using namespace adept;

  Stack stack;

  // Matrix dimension
  static const int N = 2;
  static const Real MAX_FRAC_ERR = 1.0e-5;

  // Perturbation size for numerical calculation
  Real dx = 1.0e-6;

  if (sizeof(Real) < 8) {
    // Single precision only works with larger perturbations
    dx = 1.0e-4;
  }

  // Maximum fractional error
  Real max_frac_err;
  bool error_too_large = false;

  // Input data
  Matrix X(N,N);
  X << 2, 3, 5, 7;

  // Numerical calculation
  std::cout << "NUMERICAL CALCULATION\n";
  Matrix dJ_dx_num(N,N);
  {
    Real J;
    algorithm(X, J);
    std::cout << "J = " << J << "\n";

    for (int i = 0; i < N; ++i) {
      for (int j = 0; j < N; ++j) {
	Matrix Xpert(N,N);
	Xpert = X;
	Xpert(i,j) += dx;
	Real Jpert;
	algorithm(Xpert, Jpert);
	dJ_dx_num(i,j) = (Jpert - J) / dx;
      }
    }
  }

  std::cout << "dJ_dx_num = " << dJ_dx_num << "\n";

  std::cout << "\nNUMERICAL CALCULATION WITH \"FixedArray\"\n";
  Matrix22 dJ_dx_num_FixedArray;
  {
    Real J;
    algorithm(X, J);
    std::cout << "J = " << J << "\n";

    for (int i = 0; i < N; ++i) {
      for (int j = 0; j < N; ++j) {
	Matrix22 Xpert = X;
	Xpert(i,j) += dx;
	Real Jpert;
	algorithm(Xpert, Jpert);
	dJ_dx_num_FixedArray(i,j) = (Jpert - J) / dx;
      }
    }
  }

  std::cout << "dJ_dx_num_FixedArray = " << dJ_dx_num_FixedArray << "\n";

 // Adept calculation with aArray
  std::cout << "\nADEPT CALCULATION WITH \"aArray\"\n";
  Matrix dJ_dx_adept_Array(N,N);
  {
    aMatrix aX = X;
    stack.new_recording();
    aReal aJ;
    algorithm(aX, aJ);
    std::cout << "J = " << aJ << "\n";
    aJ.set_gradient(1.0);
    stack.reverse();

    dJ_dx_adept_Array = aX.get_gradient();
  }

  std::cout << "dJ_dx_adept_Array = " << dJ_dx_adept_Array << "\n";

  max_frac_err = maxval(abs(dJ_dx_adept_Array-dJ_dx_num)/dJ_dx_num);
  if (max_frac_err <= MAX_FRAC_ERR) {
    std::cout << "max fractional error = " << max_frac_err
		<< ": PASSED\n";
  }
  else {
    std::cout << "max fractional error = "
	      << max_frac_err << ": FAILED\n";
    error_too_large = true;
  }
  // Adept calculation with aFixedArray
  std::cout << "\nADEPT CALCULATION WITH \"aFixedArray\"\n";
  Matrix dJ_dx_adept_FixedArray;
  {
    aMatrix22 aX = X;
    stack.new_recording();
    aReal aJ;
    algorithm(aX, aJ);
    std::cout << "J = " << aJ << "\n";
    aJ.set_gradient(1.0);
    stack.reverse();
    dJ_dx_adept_FixedArray = aX.get_gradient();

  }
  std::cout << "dJ_dx_adept_FixedArray = " << dJ_dx_adept_FixedArray << "\n";

  max_frac_err = maxval(abs(dJ_dx_adept_FixedArray-dJ_dx_num)/dJ_dx_num);
  if (max_frac_err <= MAX_FRAC_ERR) {
    std::cout << "max fractional error = " << max_frac_err
		<< ": PASSED\n";
  }
  else {
    std::cout << "max fractional error = "
	      << max_frac_err << ": FAILED\n";
    error_too_large = true;
  }

  std::cout << "\n";

  if (error_too_large) {
    std::cerr << "*** Error: fractional error in the derivatives of some configurations too large\n";

    if (sizeof(Real) < 8) {
      std::cerr << "*** (but you are using less than double precision so it is not surprising)\n";
    }

    return 1;
  }
  else {
    return 0;
  }

}


#pragma once

//TODO to be removed...

#include <cmath>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include <pcl/filters/uniform_sampling.h>

#include <pcl/common/centroid.h>

#include <pcl/kdtree/kdtree_flann.h>

#include <Eigen/Geometry>
#include <Eigen/Dense>

#include "types.h"

namespace PoseEstimation
{
    /**********************************************************
     *     Useful helper methods (unused at this moment)      *
     **********************************************************/

    const double PI = 3.14159265358979323846264338327950288419716939937510582;
    const double DEG2RAD = PI / 180.0;
    const double RAD2DEG = 180.0 / PI;

    /**
     * @brief Finds the minimum element in an array.
     * @details Returns the index of the minimum element in an array. -1 if array is empty / null.
     *
     * @param array The input array.
     * @return The index of the minimum element.
     */
    template<class T>
    int min_element(const T *array)
    {
        if (!array)
            return -1;
        int length = sizeof(array) / sizeof(T);
        if (length <= 0)
            return -1;
        int m = 0;
        for (int i = 1; i < length; ++i)
        {
            if (array[i] < array[m])
                m = i;
        }
        return m;
    }

    /**
     * @brief Calculates the squared distance between two points.
     * @details Calculates the squared Euclidean distance between the two points.
     *
     * @param a First point with xyz-coordinates.
     * @param b Second point with xyz-coordinates.
     *
     * @return Squared distance between a and b.
     */
    float sqr_distance(const PointType &a, const PointType &b)
    {
        return pow(a.x-b.x, 2) + pow(a.y-b.y, 2) + pow(a.z-b.z, 2);
    }

    /**
     * @brief Calculates the squared distance between two points.
     * @details Calculates the squared Euclidean distance between the two points.
     *
     * @param a First point with xyz-coordinates.
     * @param b Second point with xyz-coordinates.
     *
     * @return Squared distance between a and b.
     */
    float sqr_distance(const Eigen::Vector3f &a, const Eigen::Vector3f &b)
    {
        return pow(a[0]-b[0], 2) + pow(a[1]-b[1], 2) + pow(a[2]-b[2], 2);
    }

    /**
     * @brief Converts a PCL point to an Eigen vector.
     * @details Converts a PCL point to an Eigen 3d vector by using the xyz-coordinates.
     *
     * @param point The PCL point.
     * @return The Eigen vector.
     */
    Eigen::Vector3f point2vec(const PointType &point)
    {
        return Eigen::Vector3f(point.x, point.y, point.z);
    }

    /**
     * @brief Converts an Eigen vector to a PCL point.
     * @details Converts an Eigen 3d vector to a PCL point by using the xyz-coordinates.
     *
     * @param point The PCL point.
     * @return The Eigen vector.
     */
    PointType vec2point(const Eigen::Vector3f &point)
    {
        PointType p;
        p.x = point[0];
        p.y = point[1];
        p.z = point[2];
        return p;
    }

    /**
     * @brief Generates a skew-symmetric matrix from a vector.
     * @details The skew-symmetric matrix S from a vector u allows to calculate the
     * cross-product u x v by using only the matrix multiplication S * v.
     *
     * @param v The input vector.
     * @return The skew-symmetric matrix of v.
     */
    template <typename T>
    inline Eigen::Matrix<T, 3, 3> makeSkewSymmetric(const Eigen::Matrix<T, 3, 1> &v)
    {
        Eigen::Matrix<T, 3, 3> out;
        out <<	   0, -v[2],  v[1],
                v[2],     0, -v[0],
               -v[1],  v[0],     0;

        return out;
    }
}


// unit test file atanh.hpp for the special functions test suite

//  (C) Copyright Hubert Holin 2003. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

#include <functional>
#include <iomanip>
#include <iostream>

#include <boost/math/special_functions/atanh.hpp>

#include <boost/test/unit_test.hpp>

template<typename T>
T    atanh_error_evaluator(T x)
{
    using    ::std::abs;
    using    ::std::tanh;
    using    ::std::cosh;

    using    ::std::numeric_limits;

    using    ::boost::math::atanh;

    static T const   epsilon = numeric_limits<float>::epsilon();

    T                y = tanh(x);
    T                z = atanh(y);

    T                absolute_error = abs(z-x);
    T                relative_error = absolute_error/(cosh(x)*cosh(x));
    T                scaled_error = relative_error/epsilon;

    return(scaled_error);
}

template<typename T>
void    atanh_test(const char * more_blurb)
{
    using    ::std::abs;
    using    ::std::tanh;
    using    ::std::log;

    using    ::std::numeric_limits;

    using    ::boost::math::atanh;

    BOOST_MESSAGE("Testing atanh in the real domain for "
        << more_blurb << ".");

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(), 2,
        (
            abs(atanh<T>(static_cast<T>(0))),
            numeric_limits<T>::epsilon()
        ));

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(), 2,
        (
            abs(atanh<T>(static_cast<T>(3)/5) - log(static_cast<T>(2))),
            numeric_limits<T>::epsilon()
        ));

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(), 2,
        (
            abs(atanh<T>(static_cast<T>(-3)/5) + log(static_cast<T>(2))),
            numeric_limits<T>::epsilon()
        ));

    for    (int i = 0; i <= 100; i++)
    {
        T    x = static_cast<T>(i-50)/static_cast<T>(5);
        T    y = tanh(x);

        if    (
                (abs(y-static_cast<T>(1)) >= numeric_limits<T>::epsilon())&&
                (abs(y+static_cast<T>(1)) >= numeric_limits<T>::epsilon())
            )
        {
            BOOST_CHECK_PREDICATE(::std::less_equal<T>(), 2,
                (
                    atanh_error_evaluator(x),
                    static_cast<T>(4)
                ));
        }
    }
}

void    atanh_manual_check()
{
    using    ::std::abs;
    using    ::std::tanh;

    using    ::std::numeric_limits;

    BOOST_MESSAGE("atanh");

    for    (int i = 0; i <= 100; i++)
    {
        float        xf = static_cast<float>(i-50)/static_cast<float>(5);
        double       xd = static_cast<double>(i-50)/static_cast<double>(5);
        long double  xl =
                static_cast<long double>(i-50)/static_cast<long double>(5);

        float        yf = tanh(xf);
        double       yd = tanh(xd);
        long double  yl = tanh(xl);

        if    (
                std::numeric_limits<float>::has_infinity &&
                std::numeric_limits<double>::has_infinity &&
                std::numeric_limits<long double>::has_infinity
            )
        {
            BOOST_MESSAGE( ::std::setw(15)
                        << atanh_error_evaluator(xf)
                        << ::std::setw(15)
                        << atanh_error_evaluator(xd)
                        << ::std::setw(15)
                        << atanh_error_evaluator(xl));
        }
        else
        {
            if    (
                    (abs(yf-static_cast<float>(1)) <
                        numeric_limits<float>::epsilon())||
                    (abs(yf+static_cast<float>(1)) <
                        numeric_limits<float>::epsilon())||
                    (abs(yf-static_cast<double>(1)) <
                        numeric_limits<double>::epsilon())||
                    (abs(yf+static_cast<double>(1)) <
                        numeric_limits<double>::epsilon())||
                    (abs(yf-static_cast<long double>(1)) <
                        numeric_limits<long double>::epsilon())||
                    (abs(yf+static_cast<long double>(1)) <
                        numeric_limits<long double>::epsilon())
                )
            {
                BOOST_MESSAGE("Platform's numerics may lack precision.");
            }
            else
            {
                BOOST_MESSAGE( ::std::setw(15)
                            << atanh_error_evaluator(xf)
                            << ::std::setw(15)
                            << atanh_error_evaluator(xd)
                            << ::std::setw(15)
                            << atanh_error_evaluator(xl));
            }
        }
    }

    BOOST_MESSAGE(" ");
}


// g2o - General Graph Optimization
// Copyright (C) 2012 R. Kümmerle
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

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

#include "g2o/stuff/sampler.h"
#include "g2o/stuff/command_args.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_unary_edge.h"
#include "g2o/solvers/dense/linear_solver_dense.h"

using namespace std;

/**
 * \brief the params, a, b, and lambda for a * exp(-lambda * t) + b
 */
class VertexParams : public g2o::BaseVertex<3, Eigen::Vector3d>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexParams()
    {
    }

    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual void setToOriginImpl()
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    }

    virtual void oplusImpl(const double* update)
    {
      Eigen::Vector3d::ConstMapType v(update);
      _estimate += v;
    }
};

/**
 * \brief measurement for a point on the curve
 *
 * Here the measurement is the point which is lies on the curve.
 * The error function computes the difference between the curve
 * and the point.
 */
class EdgePointOnCurve : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, VertexParams>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgePointOnCurve()
    {
    }
    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }
    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    void computeError()
    {
      const VertexParams* params = static_cast<const VertexParams*>(vertex(0));
      const double& a = params->estimate()(0);
      const double& b = params->estimate()(1);
      const double& lambda = params->estimate()(2);
      double fval = a * exp(-lambda * measurement()(0)) + b;
      _error(0) = fval - measurement()(1);
    }
};

int main(int argc, char** argv)
{
  int numPoints;
  int maxIterations;
  bool verbose;
  std::vector<int> gaugeList;
  string dumpFilename;
  g2o::CommandArgs arg;
  arg.param("dump", dumpFilename, "", "dump the points into a file");
  arg.param("numPoints", numPoints, 50, "number of points sampled from the curve");
  arg.param("i", maxIterations, 10, "perform n iterations");
  arg.param("v", verbose, false, "verbose output of the optimization process");

  arg.parseArgs(argc, argv);

  // generate random data
  double a = 2.;
  double b = 0.4;
  double lambda = 0.2;
  Eigen::Vector2d* points = new Eigen::Vector2d[numPoints];
  for (int i = 0; i < numPoints; ++i) {
    double x = g2o::Sampler::uniformRand(0, 10);
    double y = a * exp(-lambda * x) + b;
    // add Gaussian noise
    y += g2o::Sampler::gaussRand(0, 0.02);
    points[i].x() = x;
    points[i].y() = y;
  }

  if (dumpFilename.size() > 0) {
    ofstream fout(dumpFilename.c_str());
    for (int i = 0; i < numPoints; ++i)
      fout << points[i].transpose() << endl;
  }

  // some handy typedefs
  typedef g2o::BlockSolver< g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> >  MyBlockSolver;
  typedef g2o::LinearSolverDense<MyBlockSolver::PoseMatrixType> MyLinearSolver;

  // setup the solver
  g2o::SparseOptimizer optimizer;
  optimizer.setVerbose(false);

  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
    g2o::make_unique<MyBlockSolver>(g2o::make_unique<MyLinearSolver>()));

  optimizer.setAlgorithm(solver);

  // build the optimization problem given the points
  // 1. add the parameter vertex
  VertexParams* params = new VertexParams();
  params->setId(0);
  params->setEstimate(Eigen::Vector3d(1,1,1)); // some initial value for the params
  optimizer.addVertex(params);
  // 2. add the points we measured to be on the curve
  for (int i = 0; i < numPoints; ++i) {
    EdgePointOnCurve* e = new EdgePointOnCurve;
    e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
    e->setVertex(0, params);
    e->setMeasurement(points[i]);
    optimizer.addEdge(e);
  }

  // perform the optimization
  optimizer.initializeOptimization();
  optimizer.setVerbose(verbose);
  optimizer.optimize(maxIterations);

  if (verbose)
    cout << endl;

  // print out the result
  cout << "Target curve" << endl;
  cout << "a * exp(-lambda * x) + b" << endl;
  cout << "Iterative least squares solution" << endl;
  cout << "a      = " << params->estimate()(0) << endl;
  cout << "b      = " << params->estimate()(1) << endl;
  cout << "lambda = " << params->estimate()(2) << endl;
  cout << endl;

  // clean up
  delete[] points;

  return 0;
}


#ifndef COMMON_COLOR_HPP_
#define COMMON_COLOR_HPP_
//----------------------------------------------------------------------------//
#include <Danvil/Color.h>
#include <Eigen/Dense>
#include <Slimage/Slimage.hpp>
//----------------------------------------------------------------------------//
namespace common {
//----------------------------------------------------------------------------//

template<typename K>
inline Eigen::Vector3f GreyColor(K x, K a, K b)
{
	float p = (static_cast<float>(x) - static_cast<float>(a)) / (static_cast<float>(b) - static_cast<float>(a));
	p = std::min(1.0f, std::max(0.0f, p));
	return {p,p,p};
}

/** Computes a color to express a similarity value between 0 and 1 */
inline Eigen::Vector3f SimilarityColor(float x)
{
	static auto cm = Danvil::ContinuousIntervalColorMapping<float, float>::Factor_Black_Blue_Red_Yellow_White();
	cm.setRange(0.0f, 1.0f);
	Danvil::Colorf color = cm(x);
	return {color.r,color.g,color.b};
}

inline Eigen::Vector3f IntensityColor(float x, float a=0.0f, float b=1.0f)
{
	static auto cm = Danvil::ContinuousIntervalColorMapping<float, float>::Factor_Black_Blue_Red_Yellow_White();
	cm.setRange(a, b);
	Danvil::Colorf color = cm(x);
	return {color.r,color.g,color.b};
}

inline Eigen::Vector3f InvIntensityColor(float x, float a=0.0f, float b=1.0f)
{
	return IntensityColor(a + b - x, a, b);
}

inline Eigen::Vector3f PlusMinusColor(float x, float range=1.0f)
{
	static auto cm = Danvil::ContinuousIntervalColorMapping<float, float>::Factor_MinusPlus();
	cm.setRange(-range, +range);
	Danvil::Colorf color = cm(x);
	return {color.r,color.g,color.b};
}

template<typename T>
inline Eigen::Vector3f CountColor(T num, T min, T max)
{
	return IntensityColor(
		(num < min)
			? 0.0f
			: static_cast<float>(num - min)/static_cast<float>(max-min)
	);
}

inline slimage::Pixel3ub ColorToPixel(const Eigen::Vector3f& color) {
	return slimage::Pixel3ub{{
		static_cast<unsigned char>(255.f*std::min(1.0f, std::max(0.0f, color[0]))),
		static_cast<unsigned char>(255.f*std::min(1.0f, std::max(0.0f, color[1]))),
		static_cast<unsigned char>(255.f*std::min(1.0f, std::max(0.0f, color[2])))
	}};
}

inline slimage::Image3ub Colorize(const slimage::Image1ub& img) {
	slimage::Image3ub result(img.width(), img.height());
	const int n = img.size();
	for(int i=0; i<n; i++) {
		result[i] = ColorToPixel(CountColor<uint8_t>(img[i], 0, 255));
	}
	return result;
}

inline slimage::Image3ub ColorizeDepth(const slimage::Image1ui16& img16, uint16_t min, uint16_t max) {
	slimage::Image3ub img(img16.width(), img16.height());
	const int n = img16.size();
	for(int i=0; i<n; i++) {
		img[i] = ColorToPixel(CountColor((uint16_t)img16[i], min, max));
	}
	return img;
}

inline slimage::Image3ub GreyDepth(const slimage::Image1ui16& img16, uint16_t min, uint16_t max) {
	slimage::Image3ub img(img16.width(), img16.height());
	const int n = img16.size();
	for(int i=0; i<n; i++) {
		img[i] = ColorToPixel(GreyColor<uint16_t>(img16[i], min, max));
	}
	return img;
}

template<typename CF>
inline slimage::Image3ub MatrixToImage(const Eigen::MatrixXf& mat, CF cf)
{
	slimage::Image3ub vis = slimage::Image3ub(mat.rows(), mat.cols());
	const float* p = mat.data();
	for(unsigned int i=0; i<vis.size(); i++) {
		vis[i] = ColorToPixel(cf(p[i]));
	}
	return vis;
}

namespace detail
{
	inline unsigned char mean(unsigned char x1, unsigned char x2, unsigned char x3, unsigned char x4, unsigned char x5) {
		unsigned int nb = 4*static_cast<unsigned int>(x1)
			+ static_cast<unsigned int>(x2)
			+ static_cast<unsigned int>(x3)
			+ static_cast<unsigned int>(x4)
			+ static_cast<unsigned int>(x5);
		return nb / 8;
	}
	// inline slimage::Pixel1ub mean(slimage::Pixel1ub x1, slimage::Pixel1ub x2, slimage::Pixel1ub x3, slimage::Pixel1ub x4) {
	// 	return slimage::Pixel1ub{{ mean(x1[0],x2[0],x3[0],x4[0]) }};
	// }
	inline slimage::Pixel3ub mean(slimage::Pixel3ub x1, slimage::Pixel3ub x2, slimage::Pixel3ub x3, slimage::Pixel3ub x4, slimage::Pixel3ub x5) {
		return slimage::Pixel3ub{{
			mean(x1[0],x2[0],x3[0],x4[0],x5[0]),
			mean(x1[1],x2[1],x3[1],x4[1],x5[1]),
			mean(x1[2],x2[2],x3[2],x4[2],x5[2]) }};
	}
}

template<typename T>
slimage::Image<T> Smooth(const slimage::Image<T>& src)
{
	slimage::Image3ub tmp = src.clone();
	for(unsigned int x=1; x<tmp.width()-1; x++) {
		for(unsigned int y=1; y<tmp.height()-1; y++) {
			tmp(x,y) = detail::mean(src(x,y), src(x-1,y), src(x+1,y), src(x,y-1), src(x,y+1));
		}
	}
	return tmp;
}

//----------------------------------------------------------------------------//
}
//----------------------------------------------------------------------------//
#endif


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS

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

#include <boost/config.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/arch/common/detail/generic/stirling_kernel.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/real.hpp>
#include <boost/simd/constant/sqrt_2pi.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/if_else_nan.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_gez.hpp>
#include <boost/simd/function/pow_abs.hpp>
#include <boost/simd/function/rec.hpp>
#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/is_nan.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF ( stirling_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::pack_< bd::floating_<A0>, X>
                             )
  {
    BOOST_FORCEINLINE A0 operator() (const A0& a00) const BOOST_NOEXCEPT
    {
      const A0 Stirlingsplitlim = Real<A0, 0X4061E083BA3443D4ULL, 0X41D628F6UL>();// 143.01608, 26.77f
      const A0 Stirlinglargelim = Real<A0, 0x4065800000000000ULL, 0X420C28F3UL>();// 172, 35.0399895f
      A0 a0 = if_else_nan(is_gez(a00), a00);
      A0 w = bs::rec(a0);
      w = fma(w,detail::stirling_kernel<A0>::stirling1(w), bs::One<A0>());
      A0 y = bs::exp(-a0);
      auto test = is_less(a0, Stirlingsplitlim);
      A0 z =  a0 - bs::Half<A0>();
      z =  if_else(test, z, Half<A0>()*z);
      A0 v =  bs::pow_abs(a0,z);
      y *= v;
      y = if_else(test,y, y*v); /* Avoid overflow in pow() */
      y *= bs::Sqrt_2pi<A0>()*w;
      #ifndef BOOST_SIMD_NO_INFINITIES
      y = if_else(is_equal(a0, Inf<A0>()), a0, y);
      #endif
      return if_else(a0 > Stirlinglargelim, Inf<A0>(), y);
    }
  };
} } }

#endif


//  (C) Copyright John Maddock 2021.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

//
// As detailed in https://github.com/boostorg/math/issues/544
// our original non-central T test data wasn't accurate past about
// 20 decimal places.  This data generator takes the original input
// values and generates new CDF and CDF-complement values via
// tanh_sinh integration - an option that wasn't available when
// the original values were generated back in 2008.
//
// Note that this code will take SEVERAL DAYS to run on typical
// 2021 hardware.
//

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/special_functions/hypergeometric_1F1.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/quadrature/exp_sinh.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <fstream>

#include <libs/math/test/table_type.hpp>

using namespace boost::math::tools;
using namespace boost::math;
using namespace std;
using namespace boost::multiprecision;

//using big_t = number<cpp_bin_float<200, digit_base_10, void, long long>>;
using big_t = number<mpfr_float_backend<200>>;

big_t nct_A(big_t v, big_t x, big_t nu)
{
   big_t result = boost::math::hypergeometric_1F1(v / 2 + 1, big_t(3) / 2, nu * nu * x * x / (2 * (v + x * x)));
   result *= boost::math::constants::root_two<big_t>() * nu * x;
   result /= (v + x * x) * boost::math::tgamma((v + 1) / 2);
   return result;
}
big_t nct_B(big_t v, big_t x, big_t nu)
{
   big_t result = boost::math::hypergeometric_1F1((v + 1) / 2, big_t(1) / 2, nu * nu * x * x / (2 * (x * x + v)));
   result /= sqrt(v + x * x) * boost::math::tgamma(v / 2 + 1);
   return result;
}
big_t nct_PDF(big_t v, big_t nu, big_t x)
{
   big_t result = nct_A(v, x, nu) + nct_B(v, x, nu);
   result *= pow(v, v / 2) * boost::math::tgamma(v + 1);
   result /= pow(big_t(2), v) * exp(nu * nu / 2) * pow(v + x * x, v / 2) * boost::math::tgamma(v / 2);
   return result;
}

big_t nc_t_F(big_t v, big_t nc, big_t t)
{
   unsigned j = 0;
   big_t sum = 0;
   big_t tol = std::numeric_limits<big_t>::epsilon();
   do
   {
      big_t x = t * t / (t * t + v);
      big_t p = exp(-nc * nc / 2) * pow(nc * nc / 2, j) / (2 * boost::math::factorial<big_t>(j));
      big_t q = (nc / 2) * exp(-nc * nc / 2) * pow(nc * nc / 2, j) / (boost::math::constants::root_two<big_t>() * boost::math::tgamma(j + big_t(3) / 2));
      big_t term = p * boost::math::ibeta(big_t(j) + 0.5, v / 2, x) + q * boost::math::ibeta(big_t(j + 1), v / 2, x);
      ++j;

      sum += term;

      if (fabs(sum * tol) > fabs(term))
         break;
   } while (true);

   return sum + boost::math::constants::half<big_t>() * (1 + boost::math::erf(-nc / boost::math::constants::root_two<big_t>()));
}
big_t nc_t(big_t v, big_t nc, big_t t)
{
   return t < 0 ? 1 - nc_t_F(v, -nc, t) : nc_t_F(v, nc, t);
}

#define SC_(x) BOOST_JOIN(x, f)

int main(int, char* [])
{
   mpfr_set_emax(mpfr_get_emax_max());
   mpfr_set_emin(mpfr_get_emin_min());

   boost::math::quadrature::exp_sinh<big_t> integrator(10);
   using T = float;

#include <libs/math/test/nct.ipp>

   for (unsigned i = 0; i < nct.size(); ++i)
   {
      big_t error1, error2;
      big_t v(nct[i][0]), nc(nct[i][1]), x(nct[i][2]);
      big_t cdf, ccdf;
      try{
         cdf = integrator.integrate([&](big_t y) { return nct_PDF(v, nc, y); }, -std::numeric_limits<big_t>::infinity(), x, big_t(1e-36), &error1);
         ccdf = integrator.integrate([&](big_t y) { return nct_PDF(v, nc, y); }, x, std::numeric_limits<big_t>::infinity(), big_t(1e-36), &error2);
      }
      catch(const std::exception& e)
      {
         std::cout << "// " << e.what() << " reverting to ibeta method" << std::endl;
         error1 = error2 = 0;
         cdf = nc_t(v, nc, x);
         ccdf = 1 - cdf;
      }

      if (error1 > 1e-35)
      {
         std::cout << "// Accuracy for cdf was " << error1 << " reverting to ibeta method" << std::endl;
         cdf = nc_t(v, nc, x);
      }
      if (error2 > 1e-35)
      {
         std::cout << "// Accuracy for complement cdf was " << error2 << " reverting to ibeta method" << std::endl;
         ccdf = 1 - nc_t(v, nc, x);
      }

      std::cout << std::setprecision(40);
      std::cout << "{{ SC_(" << nct[i][0] << "), SC_(" << nct[i][1] << "), SC_(" << nct[i][2] << "), SC_(";
      std::cout << cdf << "), SC_(" << ccdf << ") }}," << std::endl;
   }

#include <libs/math/test/nct_small_delta.ipp>
   for (unsigned i = 0; i < nct_small_delta.size(); ++i)
   {
      big_t error1, error2;
      big_t v(nct_small_delta[i][0]), nc(nct_small_delta[i][1]), x(nct_small_delta[i][2]);
      big_t cdf, ccdf;
      try {
         cdf = integrator.integrate([&](big_t y) { return nct_PDF(v, nc, y); }, -std::numeric_limits<big_t>::infinity(), x, big_t(1e-36), &error1);
         ccdf = integrator.integrate([&](big_t y) { return nct_PDF(v, nc, y); }, x, std::numeric_limits<big_t>::infinity(), big_t(1e-36), &error2);
      }
      catch (const std::exception& e)
      {
         std::cout << "// " << e.what() << " reverting to ibeta method" << std::endl;
         error1 = error2 = 0;
         cdf = nc_t(v, nc, x);
         ccdf = 1 - cdf;
      }

      if (error1 > 1e-35)
      {
         std::cout << "// Accuracy for cdf was " << error1 << " reverting to ibeta method" << std::endl;
         cdf = nc_t(v, nc, x);
      }
      if (error2 > 1e-35)
      {
         std::cout << "// Accuracy for complement cdf was " << error2 << " reverting to ibeta method" << std::endl;
         ccdf = 1 - nc_t(v, nc, x);
      }

      std::cout << std::setprecision(40);
      std::cout << "{{ SC_(" << v << "), SC_(" << nc << "), SC_(" << x << "), SC_(";
      std::cout << cdf << "), SC_(" << ccdf << ") }}," << std::endl;
   }

#include "../test/nct_asym.ipp"
   for (unsigned i = 0; i < nct_asym.size(); ++i)
   {
      big_t error1, error2;
      big_t v(nct_asym[i][0]), nc(nct_asym[i][1]), x(nct_asym[i][2]);
      big_t cdf, ccdf;
      try {
         cdf = integrator.integrate([&](big_t y) { return nct_PDF(v, nc, y); }, -std::numeric_limits<big_t>::infinity(), x, big_t(1e-36), &error1);
         ccdf = integrator.integrate([&](big_t y) { return nct_PDF(v, nc, y); }, x, std::numeric_limits<big_t>::infinity(), big_t(1e-36), &error2);
      }
      catch (const std::exception& e)
      {
         std::cout << "// " << e.what() << " reverting to ibeta method" << std::endl;
         error1 = error2 = 0;
         cdf = nc_t(v, nc, x);
         ccdf = 1 - cdf;
      }

      if (error1 > 1e-35)
      {
         std::cout << "// Accuracy for cdf was " << error1 << " reverting to ibeta method" << std::endl;
         cdf = nc_t(v, nc, x);
      }
      if (error2 > 1e-35)
      {
         std::cout << "// Accuracy for complement cdf was " << error2 << " reverting to ibeta method" << std::endl;
         ccdf = 1 - nc_t(v, nc, x);
      }

      std::cout << std::setprecision(40);
      std::cout << "{{ SC_(" << v << "), SC_(" << nc << "), SC_(" << x << "), SC_(";
      std::cout << cdf << "), SC_(" << ccdf << ") }}," << std::endl;
   }

   return 0;
}


#include "mex.h"
#include <array>
#include <cmath>
#include <complex>
#include <fstream>
#include <iostream>
#include <math.h>
#include <string> // for string class
#include <time.h>
#include <vector>

//#define EIGEN_USE_MKL_ALL
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <Eigen/Sparse>
#include <unsupported/Eigen/CXX11/Tensor>
#include <unsupported/Eigen/KroneckerProduct>
#include <unsupported/Eigen/SparseExtra>

#include <eigen3/Eigen/PaStiXSupport>

using namespace Eigen;
using complex_sparse_matrix = Eigen::SparseMatrix<std::complex<double>>;
using complex_vector = Eigen::VectorXcd;
using complex_matrix = Eigen::MatrixXcd;

#define PI acos(-1.0)

//*******************Changing***************************
// Without damping matrix
// full geometry, no symmetric boundarycondition
//********************************************************

//#include "mkl_lapacke.h"
//#include "lapack.h"

// typedef size_t INT;
//#define MKL_INT INT
//#ifndef lapack_int
//#define lapack_int MKL_INT
//#endif
// written by Chau Nguyen Khanh
#define MIN(a, b) ((a) < (b) ? (a) : (b))
//#ifdef __cplusplus
// extern "C" bool utIsInterruptPending();
//#else
// extern bool utIsInterruptPending();
//#endif

#if defined(NAN_EQUALS_ZERO)
#define IsNonZero(d) ((d) != 0.0 || mxIsNaN(d))
#else
#define IsNonZero(d) ((d) != 0.0)
#endif

// Dynamic force
// double force(double x)
// {
// 	double i;
// 	if ((1 - x) >= 0)
// 	{
// 		i = 1.0;
// 	}
// 	else
// 	{
// 		i = 0.0;
// 	}
// 	return -4 * (1.0 - pow((2.0 * x - 1.0), 2.0)) * i;
// }
double force(double x) { return 1e9 * sin(460 * x); }

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  //*********************************************
  /* get the values from the struct 1x1 */
  // int NoTimeStep = (int) mxGetScalar(mxGetField(prhs[0], 0, "NoTimeStep"));
  // int N_DoF = (int) mxGetScalar(mxGetField(prhs[0], 0, "N_DoF")); // number
  // of dof per element double dt = (double) mxGetScalar(mxGetField(prhs[0], 0,
  // "dt")); // number of dof per element
  //*********************************************

  /* get the values from the struct Matrix m x 1 */ /*only use the pointer*/
  // double *TIME_vec = (double *) mxGetPr(mxGetField(prhs[0], 0, "TIME_vec"));
  // int *TIMEPLOT = (int*) mxGetPr(mxGetField(prhs[0], 0, "TIMEPLOT"));
  // int LengthTIMEPLOT = (int) mxGetScalar(mxGetField(prhs[0], 0,
  // "LengthTIMEPLOT")); // number of dof per element

  // 	double *val_v0 = (double *) mxGetPr(mxGetField(prhs[0], 0, "v0")); //
  // 	Eigen::VectorXd v0 = Map < VectorXd > (val_v0, N_DoF);
  //*********************************************
  double *val_rhs_matrix =
      (double *)mxGetPr(mxGetField(prhs[0], 0, "rhs_matrix"));
  int m_rhs_matrix = mxGetM(mxGetField(prhs[0], 0, "rhs_matrix"));
  int n_rhs_matrix = mxGetN(mxGetField(prhs[0], 0, "rhs_matrix"));
  Eigen::MatrixXd rhs_matrix =
      Eigen::Map<MatrixXd>(val_rhs_matrix, m_rhs_matrix, n_rhs_matrix);

  double *nnzval_K = (double *)mxGetPr(mxGetField(prhs[0], 0, "nnzval_K"));
  int m_K = (int)mxGetScalar(mxGetField(prhs[0], 0, "m_K"));
  int nnz_K = (int)mxGetScalar(mxGetField(prhs[0], 0, "nnz_K"));
  int *Ir_K = (int *)mxGetPr(mxGetField(prhs[0], 0, "Ir_K"));
  int *Jc_K = (int *)mxGetPr(mxGetField(prhs[0], 0, "Jc_K"));
  std::vector<Eigen::Triplet<double>> trip_K(3 * nnz_K);
  for (int i = 0; i < nnz_K; ++i) {
    trip_K.push_back(
        Eigen::Triplet<double>(Ir_K[i] - 1, Jc_K[i] - 1, nnzval_K[i]));
  }
  Eigen::SparseMatrix<double> K(m_K, m_K);
  K.setFromTriplets(trip_K.begin(), trip_K.end());

  //	* Out put *//* Out put *//* Out put *//* Out put *//* Out put *//* Out
  // put */
  plhs[0] =
      mxCreateDoubleMatrix((mwSize)m_rhs_matrix, (mwSize)n_rhs_matrix, mxREAL);
  double *u0_out =
      mxGetPr(plhs[0]); // pointer pr_out will manage data in COLUMN Major.
  // plhs[1] = mxCreateDoubleMatrix((mwSize) N_DoF, (mwSize) LengthTIMEPLOT,
  // mxREAL); double *v0_out = mxGetPr(plhs[1]); // pointer pr_out will manage
  // data in COLUMN Major. plhs[2] = mxCreateDoubleMatrix((mwSize) N_DoF,
  // (mwSize) LengthTIMEPLOT, mxREAL); double *a0_out = mxGetPr(plhs[2]); //
  // pointer pr_out will manage data in COLUMN Major.

  std::cout << "finishing read matrix from matlab" << std::endl;

  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver_LHS;
  // Eigen::PastixLU<Eigen::SparseMatrix<double>, true> solver_LHS;
  // Eigen::PastixLDLT<Eigen::SparseMatrix<double>, Eigen::Upper> solver_LHS;

  std::cout << "1" << std::endl;
  // solver_LHS.iparm(34) = 4;
  std::cout << "2" << std::endl;
  solver_LHS.analyzePattern(K);
  std::cout << "3" << std::endl;
  solver_LHS.factorize(K);
  std::cout << "4" << std::endl;

  Eigen::MatrixXd u_n1(m_rhs_matrix, n_rhs_matrix);
  u_n1 = solver_LHS.solve(rhs_matrix);

  std::cout << "finishing read matrix from matlab" << std::endl;
  // Update solution to Final matrix

  Eigen::Map<MatrixXd>(u0_out + (m_rhs_matrix * 0), m_rhs_matrix,
                       n_rhs_matrix) = u_n1;
}


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

using namespace std;

typedef std::complex<double> cmp;

template <typename Vector>
void test(const Vector& , const char* name)
{
    mtl::multi_vector<Vector> A(5, 5);
    A= 3.0;
    A[3][2]= cmp(0, 1);
    cout << name << ":\n A after initialization is \n" << A << "\n";

    mtl::dense_vector<cmp>  b(5, 4.0), x;
    x= hermitian(A) * b;

    cout << "x = " << x << "\n";
    MTL_THROW_IF(x[1] != 12.0, mtl::runtime_error("Wrong value"));
    MTL_THROW_IF(x[2] != cmp(12.0, -4.0), mtl::runtime_error("Wrong value"));
}

int main(int, char**)
{
    using namespace mtl;

    dense_vector<cmp>    v;
    test(v, "dense_vector<double>");

    return 0;
}