#include <boost/chrono.hpp>

#include <sstream>
#include <fstream>
#include <iostream>
#include <vector>
#include <cstdlib>

#include <Snap.h>
#include <boost/lexical_cast.hpp>

namespace timens = boost::chrono;
using namespace std;

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

    //Input Graph
    const TStr InFNm = TStr(argv[1]);
    //Output file
    const TStr OutFNm = TStr(argv[2]);
    //File with list of nodes
    const TStr argsMod = TStr(argv[3]);
    //File with list of pairs of nodes
    const TStr pairNodes = TStr(argv[4]);

    printf("Loading %s...", InFNm.CStr());
    TIntFltH BtwH, EigH, PRankH, CcfH, CloseH, HubH, AuthH;

    std::chrono::milliseconds durationPR;
    PNGraph Graph = TSnap::LoadEdgeList<PNGraph>(InFNm);
    PUNGraph UGraph = TSnap::ConvertGraph<PUNGraph>(Graph);

    std::chrono::system_clock::time_point start = std::chrono::system_clock::now();
    TSnap::GetPageRank(Graph, PRankH, 0.85, 0, 100);
    durationPR = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - start);
    cout << endl << "Runtime PR: " << durationPR.count() << " ms." << endl;

    start = std::chrono::system_clock::now();
    TSnap::GetHits(Graph, HubH, AuthH);
    std::chrono::milliseconds durationHits = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - start);
    cout << endl << "Runtime Hits: " << durationHits.count() << " ms." << endl;

    //start = std::chrono::system_clock::now();
    //TSnap::GetEigenVectorCentr(UGraph, EigH);
    //std::chrono::milliseconds durationEV = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - start);
    //cout << endl << "Runtime EigenVector: " << durationEV.count() << " ms." << endl;

    start = std::chrono::system_clock::now();
    TSnap::GetNodeClustCf_scala(UGraph, CcfH);
    std::chrono::milliseconds durationCF = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - start);
    cout << endl << "Runtime ClusterCoeff: " << durationCF.count() << " ms." << endl;

    start = std::chrono::system_clock::now();
    long c = TSnap::CountTriangles(Graph);
    std::chrono::milliseconds durationTriangles = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - start);
    cout << endl << "Runtime Triangles: " << durationTriangles.count() << " ms." << c << endl;

    start = std::chrono::system_clock::now();
    long output = TSnap::GetBfsFullDiam(Graph, 1000, true);
    std::chrono::milliseconds durationDiam = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - start);
    cout << endl << "Runtime Diameter: " << durationDiam.count() << " ms." << output << endl;

    start = std::chrono::system_clock::now();
    double outputWcc = TSnap::GetMxWccSz(Graph);
    std::chrono::milliseconds durationMaxWcc = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - start);
    cout << endl << "Runtime MaxWcc: " << durationMaxWcc.count() << " ms." << outputWcc << endl;

    start = std::chrono::system_clock::now();
    double outputScc = TSnap::GetMxSccSz(Graph);
    std::chrono::milliseconds durationMaxScc = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - start);
    cout << endl << "Runtime MaxScc: " << durationMaxScc.count() << " ms." << outputScc << endl;

    TIntV Values;
    std::vector<long> InputNodes;
    std::ifstream ifs(string(argsMod.CStr()));
    std::string line;
    while (std::getline(ifs, line)) {
        int v = boost::lexical_cast<int>(line);
        Values.Add(v);
        InputNodes.push_back(v);
    }
    ifs.close();
    long len = 3;
    start = std::chrono::system_clock::now();
    auto randWalk = TSnap::randomWalk2(Graph, InputNodes, len);
    std::chrono::milliseconds durationRW = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - start);
    cout << endl << "Runtime RandomWalk: " << durationRW.count() << " ms." << randWalk.size() << endl;

    std::ifstream ifs2(string(pairNodes.CStr()));
    std::vector<std::pair<long, long>> pairs;
    while (std::getline(ifs2, line)) {
        auto pos = line.find('\t');
        long v1 = boost::lexical_cast<long>(line.substr(0, pos));
        long v2 = boost::lexical_cast<long>(line.substr(pos+1, line.size()));
        pairs.push_back(std::make_pair(v1, v2));
    }
    ifs2.close();
    start = std::chrono::system_clock::now();
    auto bfsoutput = TSnap::GetShortPath(Graph, pairs);
    std::chrono::milliseconds durationBFS = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - start);
    cout << endl << "Runtime BFS: " << durationBFS.count() << " ms. " << bfsoutput.size() << endl;

    const int edges = Graph->GetEdges();
    start = std::chrono::system_clock::now();
    double outputMod = TSnap::GetModularity(Graph, Values, edges);
    std::chrono::milliseconds durationMod = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - start);
    cout << endl << "Runtime Mod: " << durationMod.count() << " ms. " << outputMod << endl;

    start = std::chrono::system_clock::now();
    TSnap::GetBetweennessCentr(Graph, BtwH);
    std::chrono::milliseconds durationBetCentr = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - start);
    cout << endl << "Runtime BetCentr: " << durationBetCentr.count() << " ms." << endl;

    return 0;
}


#include <cstdlib>
#include <iostream>
#include <complex>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/lapack/driver.hpp>
#include <boost/numeric/bindings/lower.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;
namespace lapack=boost::numeric::bindings::lapack;

int main(int argc, char *argv[]) {
  typedef std::complex<double> complex;
  typedef ublas::vector<complex> vector;
  typedef ublas::matrix<complex, ublas::column_major> matrix;
  typedef typename vector::size_type size_type;

  rand_normal<complex>::reset();
  int n=128;
  // generate a random unitary matrix
  matrix U(n, n);
  for (size_type j=0; j<n; ++j)
    for (size_type i=0; i<n; ++i)
      U(i, j)=i==j ? complex(1) : complex(0);
  for (int k=0; k<n*n; ++k) {
    // generate a random 2x2 unitary matrix
    double phi(rand_uniform<double>::get(0, 1.5707963267948966192));
    double alpha(rand_uniform<double>::get(0, 6.2831853071795864770));
    double psi(rand_uniform<double>::get(0, 6.2831853071795864770));
    double chi(rand_uniform<double>::get(0, 6.2831853071795864770));
    matrix u(2, 2);
    u(0, 0)=complex(std::cos(alpha+psi), std::sin(alpha+psi))*std::cos(phi);
    u(1, 0)=-complex(std::cos(alpha-chi), std::sin(alpha-chi))*std::sin(phi);
    u(0, 1)=complex(std::cos(alpha+chi), std::sin(alpha+chi))*std::sin(phi);
    u(1, 1)=complex(std::cos(alpha-psi), std::sin(alpha-psi))*std::cos(phi);
    int j0, j1;
    j0=static_cast<int>(rand_uniform<double>::get(0, n));
    do {
      j1=static_cast<int>(rand_uniform<double>::get(0, n));
    } while (j0==j1);
    for (size_type i=0; i<n; ++i) {
      vector Uc(2);
      Uc(0)=U(j0, i);
      Uc(1)=U(j1, i);
      Uc=ublas::prod(u, Uc);
      U(j0, i)=Uc(0);
      U(j1, i)=Uc(1);
    }
  }
  matrix R(ublas::prod(ublas::trans(ublas::conj(U)), U));
  // generate random positive definite hermitian matrix
  matrix A(n, n);
  for (size_type j=0; j<n; ++j)
    for (size_type i=0; i<n; ++i)
      if (i==j)
	// eigenvalues drawn from the positive half of the normal distribution
	do {
	  A(i, j)=std::abs(rand_normal<complex>::get());
	} while (A(i, j)==complex(0));
      else
	A(i, j)=complex(0);
  // apply a random unitary transform to A
  A=ublas::prod(ublas::trans(ublas::conj(U)), A);
  A=ublas::prod(A, U);
  matrix A_bak(A);
  vector b(n);
  for (size_type i=0; i<n; ++i)
    b(i)=rand_normal<complex>::get();
  vector x(b);
  int info=lapack::posv(lapack::upper(A), x); // solve
  if (info==0) {
    // res <- A*x - b
    vector res(b);
    blas::gemv(complex(1, 0), A_bak, x, complex(-1, 0), res);
    std::cout << "norm of residual : " << blas::nrm2(res) << '\n';
  } else
    if (info>0)
      std::cout << "singular matrix\n";
    else
      std::cout << "illegal arguments\n";
  return EXIT_SUCCESS;
}


/******************************************************************************
 * Copyright (C) 2014 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

/** \file analyzer.cpp
    \brief This file analyzes realworld data.
  */

#include <cmath>

#include <iostream>
#include <iomanip>
#include <fstream>
#include <algorithm>
#include <string>
#include <vector>

#include <Eigen/Core>

#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <rosbag/message_instance.h>

#include <sm/kinematics/Transformation.hpp>
#include <sm/kinematics/rotations.hpp>
#include <sm/kinematics/EulerAnglesYawPitchRoll.hpp>
#include <sm/kinematics/quaternion_algebra.hpp>

#include <sm/BoostPropertyTree.hpp>

#include <sm/timing/TimestampCorrector.hpp>
#include <sm/timing/NsecTimeUtilities.hpp>

#include <poslv/VehicleNavigationSolutionMsg.h>
#include <poslv/VehicleNavigationPerformanceMsg.h>
#include <poslv/TimeTaggedDMIDataMsg.h>

#include <can_prius/FrontWheelsSpeedMsg.h>
#include <can_prius/RearWheelsSpeedMsg.h>
#include <can_prius/Steering1Msg.h>

#include "aslam/calibration/car/algo/CarCalibrator.h"
#include "aslam/calibration/car/algo/splinesToFile.h"
#include "aslam/calibration/car/data/WheelSpeedsMeasurement.h"
#include "aslam/calibration/car/data/SteeringMeasurement.h"
#include "aslam/calibration/car/data/DMIMeasurement.h"
#include "aslam/calibration/car/data/PoseMeasurement.h"
#include "aslam/calibration/car/data/VelocitiesMeasurement.h"
#include "aslam/calibration/car/geo/geodetic.h"

using namespace sm;
using namespace sm::timing;
using namespace sm::kinematics;
using namespace aslam::calibration;

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

  if (argc != 3) {
    std::cerr << "Usage: " << argv[0] << " <bag_file> <conf_file>" << std::endl;
    return -1;
  }

  BoostPropertyTree config;
  config.loadXml(argv[2]);

  const bool useDMI =
    config.getBool("car/calibrator/odometry/sensors/dmi/active");
  const bool useFw =
    config.getBool("car/calibrator/odometry/sensors/fws/active");
  const bool useRw =
    config.getBool("car/calibrator/odometry/sensors/rws/active");
  const bool useSt =
    config.getBool("car/calibrator/odometry/sensors/st/active");

  CarCalibrator calibrator(PropertyTree(config, "car/calibrator"));

  std::ofstream rwDataFile("rwData.txt");
  rwDataFile << std::fixed << std::setprecision(18);
  std::ofstream fwDataFile("fwData.txt");
  fwDataFile << std::fixed << std::setprecision(18);
  std::ofstream stDataFile("stData.txt");
  stDataFile << std::fixed << std::setprecision(18);
  std::ofstream dmiDataFile("dmiData.txt");
  dmiDataFile << std::fixed << std::setprecision(18);
  std::ofstream poseDataFile("poseData.txt");
  poseDataFile << std::fixed << std::setprecision(18);
  std::ofstream velDataFile("velData.txt");
  velDataFile << std::fixed << std::setprecision(18);

  rosbag::Bag bag(argv[1]);
  std::vector<std::string> topics;
  topics.push_back(config.getString(
    "car/calibrator/odometry/sensors/fws/topic"));
  topics.push_back(config.getString(
    "car/calibrator/odometry/sensors/rws/topic"));
  topics.push_back(config.getString(
    "car/calibrator/odometry/sensors/st/topic"));
  topics.push_back(config.getString(
    "car/calibrator/odometry/sensors/dmi/topic"));
  topics.push_back(config.getString(
    "car/calibrator/applanix/vns/topic"));
  topics.push_back(config.getString(
    "car/calibrator/applanix/vnp/topic"));
  rosbag::View view(bag, rosbag::TopicQuery(topics));
  TimestampCorrector<double> timestampCorrectorVns;
  TimestampCorrector<double> timestampCorrectorDmi;
  TimestampCorrector<double> timestampCorrectorFw;
  TimestampCorrector<double> timestampCorrectorRw;
  TimestampCorrector<double> timestampCorrectorSt;
  double lastDMITimestamp = -1;
  double lastDMIDistance = -1;
  bool firstVns = true;
  poslv::VehicleNavigationPerformanceMsgConstPtr lastVnp;
  Transformation m_T_e;
  const EulerAnglesYawPitchRoll ypr;
  Transformation m_T_r_0;
  for (auto it = view.begin(); it != view.end(); ++it) {
    if (it->getTopic() == config.getString(
        "car/calibrator/applanix/vnp/topic")) {
      poslv::VehicleNavigationPerformanceMsgConstPtr vnp(
        it->instantiate<poslv::VehicleNavigationPerformanceMsg>());
      lastVnp = vnp;
    }
    if (it->getTopic() == config.getString(
        "car/calibrator/applanix/vns/topic")) {
      if (!lastVnp)
        continue;
      poslv::VehicleNavigationSolutionMsgConstPtr vns(
        it->instantiate<poslv::VehicleNavigationSolutionMsg>());
      double x_ecef, y_ecef, z_ecef;
      wgs84ToEcef(deg2rad(vns->latitude), deg2rad(vns->longitude),
        vns->altitude, x_ecef, y_ecef, z_ecef);
      if (firstVns) {
        m_T_e = ecef2enu(x_ecef, y_ecef, z_ecef, deg2rad(vns->latitude),
          deg2rad(vns->longitude));
      }
      PoseMeasurement pose;
      pose.m_r_mr = m_T_e * Eigen::Vector3d(x_ecef, y_ecef, z_ecef);
      const Eigen::Matrix3d l_ned_R_r = ypr.parametersToRotationMatrix(
        Eigen::Vector3d(deg2rad(vns->heading), deg2rad(vns->pitch),
        deg2rad(vns->roll)));
      const Transformation e_T_l_ned = ned2ecef(x_ecef, y_ecef, z_ecef,
        deg2rad(vns->latitude), deg2rad(vns->longitude));
      pose.m_R_r = ypr.rotationMatrixToParameters(m_T_e.C() * e_T_l_ned.C() *
        l_ned_R_r);
      pose.sigma2_m_r_mr = Eigen::Vector3d(lastVnp->northPositionRMSError *
        lastVnp->northPositionRMSError, lastVnp->eastPositionRMSError *
        lastVnp->eastPositionRMSError, lastVnp->downPositionRMSError *
        lastVnp->downPositionRMSError).asDiagonal();
      pose.sigma2_m_R_r = Eigen::Vector3d(deg2rad(lastVnp->headingRMSError) *
        deg2rad(lastVnp->headingRMSError), deg2rad(lastVnp->pitchRMSError) *
        deg2rad(lastVnp->pitchRMSError), deg2rad(lastVnp->rollRMSError) *
        deg2rad(lastVnp->rollRMSError)).asDiagonal();
      poseDataFile << vns->header.stamp.toSec() << " " <<
        pose.m_r_mr.transpose() << " " << pose.m_R_r.transpose() <<
        " " << pose.sigma2_m_r_mr.diagonal().transpose() << " " <<
        pose.sigma2_m_R_r.diagonal().transpose() << std::endl;
      if (firstVns) {
        m_T_r_0 = Transformation(
          r2quat(ypr.parametersToRotationMatrix(pose.m_R_r)), pose.m_r_mr);
        firstVns = false;
      }
      VelocitiesMeasurement vel;
      vel.r_v_mr = l_ned_R_r.transpose() * Eigen::Vector3d(vns->northVelocity,
        vns->eastVelocity, vns->downVelocity);
      vel.sigma2_r_v_mr = Eigen::Vector3d(lastVnp->northVelocityRMSError *
        lastVnp->northVelocityRMSError, lastVnp->eastVelocityRMSError *
        lastVnp->eastVelocityRMSError, lastVnp->downVelocityRMSError *
        lastVnp->downVelocityRMSError).asDiagonal();
      vel.r_om_mr = Eigen::Vector3d(deg2rad(vns->angularRateLong),
        deg2rad(vns->angularRateTrans), deg2rad(vns->angularRateDown));
      vel.sigma2_r_om_mr = Eigen::Vector3d(deg2rad(lastVnp->rollRMSError) *
        deg2rad(lastVnp->rollRMSError), deg2rad(lastVnp->pitchRMSError) *
        deg2rad(lastVnp->pitchRMSError), deg2rad(lastVnp->headingRMSError) *
        deg2rad(lastVnp->headingRMSError)).asDiagonal();
      velDataFile << vns->header.stamp.toSec() << " " <<
        vel.r_v_mr.transpose() << " " << vel.r_om_mr.transpose() <<
        " " << vel.sigma2_r_v_mr.diagonal().transpose() << " " <<
        vel.sigma2_r_om_mr.diagonal().transpose() << std::endl;
      auto timestamp = std::round(timestampCorrectorVns.correctTimestamp(
        secToNsec(vns->timeDistance.time1), vns->header.stamp.toNSec()));
      calibrator.addPoseMeasurement(pose, timestamp);
      calibrator.addVelocitiesMeasurement(vel, timestamp);
    }
    if (it->getTopic() == config.getString(
        "car/calibrator/odometry/sensors/fws/topic") && useFw) {
      can_prius::FrontWheelsSpeedMsgConstPtr fws(
        it->instantiate<can_prius::FrontWheelsSpeedMsg>());
      WheelSpeedsMeasurement data;
      data.left = fws->Left;
      data.right = fws->Right;
      auto timestamp = std::round(timestampCorrectorFw.correctTimestamp(
        fws->header.seq, fws->header.stamp.toNSec()));
      calibrator.addFrontWheelsMeasurement(data, timestamp);
      fwDataFile << fws->header.stamp.toSec() << " " << data.left << " "
        << data.right << std::endl;
    }
    if (it->getTopic() == config.getString(
        "car/calibrator/odometry/sensors/rws/topic") && useRw) {
      can_prius::RearWheelsSpeedMsgConstPtr rws(
        it->instantiate<can_prius::RearWheelsSpeedMsg>());
      WheelSpeedsMeasurement data;
      data.left = rws->Left;
      data.right = rws->Right;
      auto timestamp = std::round(timestampCorrectorRw.correctTimestamp(
        rws->header.seq, rws->header.stamp.toNSec()));
      calibrator.addRearWheelsMeasurement(data, timestamp);
      rwDataFile << rws->header.stamp.toSec() << " " << data.left << " "
        << data.right << std::endl;
    }
    if (it->getTopic() == config.getString(
        "car/calibrator/odometry/sensors/st/topic") && useSt) {
      can_prius::Steering1MsgConstPtr st(
        it->instantiate<can_prius::Steering1Msg>());
      SteeringMeasurement data;
      data.value = st->value;
      auto timestamp = std::round(timestampCorrectorSt.correctTimestamp(
        st->header.seq, st->header.stamp.toNSec()));
      calibrator.addSteeringMeasurement(data, timestamp);
      stDataFile << st->header.stamp.toSec() << " " << data.value << std::endl;
    }
    if (it->getTopic() == config.getString(
        "car/calibrator/odometry/sensors/dmi/topic") && useDMI) {
      poslv::TimeTaggedDMIDataMsgConstPtr dmi(
        it->instantiate<poslv::TimeTaggedDMIDataMsg>());
      if (lastDMITimestamp != -1) {
        DMIMeasurement data;
        data.wheelSpeed = (dmi->signedDistanceTraveled - lastDMIDistance) /
          (dmi->timeDistance.time1 - lastDMITimestamp);
        calibrator.addDMIMeasurement(data,
          std::round(timestampCorrectorDmi.correctTimestamp(
          secToNsec(dmi->timeDistance.time1), dmi->header.stamp.toNSec())));
        dmiDataFile << dmi->header.stamp.toSec() << " " << data.wheelSpeed
          << std::endl;
      }
      lastDMITimestamp = dmi->timeDistance.time1;
      lastDMIDistance = dmi->signedDistanceTraveled;
    }
  }

  if (calibrator.unprocessedMeasurements())
    calibrator.predict();

  std::ofstream m_T_v_estFile("m_T_v.txt");
  m_T_v_estFile << std::fixed << std::setprecision(18);
  writeSplines(calibrator.getEstimator(), 0.01, m_T_v_estFile);

  std::ofstream rwDataPredFile("rwDataPred.txt");
  rwDataPredFile << std::fixed << std::setprecision(18);
  std::for_each(calibrator.getRearWheelsPredictions().cbegin(),
    calibrator.getRearWheelsPredictions().cend(), [&](decltype(
    *calibrator.getRearWheelsPredictions().cbegin()) x) {rwDataPredFile
    << nsecToSec(x.first) << " " << x.second.left << " " << x.second.right <<
    std::endl;});
  std::ofstream rwPredError("rwPredError.txt");
  rwPredError << std::fixed << std::setprecision(18);
  std::for_each(calibrator.getRearWheelsPredictionErrors().cbegin(),
    calibrator.getRearWheelsPredictionErrors().cend(), [&](decltype(
    *calibrator.getRearWheelsPredictionErrors().cbegin()) x) {rwPredError
    << x.transpose() << std::endl;});
  std::ofstream fwDataPredFile("fwDataPred.txt");
  fwDataPredFile << std::fixed << std::setprecision(18);
  std::for_each(calibrator.getFrontWheelsPredictions().cbegin(),
    calibrator.getFrontWheelsPredictions().cend(), [&](decltype(
    *calibrator.getFrontWheelsPredictions().cbegin()) x) {fwDataPredFile
    << nsecToSec(x.first) << " " << x.second.left << " " << x.second.right <<
    std::endl;});
  std::ofstream fwPredError("fwPredError.txt");
  fwPredError << std::fixed << std::setprecision(18);
  std::for_each(calibrator.getFrontWheelsPredictionErrors().cbegin(),
    calibrator.getFrontWheelsPredictionErrors().cend(), [&](decltype(
    *calibrator.getFrontWheelsPredictionErrors().cbegin()) x) {fwPredError
    << x.transpose() << std::endl;});
  std::ofstream stDataPredFile("stDataPred.txt");
  stDataPredFile << std::fixed << std::setprecision(18);
  std::for_each(calibrator.getSteeringPredictions().cbegin(),
    calibrator.getSteeringPredictions().cend(), [&](decltype(
    *calibrator.getSteeringPredictions().cbegin()) x) {stDataPredFile
    << nsecToSec(x.first) << " " << x.second.value <<std::endl;});
  std::ofstream stPredError("stPredError.txt");
  stPredError << std::fixed << std::setprecision(18);
  std::for_each(calibrator.getSteeringPredictionErrors().cbegin(),
    calibrator.getSteeringPredictionErrors().cend(), [&](decltype(
    *calibrator.getSteeringPredictionErrors().cbegin()) x) {stPredError
    << x.transpose() << std::endl;});
  std::ofstream dmiDataPredFile("dmiDataPred.txt");
  dmiDataPredFile << std::fixed << std::setprecision(18);
  std::for_each(calibrator.getDMIPredictions().cbegin(),
    calibrator.getDMIPredictions().cend(), [&](decltype(
    *calibrator.getDMIPredictions().cbegin()) x) {dmiDataPredFile
    << nsecToSec(x.first) << " " << x.second.wheelSpeed << std::endl;});
  std::ofstream dmiPredError("dmiPredError.txt");
  dmiPredError << std::fixed << std::setprecision(18);
  std::for_each(calibrator.getDMIPredictionErrors().cbegin(),
    calibrator.getDMIPredictionErrors().cend(), [&](decltype(
    *calibrator.getDMIPredictionErrors().cbegin()) x) {dmiPredError
    << x.transpose() << std::endl;});
  std::ofstream poseDataPredFile("poseDataPred.txt");
  poseDataPredFile << std::fixed << std::setprecision(18);
  std::for_each(calibrator.getPosePredictions().cbegin(),
    calibrator.getPosePredictions().cend(), [&](decltype(
    *calibrator.getPosePredictions().cbegin()) x) {poseDataPredFile
    << nsecToSec(x.first) << " " << x.second.m_r_mr.transpose() << " "
    << x.second.m_R_r.transpose() << std::endl;});
  std::ofstream posePredError("posePredError.txt");
  posePredError << std::fixed << std::setprecision(18);
  std::for_each(calibrator.getPosePredictionErrors().cbegin(),
    calibrator.getPosePredictionErrors().cend(), [&](decltype(
    *calibrator.getPosePredictionErrors().cbegin()) x) {posePredError
    << x.transpose() << std::endl;});
  std::ofstream velDataPredFile("velDataPred.txt");
  velDataPredFile << std::fixed << std::setprecision(18);
  std::for_each(calibrator.getVelocitiesPredictions().cbegin(),
    calibrator.getVelocitiesPredictions().cend(), [&](decltype(
    *calibrator.getVelocitiesPredictions().cbegin()) x) {velDataPredFile
    << nsecToSec(x.first) << " " << x.second.r_v_mr.transpose() << " "
    << x.second.r_om_mr.transpose() << std::endl;});
  std::ofstream velPredError("velPredError.txt");
  velPredError << std::fixed << std::setprecision(18);
  std::for_each(calibrator.getVelocitiesPredictionErrors().cbegin(),
    calibrator.getVelocitiesPredictionErrors().cend(), [&](decltype(
    *calibrator.getVelocitiesPredictionErrors().cbegin()) x) {velPredError
    << x.transpose() << std::endl;});

  return 0;
}


// Copyright Aleksey Gurtovoy 2004
//
// Distributed under 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)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id: partition.cpp 49268 2008-10-11 06:26:17Z agurtovoy $
// $Date: 2008-10-10 23:26:17 -0700 (Fri, 10 Oct 2008) $
// $Revision: 49268 $

#include <boost/mpl/partition.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/back_inserter.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/modulus.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/aux_/test.hpp>

template< typename N > struct is_odd
    : modulus< N, int_<2> >
{
    BOOST_MPL_AUX_LAMBDA_SUPPORT(1, is_odd, (N))
};

MPL_TEST_CASE()
{
    typedef partition<
          range_c<int,0,10>
        , is_odd<_1>
        , mpl::back_inserter< vector<> >
        , mpl::back_inserter< vector<> >
        >::type r;

    MPL_ASSERT(( equal< r::first, vector_c<int,1,3,5,7,9> > ));
    MPL_ASSERT(( equal< r::second, vector_c<int,0,2,4,6,8> > ));
}


//==================================================================================================
/*!

  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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/plus.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/mtwo.hpp>
#include <boost/simd/constant/two.hpp>

//saturated_TODO
STF_CASE_TPL (" plus signed int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;

  using bs::plus;
  using r_t = decltype(bs::saturated_(plus)(T(),T()));
  typedef T wished_r_t;

  // return type conformity test
  STF_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  STF_EQUAL(bs::saturated_(plus)(bs::Mone<T>(), bs::Mone<T>()), bs::Mtwo<T>());
  STF_EQUAL(bs::saturated_(plus)(bs::One<T>(), bs::One<T>()), bs::Two<T>());
  STF_EQUAL(bs::saturated_(plus)(bs::Valmax<T>(),bs::One<T>()), bs::Valmax<T>());
  STF_EQUAL(bs::saturated_(plus)(bs::Valmin<T>(),bs::Mone<T>()), bs::Valmin<T>());
  STF_EQUAL(bs::saturated_(plus)(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
} // end of test for signed_int_

STF_CASE_TPL (" plus unsigned int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;

  using bs::plus;
  using r_t = decltype(bs::saturated_(plus)(T(),T()));
  typedef T wished_r_t;
  // return type conformity test
  STF_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  STF_EQUAL(bs::saturated_(plus)(bs::One<T>(), bs::One<T>()), bs::Two<T>());
  STF_EQUAL(bs::saturated_(plus)(bs::Valmax<T>(),bs::One<T>()), bs::Valmax<T>());
  STF_EQUAL(bs::saturated_(plus)(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
} // end of test for unsigned_int_

STF_CASE_TPL (" plus real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;

  using bs::plus;
  using r_t = decltype(bs::saturated_(plus)(T(),T()));
  typedef T wished_r_t;

  // return type conformity test
  STF_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(bs::saturated_(plus)(bs::Inf<T>(), bs::Inf<T>()), bs::Inf<T>());
  STF_EQUAL(bs::saturated_(plus)(bs::Minf<T>(), bs::Minf<T>()), bs::Minf<T>());
  STF_IEEE_EQUAL(bs::saturated_(plus)(bs::Nan<T>(), bs::Nan<T>()), bs::Nan<T>());
#endif
  STF_EQUAL(bs::saturated_(plus)(bs::Mone<T>(), bs::Mone<T>()), bs::Mtwo<T>());
  STF_EQUAL(bs::saturated_(plus)(bs::One<T>(), bs::One<T>()), bs::Two<T>());
  STF_EQUAL(bs::saturated_(plus)(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
} // end of test for floating_


/*
提供了两种导入数据(double)的方法，一种需要提前知道的数据的尺寸(m*n)，另一种不需要
double ** load(string filename, int m, int n)
void load(string filename, vector<vector<double> > data)
*/
// #include <iostream>
// #include <string>
// #include <vector>
// #include <fstream>
// #include <sstream>
// using namespace std;

#include <Eigen/Dense>
#include "LoadData.hpp"

using namespace Eigen;

// 需要指定数据的行和列
Matrix<double, Dynamic, Dynamic> loadEigen(string filename, int m ,int n){
    Matrix<double, Dynamic, Dynamic> ans(m,n);
    ifstream inFile(filename, ios::in);
	string lineStr; //每一行的结果
    for(int i=0;i<m;i++){
        getline(inFile, lineStr);
        stringstream ss(lineStr);
        string str;
        for(int j=0;j<n;j++){
            getline(ss, str, ',');
            ans(i,j) = atof(str.c_str());
        }
    }
    return ans;
}

double** load(string filename, int m, int n){
    // 初始化数组
    double** ans = (double **)malloc(m * sizeof(double *));
    for(int i=0;i<m;i++)
        ans[i] = (double *)malloc(n * sizeof(double));

    ifstream inFile(filename, ios::in);
	string lineStr; //每一行的结果
    for(int i=0;i<m;i++){
        getline(inFile, lineStr);
        stringstream ss(lineStr);
        string str;
        for(int j=0;j<n;j++){
            getline(ss, str, ',');
            ans[i][j] = atof(str.c_str());
        }
    }
    return ans;
}

void load(string filename, vector<vector<double> >& data){
    ifstream inFile(filename, ios::in);
	string lineStr; //每一行的结果

    // 每行的结果会存在lineStr中
	while (getline(inFile, lineStr))
	{
		// 存成二维表结构
		stringstream ss(lineStr);
		string str;
        vector<double> doubleArray;
		// 按照逗号分隔
		while (getline(ss, str, ',')){
            doubleArray.push_back(atof(str.c_str()));
        }
        data.push_back(doubleArray);
	}

}

// 测试用
void test(){
    // 第一种导入方法测试
    // vector<vector<double> > data;
    // LoadData::load("testX.csv",data);
    // for(int i=0;i<data.size();i++){
    //     for(int j=0;j<data.at(0).size();j++){
    //         cout << data.at(i).at(j) << " ";
    //     }
    //     cout << "\n";
    // }

    // 第二种：导入m行n列的数据
    // int m=3,n=1;
    // double ** ans = load("testX.csv",m,n);
    // for(int i=0;i<m;i++){
    //     for(int j=0;j<n;j++){
    //         cout << ans[i][j] << " ";
    //     }
    //     cout << "\n";
    // }

    // 第三种：导入为Matrix的形式
    int m = 100, n = 1;
    Matrix<double,Dynamic,Dynamic> ans = loadEigen("testX.csv",m,n);
    // ans.resize(m,n);
    cout << ans << endl;
    cout << "Shape " << "( " << ans.rows() << ", " << ans.cols() << " )" << endl;
}

int main(){
    cout << "Hello World" << endl;
    test();
    return -1;
}


#include <boost/test/unit_test.hpp>
#include "algorithms/math/valid_perfect_square.hpp"

BOOST_AUTO_TEST_SUITE(TestValidPerfectSquare)

BOOST_AUTO_TEST_CASE(test_vps_check)
{
    ValidPerfectSquare::Solution solution;
    BOOST_CHECK(false == solution.isPerfectSquare(0));
    BOOST_CHECK(true == solution.isPerfectSquare(1));
    BOOST_CHECK(false == solution.isPerfectSquare(2));
    BOOST_CHECK(false == solution.isPerfectSquare(3));
    BOOST_CHECK(true == solution.isPerfectSquare(4));
    BOOST_CHECK(false == solution.isPerfectSquare(7));
    BOOST_CHECK(true == solution.isPerfectSquare(9));
    BOOST_CHECK(false == solution.isPerfectSquare(15));
    BOOST_CHECK(true == solution.isPerfectSquare(16));
    BOOST_CHECK(true == solution.isPerfectSquare(256));
    BOOST_CHECK(false == solution.isPerfectSquare(257));
    BOOST_CHECK(false == solution.isPerfectSquare(125348));
}

BOOST_AUTO_TEST_SUITE_END()


// Bring in gtest
#include <gtest/gtest.h>
#include <boost/cstdint.hpp>

// Helpful functions from libasrl
#include <sm/eigen/gtest.hpp>

#include <sparse_block_matrix/linear_solver_cholmod.h>
#include <sparse_block_matrix/linear_solver_dense.h>
#include <sparse_block_matrix/linear_solver_spqr.h>

template<typename SOLVER_T>
void randomSparseBlockMatrix(sparse_block_matrix::SparseBlockMatrix<typename SOLVER_T::matrix_t> * A,   Eigen::MatrixXd & Adense ) {
	typedef typename SOLVER_T::matrix_t SparseMatrixBlock;

	// Fill in like this:
	  //   0  1   2
	  // 0 a
	  // 1    b   d
	  // 2        c
	  //
	  // where a,b,c are symmetric
	  // d is 3x5

	  SparseMatrixBlock * a = A->block(0,0,true);
	  ASSERT_TRUE(a != NULL);
	  ASSERT_EQ(a->rows(),3);
	  ASSERT_EQ(a->cols(),3);
	  a->setRandom();
	  *a = (*a * a->transpose()) + Eigen::Matrix3d::Identity();
	  Adense.block(0,0,3,3) = *a;

	  SparseMatrixBlock * b = A->block(1,1,true);
	  ASSERT_TRUE(b != NULL);
	  ASSERT_EQ(b->rows(),3);
	  ASSERT_EQ(b->cols(),3);
	  b->setRandom();
	  *b = (*b * b->transpose()) + Eigen::Matrix3d::Identity();
	  Adense.block(3,3,3,3) = *b;

	  SparseMatrixBlock * c = A->block(2,2,true);
	  ASSERT_TRUE(c != NULL);
	  ASSERT_EQ(c->rows(),5);
	  ASSERT_EQ(c->cols(),5);
	  c->setRandom();
	  *c = (*c * c->transpose()).eval() + Eigen::MatrixXd::Identity(5,5);
	  //std::cout << "c:\n" << *c << std::endl;
	  Adense.block(6,6,5,5) = *c;

	  SparseMatrixBlock * d = A->block(1,2,true);
	  ASSERT_TRUE(d != NULL);
	  ASSERT_EQ(d->rows(),3);
	  ASSERT_EQ(d->cols(),5);
	  d->setRandom();
	  Adense.block(3,6,3,5) = *d;

}

template<typename SOLVER_T>
void testSolver(const std::string & solver_name)
{
  // Build up a sparse matrix
  // 3x3 3x3 3x5
  // 3x3 3x3 3x5
  // 5x3 5x3 5x5
  int rows[] = {3,6,11};
  int cols[] = {3,6,11};
  sparse_block_matrix::SparseBlockMatrix<typename SOLVER_T::matrix_t> A(rows,cols,3,3);

  Eigen::MatrixXd Adense(11,11);
  Adense.setZero();

  ASSERT_EQ(A.rows(),11);
  ASSERT_EQ(A.cols(),11);

  randomSparseBlockMatrix<SOLVER_T>(&A, Adense);

  Eigen::VectorXd xx(A.rows());
  xx.setZero();
  Eigen::VectorXd xx2(A.rows());
  xx2.setZero();

  SOLVER_T solver;
  ASSERT_TRUE(solver.init());

    Eigen::VectorXd bb(A.rows());
    bb.setRandom();

  // virtual bool solve(const SparseBlockMatrix<MatrixType>& A, double* x, double* b) = 0;
 	ASSERT_TRUE( solver.solve(A,&xx[0],&bb[0]));
 	// Solve dense
 	Eigen::VectorXd dx = Adense.selfadjointView<Eigen::Upper>().ldlt().solve(bb);

 	//  always solve twice to make sure the value only copying is working for symbolic factorizations
 	randomSparseBlockMatrix<SOLVER_T>(&A, Adense);
 	ASSERT_TRUE( solver.solve(A,&xx2[0],&bb[0]));

 	Eigen::VectorXd dx2 = Adense.selfadjointView<Eigen::Upper>().ldlt().solve(bb);

 	// Solve dense

 	// Make sure the solutions match.
 	sm::eigen::assertNear(dx,xx,1e-10,SM_SOURCE_FILE_POS, "A: dense solution, B: solution from " + solver_name);
 	sm::eigen::assertNear(dx2,xx2,1e-10,SM_SOURCE_FILE_POS, "A: dense solution, B: solution from " + solver_name);

}

// Check that the setup as a whole is correct
TEST(g2oTestSuite, testCholmod)
{

//
   testSolver< sparse_block_matrix::LinearSolverQr<Eigen::MatrixXd> >("sparseQR");

  testSolver< sparse_block_matrix::LinearSolverCholmod<Eigen::MatrixXd> >("cholmod");

  testSolver< sparse_block_matrix::LinearSolverDense<Eigen::MatrixXd> >("dense");

}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file   testPose3.cpp
 * @brief  Unit tests for Pose3 class
 */

#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/lieProxies.h>

#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;

#include <CppUnitLite/TestHarness.h>
#include <cmath>

using namespace std;
using namespace gtsam;

GTSAM_CONCEPT_TESTABLE_INST(Pose3)
GTSAM_CONCEPT_LIE_INST(Pose3)

static const Point3 P(0.2,0.7,-2);
static const Rot3 R = Rot3::Rodrigues(0.3,0,0);
static const Point3 P2(3.5,-8.2,4.2);
static const Pose3 T(R,P2);
static const Pose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),P2);
static const Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3));
static const double tol=1e-5;

/* ************************************************************************* */
TEST( Pose3, equals)
{
  Pose3 pose2 = T3;
  EXPECT(T3.equals(pose2));
  Pose3 origin;
  EXPECT(!T3.equals(origin));
}

/* ************************************************************************* */
TEST( Pose3, constructors)
{
  Pose3 expected(Rot3::Rodrigues(0,0,3),Point3(1,2,0));
  Pose2 pose2(1,2,3);
  EXPECT(assert_equal(expected,Pose3(pose2)));
}

/* ************************************************************************* */
#ifndef GTSAM_POSE3_EXPMAP
TEST( Pose3, retract_first_order)
{
  Pose3 id;
  Vector v = Z_6x1;
  v(0) = 0.3;
  EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), id.retract(v),1e-2));
  v(3)=0.2;v(4)=0.7;v(5)=-2;
  EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2));
}
#endif
/* ************************************************************************* */
TEST( Pose3, retract_expmap)
{
  Vector v = Z_6x1; v(0) = 0.3;
  Pose3 pose = Pose3::Expmap(v);
  EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), pose, 1e-2));
  EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2));
}

/* ************************************************************************* */
TEST( Pose3, expmap_a_full)
{
  Pose3 id;
  Vector v = Z_6x1;
  v(0) = 0.3;
  EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3(0,0,0))));
  v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
  EXPECT(assert_equal(Pose3(R, P),expmap_default<Pose3>(id, v),1e-5));
}

/* ************************************************************************* */
TEST( Pose3, expmap_a_full2)
{
  Pose3 id;
  Vector v = Z_6x1;
  v(0) = 0.3;
  EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3(0,0,0))));
  v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
  EXPECT(assert_equal(Pose3(R, P),expmap_default<Pose3>(id, v),1e-5));
}

/* ************************************************************************* */
TEST(Pose3, expmap_b)
{
  Pose3 p1(Rot3(), Point3(100, 0, 0));
  Pose3 p2 = p1.retract((Vector(6) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0).finished());
  Pose3 expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0));
  EXPECT(assert_equal(expected, p2,1e-2));
}

/* ************************************************************************* */
// test case for screw motion in the plane
namespace screwPose3 {
  double a=0.3, c=cos(a), s=sin(a), w=0.3;
  Vector xi = (Vector(6) << 0.0, 0.0, w, w, 0.0, 1.0).finished();
  Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1);
  Point3 expectedT(0.29552, 0.0446635, 1);
  Pose3 expected(expectedR, expectedT);
}

/* ************************************************************************* */
// Checks correct exponential map (Expmap) with brute force matrix exponential
TEST(Pose3, expmap_c_full)
{
  EXPECT(assert_equal(screwPose3::expected, expm<Pose3>(screwPose3::xi),1e-6));
  EXPECT(assert_equal(screwPose3::expected, Pose3::Expmap(screwPose3::xi),1e-6));
}

/* ************************************************************************* */
// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
TEST(Pose3, Adjoint_full)
{
  Pose3 expected = T * Pose3::Expmap(screwPose3::xi) * T.inverse();
  Vector xiprime = T.Adjoint(screwPose3::xi);
  EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6));

  Pose3 expected2 = T2 * Pose3::Expmap(screwPose3::xi) * T2.inverse();
  Vector xiprime2 = T2.Adjoint(screwPose3::xi);
  EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6));

  Pose3 expected3 = T3 * Pose3::Expmap(screwPose3::xi) * T3.inverse();
  Vector xiprime3 = T3.Adjoint(screwPose3::xi);
  EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
}

/* ************************************************************************* */
// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
TEST(Pose3, Adjoint_hat)
{
  auto hat = [](const Vector& xi) { return ::wedge<Pose3>(xi); };
  Matrix4 expected = T.matrix() * hat(screwPose3::xi) * T.matrix().inverse();
  Matrix4 xiprime = hat(T.Adjoint(screwPose3::xi));
  EXPECT(assert_equal(expected, xiprime, 1e-6));

  Matrix4 expected2 = T2.matrix() * hat(screwPose3::xi) * T2.matrix().inverse();
  Matrix4 xiprime2 = hat(T2.Adjoint(screwPose3::xi));
  EXPECT(assert_equal(expected2, xiprime2, 1e-6));

  Matrix4 expected3 = T3.matrix() * hat(screwPose3::xi) * T3.matrix().inverse();
  Matrix4 xiprime3 = hat(T3.Adjoint(screwPose3::xi));
  EXPECT(assert_equal(expected3, xiprime3, 1e-6));
}

/* ************************************************************************* */
/** Agrawal06iros version of exponential map */
Pose3 Agrawal06iros(const Vector& xi) {
  Vector w = xi.head(3);
  Vector v = xi.tail(3);
  double t = w.norm();
  if (t < 1e-5)
    return Pose3(Rot3(), Point3(v));
  else {
    Matrix W = skewSymmetric(w/t);
    Matrix A = I_3x3 + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
    return Pose3(Rot3::Expmap (w), Point3(A * v));
  }
}

/* ************************************************************************* */
TEST(Pose3, expmaps_galore_full)
{
  Vector xi; Pose3 actual;
  xi = (Vector(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6).finished();
  actual = Pose3::Expmap(xi);
  EXPECT(assert_equal(expm<Pose3>(xi), actual,1e-6));
  EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
  EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6));

  xi = (Vector(6) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6).finished();
  for (double theta=1.0;0.3*theta<=M_PI;theta*=2) {
    Vector txi = xi*theta;
    actual = Pose3::Expmap(txi);
    EXPECT(assert_equal(expm<Pose3>(txi,30), actual,1e-6));
    EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6));
    Vector log = Pose3::Logmap(actual);
    EXPECT(assert_equal(actual, Pose3::Expmap(log),1e-6));
    EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps
  }

  // Works with large v as well, but expm needs 10 iterations!
  xi = (Vector(6) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0).finished();
  actual = Pose3::Expmap(xi);
  EXPECT(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
  EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-9));
  EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-9));
}

/* ************************************************************************* */
// Check translation and its pushforward
TEST(Pose3, translation) {
  Matrix actualH;
  EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8));

  Matrix numericalH = numericalDerivative11<Point3, Pose3>(
      boost::bind(&Pose3::translation, _1, boost::none), T);
  EXPECT(assert_equal(numericalH, actualH, 1e-6));
}

/* ************************************************************************* */
// Check rotation and its pushforward
TEST(Pose3, rotation) {
  Matrix actualH;
  EXPECT(assert_equal(R, T.rotation(actualH), 1e-8));

  Matrix numericalH = numericalDerivative11<Rot3, Pose3>(
      boost::bind(&Pose3::rotation, _1, boost::none), T);
  EXPECT(assert_equal(numericalH, actualH, 1e-6));
}

/* ************************************************************************* */
TEST(Pose3, Adjoint_compose_full)
{
  // To debug derivatives of compose, assert that
  // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
  const Pose3& T1 = T;
  Vector x = (Vector(6) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8).finished();
  Pose3 expected = T1 * Pose3::Expmap(x) * T2;
  Vector y = T2.inverse().Adjoint(x);
  Pose3 actual = T1 * T2 * Pose3::Expmap(y);
  EXPECT(assert_equal(expected, actual, 1e-6));
}

/* ************************************************************************* */
// Check compose and its pushforward
// NOTE: testing::compose<Pose3>(t1,t2) = t1.compose(t2)  (see lieProxies.h)
TEST( Pose3, compose )
{
  Matrix actual = (T2*T2).matrix();
  Matrix expected = T2.matrix()*T2.matrix();
  EXPECT(assert_equal(actual,expected,1e-8));

  Matrix actualDcompose1, actualDcompose2;
  T2.compose(T2, actualDcompose1, actualDcompose2);

  Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T2, T2);
  EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
  EXPECT(assert_equal(T2.inverse().AdjointMap(),actualDcompose1,5e-3));

  Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2);
  EXPECT(assert_equal(numericalH2,actualDcompose2,1e-4));
}

/* ************************************************************************* */
// Check compose and its pushforward, another case
TEST( Pose3, compose2 )
{
  const Pose3& T1 = T;
  Matrix actual = (T1*T2).matrix();
  Matrix expected = T1.matrix()*T2.matrix();
  EXPECT(assert_equal(actual,expected,1e-8));

  Matrix actualDcompose1, actualDcompose2;
  T1.compose(T2, actualDcompose1, actualDcompose2);

  Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T1, T2);
  EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
  EXPECT(assert_equal(T2.inverse().AdjointMap(),actualDcompose1,5e-3));

  Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T1, T2);
  EXPECT(assert_equal(numericalH2,actualDcompose2,1e-5));
}

/* ************************************************************************* */
TEST( Pose3, inverse)
{
  Matrix actualDinverse;
  Matrix actual = T.inverse(actualDinverse).matrix();
  Matrix expected = T.matrix().inverse();
  EXPECT(assert_equal(actual,expected,1e-8));

  Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T);
  EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
  EXPECT(assert_equal(-T.AdjointMap(),actualDinverse,5e-3));
}

/* ************************************************************************* */
TEST( Pose3, inverseDerivatives2)
{
  Rot3 R = Rot3::Rodrigues(0.3,0.4,-0.5);
  Point3 t(3.5,-8.2,4.2);
  Pose3 T(R,t);

  Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T);
  Matrix actualDinverse;
  T.inverse(actualDinverse);
  EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
  EXPECT(assert_equal(-T.AdjointMap(),actualDinverse,5e-3));
}

/* ************************************************************************* */
TEST( Pose3, compose_inverse)
{
  Matrix actual = (T*T.inverse()).matrix();
  Matrix expected = I_4x4;
  EXPECT(assert_equal(actual,expected,1e-8));
}

/* ************************************************************************* */
Point3 transform_from_(const Pose3& pose, const Point3& point) { return pose.transform_from(point); }
TEST( Pose3, Dtransform_from1_a)
{
  Matrix actualDtransform_from1;
  T.transform_from(P, actualDtransform_from1, boost::none);
  Matrix numerical = numericalDerivative21(transform_from_,T,P);
  EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8));
}

TEST( Pose3, Dtransform_from1_b)
{
  Pose3 origin;
  Matrix actualDtransform_from1;
  origin.transform_from(P, actualDtransform_from1, boost::none);
  Matrix numerical = numericalDerivative21(transform_from_,origin,P);
  EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8));
}

TEST( Pose3, Dtransform_from1_c)
{
  Point3 origin(0,0,0);
  Pose3 T0(R,origin);
  Matrix actualDtransform_from1;
  T0.transform_from(P, actualDtransform_from1, boost::none);
  Matrix numerical = numericalDerivative21(transform_from_,T0,P);
  EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8));
}

TEST( Pose3, Dtransform_from1_d)
{
  Rot3 I;
  Point3 t0(100,0,0);
  Pose3 T0(I,t0);
  Matrix actualDtransform_from1;
  T0.transform_from(P, actualDtransform_from1, boost::none);
  //print(computed, "Dtransform_from1_d computed:");
  Matrix numerical = numericalDerivative21(transform_from_,T0,P);
  //print(numerical, "Dtransform_from1_d numerical:");
  EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8));
}

/* ************************************************************************* */
TEST( Pose3, Dtransform_from2)
{
  Matrix actualDtransform_from2;
  T.transform_from(P, boost::none, actualDtransform_from2);
  Matrix numerical = numericalDerivative22(transform_from_,T,P);
  EXPECT(assert_equal(numerical,actualDtransform_from2,1e-8));
}

/* ************************************************************************* */
Point3 transform_to_(const Pose3& pose, const Point3& point) { return pose.transform_to(point); }
TEST( Pose3, Dtransform_to1)
{
  Matrix computed;
  T.transform_to(P, computed, boost::none);
  Matrix numerical = numericalDerivative21(transform_to_,T,P);
  EXPECT(assert_equal(numerical,computed,1e-8));
}

/* ************************************************************************* */
TEST( Pose3, Dtransform_to2)
{
  Matrix computed;
  T.transform_to(P, boost::none, computed);
  Matrix numerical = numericalDerivative22(transform_to_,T,P);
  EXPECT(assert_equal(numerical,computed,1e-8));
}

/* ************************************************************************* */
TEST( Pose3, transform_to_with_derivatives)
{
  Matrix actH1, actH2;
  T.transform_to(P,actH1,actH2);
  Matrix expH1 = numericalDerivative21(transform_to_, T,P),
       expH2 = numericalDerivative22(transform_to_, T,P);
  EXPECT(assert_equal(expH1, actH1, 1e-8));
  EXPECT(assert_equal(expH2, actH2, 1e-8));
}

/* ************************************************************************* */
TEST( Pose3, transform_from_with_derivatives)
{
  Matrix actH1, actH2;
  T.transform_from(P,actH1,actH2);
  Matrix expH1 = numericalDerivative21(transform_from_, T,P),
       expH2 = numericalDerivative22(transform_from_, T,P);
  EXPECT(assert_equal(expH1, actH1, 1e-8));
  EXPECT(assert_equal(expH2, actH2, 1e-8));
}

/* ************************************************************************* */
TEST( Pose3, transform_to_translate)
{
    Point3 actual = Pose3(Rot3(), Point3(1, 2, 3)).transform_to(Point3(10.,20.,30.));
    Point3 expected(9.,18.,27.);
    EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST( Pose3, transform_to_rotate)
{
    Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(0,0,0));
    Point3 actual = transform.transform_to(Point3(2,1,10));
    Point3 expected(-1,2,10);
    EXPECT(assert_equal(expected, actual, 0.001));
}

/* ************************************************************************* */
TEST( Pose3, transform_to)
{
    Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(2,4, 0));
    Point3 actual = transform.transform_to(Point3(3,2,10));
    Point3 expected(2,1,10);
    EXPECT(assert_equal(expected, actual, 0.001));
}

Pose3 transform_pose_to_(const Pose3& pose, const Pose3& pose2) { return pose.transform_pose_to(pose2); }

/* ************************************************************************* */
TEST( Pose3, transform_pose_to)
{
  Pose3 origin = T.transform_pose_to(T);
  EXPECT(assert_equal(Pose3{}, origin));
}

/* ************************************************************************* */
TEST( Pose3, transform_pose_to_with_derivatives)
{
  Matrix actH1, actH2;
  Pose3 res = T.transform_pose_to(T2,actH1,actH2);
  EXPECT(assert_equal(res, T.inverse().compose(T2)));

  Matrix expH1 = numericalDerivative21(transform_pose_to_, T, T2),
       expH2 = numericalDerivative22(transform_pose_to_, T, T2);
  EXPECT(assert_equal(expH1, actH1, 1e-8));
  EXPECT(assert_equal(expH2, actH2, 1e-8));
}

/* ************************************************************************* */
TEST( Pose3, transform_pose_to_with_derivatives2)
{
  Matrix actH1, actH2;
  Pose3 res = T.transform_pose_to(T3,actH1,actH2);
  EXPECT(assert_equal(res, T.inverse().compose(T3)));

  Matrix expH1 = numericalDerivative21(transform_pose_to_, T, T3),
       expH2 = numericalDerivative22(transform_pose_to_, T, T3);
  EXPECT(assert_equal(expH1, actH1, 1e-8));
  EXPECT(assert_equal(expH2, actH2, 1e-8));
}

/* ************************************************************************* */
TEST( Pose3, transform_from)
{
    Point3 actual = T3.transform_from(Point3(0,0,0));
    Point3 expected = Point3(1.,2.,3.);
    EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST( Pose3, transform_roundtrip)
{
    Point3 actual = T3.transform_from(T3.transform_to(Point3(12., -0.11,7.0)));
    Point3 expected(12., -0.11,7.0);
    EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST( Pose3, transformPose_to_origin)
{
    // transform to origin
    Pose3 actual = T3.transform_to(Pose3());
    EXPECT(assert_equal(T3, actual, 1e-8));
}

/* ************************************************************************* */
TEST( Pose3, transformPose_to_itself)
{
    // transform to itself
    Pose3 actual = T3.transform_to(T3);
    EXPECT(assert_equal(Pose3(), actual, 1e-8));
}

/* ************************************************************************* */
TEST( Pose3, transformPose_to_translation)
{
    // transform translation only
    Rot3 r = Rot3::Rodrigues(-1.570796,0,0);
    Pose3 pose2(r, Point3(21.,32.,13.));
    Pose3 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3)));
    Pose3 expected(r, Point3(20.,30.,10.));
    EXPECT(assert_equal(expected, actual, 1e-8));
}

/* ************************************************************************* */
TEST( Pose3, transformPose_to_simple_rotate)
{
    // transform translation only
    Rot3 r = Rot3::Rodrigues(0,0,-1.570796);
    Pose3 pose2(r, Point3(21.,32.,13.));
    Pose3 transform(r, Point3(1,2,3));
    Pose3 actual = pose2.transform_to(transform);
    Pose3 expected(Rot3(), Point3(-30.,20.,10.));
    EXPECT(assert_equal(expected, actual, 0.001));
}

/* ************************************************************************* */
TEST( Pose3, transformPose_to)
{
    // transform to
    Rot3 r = Rot3::Rodrigues(0,0,-1.570796); //-90 degree yaw
    Rot3 r2 = Rot3::Rodrigues(0,0,0.698131701); //40 degree yaw
    Pose3 pose2(r2, Point3(21.,32.,13.));
    Pose3 transform(r, Point3(1,2,3));
    Pose3 actual = pose2.transform_to(transform);
    Pose3 expected(Rot3::Rodrigues(0,0,2.26892803), Point3(-30.,20.,10.));
    EXPECT(assert_equal(expected, actual, 0.001));
}

/* ************************************************************************* */
TEST(Pose3, Retract_LocalCoordinates)
{
  Vector6 d;
  d << 1,2,3,4,5,6; d/=10;
  const Rot3 R = Rot3::Retract(d.head<3>());
  Pose3 t = Pose3::Retract(d);
  EXPECT(assert_equal(d, Pose3::LocalCoordinates(t)));
}
/* ************************************************************************* */
TEST(Pose3, retract_localCoordinates)
{
  Vector6 d12;
  d12 << 1,2,3,4,5,6; d12/=10;
  Pose3 t1 = T, t2 = t1.retract(d12);
  EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
}
/* ************************************************************************* */
TEST(Pose3, expmap_logmap)
{
  Vector d12 = Vector6::Constant(0.1);
  Pose3 t1 = T, t2 = t1.expmap(d12);
  EXPECT(assert_equal(d12, t1.logmap(t2)));
}

/* ************************************************************************* */
TEST(Pose3, retract_localCoordinates2)
{
  Pose3 t1 = T;
  Pose3 t2 = T3;
  Pose3 origin;
  Vector d12 = t1.localCoordinates(t2);
  EXPECT(assert_equal(t2, t1.retract(d12)));
  Vector d21 = t2.localCoordinates(t1);
  EXPECT(assert_equal(t1, t2.retract(d21)));
  // TODO(hayk): This currently fails!
  // EXPECT(assert_equal(d12, -d21));
}
/* ************************************************************************* */
TEST(Pose3, manifold_expmap)
{
  Pose3 t1 = T;
  Pose3 t2 = T3;
  Pose3 origin;
  Vector d12 = t1.logmap(t2);
  EXPECT(assert_equal(t2, t1.expmap(d12)));
  Vector d21 = t2.logmap(t1);
  EXPECT(assert_equal(t1, t2.expmap(d21)));

  // Check that log(t1,t2)=-log(t2,t1)
  EXPECT(assert_equal(d12,-d21));
}

/* ************************************************************************* */
TEST(Pose3, subgroups)
{
  // Frank - Below only works for correct "Agrawal06iros style expmap
  // lines in canonical coordinates correspond to Abelian subgroups in SE(3)
   Vector d = (Vector(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6).finished();
  // exp(-d)=inverse(exp(d))
   EXPECT(assert_equal(Pose3::Expmap(-d),Pose3::Expmap(d).inverse()));
  // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
   Pose3 T2 = Pose3::Expmap(2*d);
   Pose3 T3 = Pose3::Expmap(3*d);
   Pose3 T5 = Pose3::Expmap(5*d);
   EXPECT(assert_equal(T5,T2*T3));
   EXPECT(assert_equal(T5,T3*T2));
}

/* ************************************************************************* */
TEST( Pose3, between )
{
  Pose3 expected = T2.inverse() * T3;
  Matrix actualDBetween1,actualDBetween2;
  Pose3 actual = T2.between(T3, actualDBetween1,actualDBetween2);
  EXPECT(assert_equal(expected,actual));

  Matrix numericalH1 = numericalDerivative21(testing::between<Pose3> , T2, T3);
  EXPECT(assert_equal(numericalH1,actualDBetween1,5e-3));

  Matrix numericalH2 = numericalDerivative22(testing::between<Pose3> , T2, T3);
  EXPECT(assert_equal(numericalH2,actualDBetween2,1e-5));
}

/* ************************************************************************* */
// some shared test values - pulled from equivalent test in Pose2
Point3 l1(1, 0, 0), l2(1, 1, 0), l3(2, 2, 0), l4(1, 4,-4);
Pose3 x1, x2(Rot3::Ypr(0.0, 0.0, 0.0), l2), x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2);
Pose3
    xl1(Rot3::Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)),
    xl2(Rot3::Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)),
    xl3(Rot3::Ypr(1.0, 0.0, 0.0), Point3(2, 2, 0)),
    xl4(Rot3::Ypr(0.0, 0.0, 1.0), Point3(1, 4,-4));

/* ************************************************************************* */
double range_proxy(const Pose3& pose, const Point3& point) {
  return pose.range(point);
}
TEST( Pose3, range )
{
  Matrix expectedH1, actualH1, expectedH2, actualH2;

  // establish range is indeed zero
  EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9);

  // establish range is indeed sqrt2
  EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(l2),1e-9);

  // Another pair
  double actual23 = x2.range(l3, actualH1, actualH2);
  EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);

  // Check numerical derivatives
  expectedH1 = numericalDerivative21(range_proxy, x2, l3);
  expectedH2 = numericalDerivative22(range_proxy, x2, l3);
  EXPECT(assert_equal(expectedH1,actualH1));
  EXPECT(assert_equal(expectedH2,actualH2));

  // Another test
  double actual34 = x3.range(l4, actualH1, actualH2);
  EXPECT_DOUBLES_EQUAL(5,actual34,1e-9);

  // Check numerical derivatives
  expectedH1 = numericalDerivative21(range_proxy, x3, l4);
  expectedH2 = numericalDerivative22(range_proxy, x3, l4);
  EXPECT(assert_equal(expectedH1,actualH1));
  EXPECT(assert_equal(expectedH2,actualH2));
}

/* ************************************************************************* */
double range_pose_proxy(const Pose3& pose, const Pose3& point) {
  return pose.range(point);
}
TEST( Pose3, range_pose )
{
  Matrix expectedH1, actualH1, expectedH2, actualH2;

  // establish range is indeed zero
  EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);

  // establish range is indeed sqrt2
  EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(xl2),1e-9);

  // Another pair
  double actual23 = x2.range(xl3, actualH1, actualH2);
  EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);

  // Check numerical derivatives
  expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3);
  expectedH2 = numericalDerivative22(range_pose_proxy, x2, xl3);
  EXPECT(assert_equal(expectedH1,actualH1));
  EXPECT(assert_equal(expectedH2,actualH2));

  // Another test
  double actual34 = x3.range(xl4, actualH1, actualH2);
  EXPECT_DOUBLES_EQUAL(5,actual34,1e-9);

  // Check numerical derivatives
  expectedH1 = numericalDerivative21(range_pose_proxy, x3, xl4);
  expectedH2 = numericalDerivative22(range_pose_proxy, x3, xl4);
  EXPECT(assert_equal(expectedH1,actualH1));
  EXPECT(assert_equal(expectedH2,actualH2));
}

/* ************************************************************************* */
Unit3 bearing_proxy(const Pose3& pose, const Point3& point) {
  return pose.bearing(point);
}
TEST(Pose3, Bearing) {
  Matrix expectedH1, actualH1, expectedH2, actualH2;
  EXPECT(assert_equal(Unit3(1, 0, 0), x1.bearing(l1, actualH1, actualH2), 1e-9));

  // Check numerical derivatives
  expectedH1 = numericalDerivative21(bearing_proxy, x1, l1);
  expectedH2 = numericalDerivative22(bearing_proxy, x1, l1);
  EXPECT(assert_equal(expectedH1, actualH1));
  EXPECT(assert_equal(expectedH2, actualH2));
}

TEST(Pose3, Bearing2) {
  Matrix expectedH1, actualH1, expectedH2, actualH2;
  EXPECT(assert_equal(Unit3(0,0.6,-0.8), x2.bearing(l4, actualH1, actualH2), 1e-9));

  // Check numerical derivatives
  expectedH1 = numericalDerivative21(bearing_proxy, x2, l4);
  expectedH2 = numericalDerivative22(bearing_proxy, x2, l4);
  EXPECT(assert_equal(expectedH1, actualH1));
  EXPECT(assert_equal(expectedH2, actualH2));
}

TEST(Pose3, PoseToPoseBearing) {
  Matrix expectedH1, actualH1, expectedH2, actualH2, H2block;
  EXPECT(assert_equal(Unit3(0,1,0), xl1.bearing(xl2, actualH1, actualH2), 1e-9));

  // Check numerical derivatives
  expectedH1 = numericalDerivative21(bearing_proxy, xl1, l2);

  // Since the second pose is treated as a point, the value calculated by
  // numericalDerivative22 only depends on the position of the pose. Here, we
  // calculate the Jacobian w.r.t. the second pose's position, and then augment
  // that with zeroes in the block that is w.r.t. the second pose's
  // orientation.
  H2block = numericalDerivative22(bearing_proxy, xl1, l2);
  expectedH2 = Matrix(2, 6);
  expectedH2.setZero();
  expectedH2.block<2, 3>(0, 3) = H2block;

  EXPECT(assert_equal(expectedH1, actualH1));
  EXPECT(assert_equal(expectedH2, actualH2));
}

/* ************************************************************************* */
TEST( Pose3, unicycle )
{
  // velocity in X should be X in inertial frame, rather than global frame
  Vector x_step = Vector::Unit(6,3)*1.0;
  EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), l1), expmap_default<Pose3>(x1, x_step), tol));
  EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), Point3(2,1,0)), expmap_default<Pose3>(x2, x_step), tol));
  EXPECT(assert_equal(Pose3(Rot3::Ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default<Pose3>(x3, sqrt(2.0) * x_step), tol));
}

/* ************************************************************************* */
TEST( Pose3, adjointMap) {
  Matrix res = Pose3::adjointMap(screwPose3::xi);
  Matrix wh = skewSymmetric(screwPose3::xi(0), screwPose3::xi(1), screwPose3::xi(2));
  Matrix vh = skewSymmetric(screwPose3::xi(3), screwPose3::xi(4), screwPose3::xi(5));
  Matrix6 expected;
  expected << wh, Z_3x3, vh, wh;
  EXPECT(assert_equal(expected,res,1e-5));
}

/* ************************************************************************* */
TEST(Pose3, Align1) {
  Pose3 expected(Rot3(), Point3(10,10,0));

  vector<Point3Pair> correspondences;
  Point3Pair ab1(make_pair(Point3(10,10,0), Point3(0,0,0)));
  Point3Pair ab2(make_pair(Point3(30,20,0), Point3(20,10,0)));
  Point3Pair ab3(make_pair(Point3(20,30,0), Point3(10,20,0)));
  correspondences += ab1, ab2, ab3;

  boost::optional<Pose3> actual = Pose3::Align(correspondences);
  EXPECT(assert_equal(expected, *actual));
}

/* ************************************************************************* */
TEST(Pose3, Align2) {
  Point3 t(20,10,5);
  Rot3 R = Rot3::RzRyRx(0.3, 0.2, 0.1);
  Pose3 expected(R, t);

  vector<Point3Pair> correspondences;
  Point3 p1(0,0,1), p2(10,0,2), p3(20,-10,30);
  Point3 q1 = expected.transform_from(p1),
         q2 = expected.transform_from(p2),
         q3 = expected.transform_from(p3);
  Point3Pair ab1(make_pair(q1, p1));
  Point3Pair ab2(make_pair(q2, p2));
  Point3Pair ab3(make_pair(q3, p3));
  correspondences += ab1, ab2, ab3;

  boost::optional<Pose3> actual = Pose3::Align(correspondences);
  EXPECT(assert_equal(expected, *actual, 1e-5));
}

/* ************************************************************************* */
TEST( Pose3, ExpmapDerivative1) {
  Matrix6 actualH;
  Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
  Pose3::Expmap(w,actualH);
  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none);
  EXPECT(assert_equal(expectedH, actualH));
}

/* ************************************************************************* */
TEST(Pose3, ExpmapDerivative2) {
  // Iserles05an (Lie-group Methods) says:
  // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
  // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
  // where A(t): T -> se(3) is a trajectory in the tangent space of SE(3)
  // and dexp[A] is a linear map from 4*4 to 4*4 derivatives of se(3)
  // Hence, the above matrix equation is typed: 4*4 = SE(3) * linear_map(4*4)

  // In GTSAM, we don't work with the Lie-algebra elements A directly, but with 6-vectors.
  // xi is easy: d Expmap(xi(t)) / dt = ExmapDerivative[xi(t)] * xi'(t)

  // Let's verify the above formula.

  auto xi = [](double t) {
    Vector6 v;
    v << 2 * t, sin(t), 4 * t * t, 2 * t, sin(t), 4 * t * t;
    return v;
  };
  auto xi_dot = [](double t) {
    Vector6 v;
    v << 2, cos(t), 8 * t, 2, cos(t), 8 * t;
    return v;
  };

  // We define a function T
  auto T = [xi](double t) { return Pose3::Expmap(xi(t)); };

  for (double t = -2.0; t < 2.0; t += 0.3) {
    const Matrix expected = numericalDerivative11<Pose3, double>(T, t);
    const Matrix actual = Pose3::ExpmapDerivative(xi(t)) * xi_dot(t);
    CHECK(assert_equal(expected, actual, 1e-7));
  }
}

/* ************************************************************************* */
TEST( Pose3, LogmapDerivative) {
  Matrix6 actualH;
  Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
  Pose3 p = Pose3::Expmap(w);
  EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5));
  Matrix expectedH = numericalDerivative21<Vector6, Pose3,
      OptionalJacobian<6, 6> >(&Pose3::Logmap, p, boost::none);
  EXPECT(assert_equal(expectedH, actualH));
}

/* ************************************************************************* */
Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
  return Pose3::adjointMap(xi) * v;
}

TEST( Pose3, adjoint) {
  Vector expected = testDerivAdjoint(screwPose3::xi, screwPose3::xi);

  Matrix actualH;
  Vector actual = Pose3::adjoint(screwPose3::xi, screwPose3::xi, actualH);

  Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>(
      testDerivAdjoint, screwPose3::xi, screwPose3::xi, 1e-5);

  EXPECT(assert_equal(expected,actual,1e-5));
  EXPECT(assert_equal(numericalH,actualH,1e-5));
}

/* ************************************************************************* */
Vector6 testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) {
  return Pose3::adjointMap(xi).transpose() * v;
}

TEST( Pose3, adjointTranspose) {
  Vector xi = (Vector(6) << 0.01, 0.02, 0.03, 1.0, 2.0, 3.0).finished();
  Vector v = (Vector(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0).finished();
  Vector expected = testDerivAdjointTranspose(xi, v);

  Matrix actualH;
  Vector actual = Pose3::adjointTranspose(xi, v, actualH);

  Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>(
      testDerivAdjointTranspose, xi, v, 1e-5);

  EXPECT(assert_equal(expected,actual,1e-15));
  EXPECT(assert_equal(numericalH,actualH,1e-5));
}

/* ************************************************************************* */
TEST( Pose3, stream)
{
  Pose3 T;
  std::ostringstream os;
  os << T;
  EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n\n[0, 0, 0]';\n");
}

//******************************************************************************
TEST(Pose3 , Invariants) {
  Pose3 id;

  EXPECT(check_group_invariants(id,id));
  EXPECT(check_group_invariants(id,T3));
  EXPECT(check_group_invariants(T2,id));
  EXPECT(check_group_invariants(T2,T3));

  EXPECT(check_manifold_invariants(id,id));
  EXPECT(check_manifold_invariants(id,T3));
  EXPECT(check_manifold_invariants(T2,id));
  EXPECT(check_manifold_invariants(T2,T3));
}

//******************************************************************************
TEST(Pose3 , LieGroupDerivatives) {
  Pose3 id;

  CHECK_LIE_GROUP_DERIVATIVES(id,id);
  CHECK_LIE_GROUP_DERIVATIVES(id,T2);
  CHECK_LIE_GROUP_DERIVATIVES(T2,id);
  CHECK_LIE_GROUP_DERIVATIVES(T2,T3);
}

//******************************************************************************
TEST(Pose3 , ChartDerivatives) {
  Pose3 id;
  if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
    CHECK_CHART_DERIVATIVES(id,id);
//    CHECK_CHART_DERIVATIVES(id,T2);
//    CHECK_CHART_DERIVATIVES(T2,id);
//    CHECK_CHART_DERIVATIVES(T2,T3);
  }
}

/* ************************************************************************* */
TEST(Pose3, interpolate) {
  EXPECT(assert_equal(T2, interpolate(T2,T3, 0.0)));
  EXPECT(assert_equal(T3, interpolate(T2,T3, 1.0)));
}

/* ************************************************************************* */
TEST(Pose3, Create) {
  Matrix63 actualH1, actualH2;
  Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2);
  EXPECT(assert_equal(T, actual));
  boost::function<Pose3(Rot3,Point3)> create = boost::bind(Pose3::Create,_1,_2,boost::none,boost::none);
  EXPECT(assert_equal(numericalDerivative21<Pose3,Rot3,Point3>(create, R, P2), actualH1, 1e-9));
  EXPECT(assert_equal(numericalDerivative22<Pose3,Rot3,Point3>(create, R, P2), actualH2, 1e-9));
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


// test_nc_beta.cpp

// Copyright John Maddock 2008.

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

//
// This must appear *before* any #includes, and precludes pch usage:
//
#define BOOST_MATH_ASSERT_UNDEFINED_POLICY false

#ifdef _MSC_VER
#pragma warning (disable:4127 4512)
#endif

#if !defined(TEST_FLOAT) && !defined(TEST_DOUBLE) && !defined(TEST_LDOUBLE) && !defined(TEST_REAL_CONCEPT)
#  define TEST_FLOAT
#  define TEST_DOUBLE
#  define TEST_LDOUBLE
#  define TEST_REAL_CONCEPT
#endif

#include <boost/math/concepts/real_concept.hpp> // for real_concept
#include <boost/math/distributions/non_central_beta.hpp> // for chi_squared_distribution
#include <boost/math/distributions/poisson.hpp> // for poisson_distribution
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp> // for test_main
#include <boost/test/results_collector.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp> // for BOOST_CHECK_CLOSE

#include "functor.hpp"
#include "handle_test_result.hpp"
#include "test_ncbeta_hooks.hpp"
#include "table_type.hpp"
#include "test_nc_beta.hpp"

#include <iostream>
using std::cout;
using std::endl;
#include <limits>
using std::numeric_limits;

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   const char* largest_type;
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   if(boost::math::policies::digits<double, boost::math::policies::policy<> >() == boost::math::policies::digits<long double, boost::math::policies::policy<> >())
   {
      largest_type = "(long\\s+)?double|real_concept";
   }
   else
   {
      largest_type = "long double|real_concept";
   }
#else
   largest_type = "(long\\s+)?double|real_concept";
#endif

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   if(boost::math::tools::digits<long double>() == 64)
   {
      //
      // Allow a small amount of error leakage from long double to double:
      //
      add_expected_result(
         "[^|]*",                          // compiler
         "[^|]*",                          // stdlib
         "[^|]*",                          // platform
         "double",                         // test type(s)
         "[^|]*large[^|]*",                // test data group
         "[^|]*", 5, 5);                   // test function
   }

   if(boost::math::tools::digits<long double>() == 64)
   {
      add_expected_result(
         "[^|]*",                          // compiler
         "[^|]*",                          // stdlib
         "[^|]*",                          // platform
         largest_type,                     // test type(s)
         "[^|]*medium[^|]*",               // test data group
         "[^|]*", 1200, 500);               // test function
      add_expected_result(
         "[^|]*",                          // compiler
         "[^|]*",                          // stdlib
         "[^|]*",                          // platform
         largest_type,                     // test type(s)
         "[^|]*large[^|]*",                // test data group
         "[^|]*", 40000, 6000);            // test function
   }
#endif
   //
   // Catch all cases come last:
   //
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      largest_type,                     // test type(s)
      "[^|]*medium[^|]*",               // test data group
      "[^|]*", 1500, 500);               // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      "real_concept",                   // test type(s)
      "[^|]*large[^|]*",                // test data group
      "[^|]*", 30000, 4000);             // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      largest_type,                     // test type(s)
      "[^|]*large[^|]*",                // test data group
      "[^|]*", 20000, 2000);             // test function
   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

template <class RealType>
RealType naive_pdf(RealType a, RealType b, RealType lam, RealType x)
{
   using namespace boost::math;

   RealType term = pdf(poisson_distribution<RealType>(lam/2), 0)
      * ibeta_derivative(a, b, x);
   RealType sum = term;

   int i = 1;
   while(term / sum > tools::epsilon<RealType>())
   {
      term = pdf(poisson_distribution<RealType>(lam/2), i)
      * ibeta_derivative(a + i, b, x);
      ++i;
      sum += term;
   }
   return sum;
}

template <class RealType>
void test_spot(
     RealType a,     // alpha
     RealType b,     // beta
     RealType ncp,   // non-centrality param
     RealType cs,    // Chi Square statistic
     RealType P,     // CDF
     RealType Q,     // Complement of CDF
     RealType D,     // PDF
     RealType tol)   // Test tolerance
{
   boost::math::non_central_beta_distribution<RealType> dist(a, b, ncp);
   BOOST_CHECK_CLOSE(
      cdf(dist, cs), P, tol);
   //
   // Sanity checking using the naive PDF calculation above fails at
   // float precision:
   //
   if(!boost::is_same<float, RealType>::value)
   {
      BOOST_CHECK_CLOSE(
         pdf(dist, cs), naive_pdf(dist.alpha(), dist.beta(), ncp, cs), tol);
   }
   BOOST_CHECK_CLOSE(
      pdf(dist, cs), D, tol);

   if((P < 0.99) && (Q < 0.99))
   {
      //
      // We can only check this if P is not too close to 1,
      // so that we can guarantee Q is reasonably free of error:
      //
      BOOST_CHECK_CLOSE(
         cdf(complement(dist, cs)), Q, tol);
      BOOST_CHECK_CLOSE(
            quantile(dist, P), cs, tol * 10);
      BOOST_CHECK_CLOSE(
            quantile(complement(dist, Q)), cs, tol * 10);
   }
}

template <class RealType> // Any floating-point type RealType.
void test_spots(RealType)
{
   RealType tolerance = (std::max)(
      boost::math::tools::epsilon<RealType>() * 100,
      (RealType)1e-6) * 100;
   RealType abs_tolerance = boost::math::tools::epsilon<RealType>() * 100;

   cout << "Tolerance = " << tolerance << "%." << endl;

   //
   // Spot tests use values computed by the R statistical
   // package and the pbeta and dbeta functions:
   //
   test_spot(
     RealType(2),                   // alpha
     RealType(5),                   // beta
     RealType(1),                   // non-centrality param
     RealType(0.25),                // Chi Square statistic
     RealType(0.3658349),           // CDF
     RealType(1-0.3658349),         // Complement of CDF
     RealType(2.184465),            // PDF
     RealType(tolerance));
   test_spot(
     RealType(20),                  // alpha
     RealType(15),                  // beta
     RealType(35),                  // non-centrality param
     RealType(0.75),                // Chi Square statistic
     RealType(0.6994175),           // CDF
     RealType(1-0.6994175),         // Complement of CDF
     RealType(5.576146),            // PDF
     RealType(tolerance));
   test_spot(
     RealType(100),                 // alpha
     RealType(3),                   // beta
     RealType(63),                  // non-centrality param
     RealType(0.95),                // Chi Square statistic
     RealType(0.03529306),          // CDF
     RealType(1-0.03529306),        // Complement of CDF
     RealType(3.637894),            // PDF
     RealType(tolerance));
   test_spot(
     RealType(0.25),                // alpha
     RealType(0.75),                // beta
     RealType(150),                 // non-centrality param
     RealType(0.975),               // Chi Square statistic
     RealType(0.09752216),          // CDF
     RealType(1-0.09752216),        // Complement of CDF
     RealType(8.020935),            // PDF
     RealType(tolerance));

   BOOST_MATH_STD_USING
   boost::math::non_central_beta_distribution<RealType> dist(100, 3, 63);
   BOOST_CHECK_CLOSE(mean(dist), RealType(4.82280451915522329944315287538684030781836554279474240490936e13L) * exp(-RealType(31.5)) * 100 / 103, tolerance);
   // Variance only guarantees small absolute error:
   BOOST_CHECK_SMALL(variance(dist)
      - static_cast<RealType>(RealType(4.85592267707818899235900237275021938334418424134218087127572e13L)
      * exp(RealType(-31.5)) * 100 * 101 / (103 * 104) -
      RealType(4.82280451915522329944315287538684030781836554279474240490936e13L) * RealType(4.82280451915522329944315287538684030781836554279474240490936e13L)
      * exp(RealType(-63)) * 10000 / (103 * 103)), abs_tolerance);
   BOOST_MATH_CHECK_THROW(skewness(dist), boost::math::evaluation_error);
   BOOST_MATH_CHECK_THROW(kurtosis(dist), boost::math::evaluation_error);
   BOOST_MATH_CHECK_THROW(kurtosis_excess(dist), boost::math::evaluation_error);
} // template <class RealType>void test_spots(RealType)

BOOST_AUTO_TEST_CASE( test_main )
{
   BOOST_MATH_CONTROL_FP;
   // Basic sanity-check spot values.
    expected_results();
   // (Parameter value, arbitrarily zero, only communicates the floating point type).
#ifdef TEST_FLOAT
   test_spots(0.0F); // Test float.
#endif
#ifdef TEST_DOUBLE
   test_spots(0.0); // Test double.
#endif
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
#ifdef TEST_LDOUBLE
   test_spots(0.0L); // Test long double.
#endif
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
#ifdef TEST_REAL_CONCEPT
   test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
#endif
#endif
#endif

#ifdef TEST_FLOAT
   test_accuracy(0.0F, "float"); // Test float.
#endif
#ifdef TEST_DOUBLE
   test_accuracy(0.0, "double"); // Test double.
#endif
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
#ifdef TEST_LDOUBLE
   test_accuracy(0.0L, "long double"); // Test long double.
#endif
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
#ifdef TEST_REAL_CONCEPT
   test_accuracy(boost::math::concepts::real_concept(0.), "real_concept"); // Test real concept.
#endif
#endif
#endif

} // BOOST_AUTO_TEST_CASE( test_main )


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/not_equal.hpp>

#include <type_traits>
namespace hana = boost::hana;

static_assert(hana::value(std::integral_constant<int, 3>{}) == 3, "");
static_assert(std::integral_constant<int, 3>::value == 3, "");

BOOST_HANA_CONSTANT_CHECK(hana::equal(std::integral_constant<int, 3>{}, hana::int_c<3>));
BOOST_HANA_CONSTANT_CHECK(hana::equal(std::integral_constant<int, 3>{}, hana::long_c<3>));
BOOST_HANA_CONSTANT_CHECK(hana::not_equal(std::integral_constant<int, 3>{}, hana::int_c<0>));

int main() { }


//yqq

#include <iostream>
#include <algorithm>
#include <string>
#include <boost/lockfree/spsc_queue.hpp>
#include <boost/thread/thread.hpp>
#include <boost/container/vector.hpp>
#include <boost/thread/future.hpp>
#include <boost/random.hpp>
#include <boost/move/move.hpp>
#include <boost/container/deque.hpp>
// #include <boost/thread/a
#include <boost/thread/mutex.hpp>

using namespace std;

// void foo()
// {
//     for (int i = 0; i < 10; i++)
//     {
//         std::cout << "sub thread" << std::endl;
//     }
// }

int main()
{
    using boost::thread;
    using boost::container::vector;
    // using boost::
    // using boost::lockfree::queue ;
    using boost::lockfree::spsc_queue;
    // using boost::move;
    // using boost::random;

    spsc_queue<double> lockfreeQueue(100000000);
    // boost::container::deque<double> lockfreeQueue;
    const int cnProducer = 10, cnConsumer = 10;

    boost::mutex  mtx;

    vector<thread> vctProducer;
    const double pi = 3.14159261314;

    for (int i = 0; i < cnProducer; i++)
    {
        vctProducer.push_back( boost::move( thread([&pi, &lockfreeQueue, &mtx]() {
            for (int i = 0; i < 10000000; i++)
            {
                // std::cout << "sub thread" << std::endl;
                lockfreeQueue.push( pow(i, 3) * pi / 2  );

                // boost::lock_guard<boost::mutex>  lock(mtx);
                // lockfreeQueue.push_back( pow(i, 3) * pi / 2  );
            }
        })));
    }

    for(auto &thd : vctProducer)
    {
        thd.join();
    }

    return 0;
}

// g++ 7_5_boost_lockfree_queue.cpp -lpthread -lboost_thread


#ifndef PYBNESIAN_UTIL_VECH_OPS_HPP
#define PYBNESIAN_UTIL_VECH_OPS_HPP

#include <Eigen/Dense>

using Eigen::VectorXd, Eigen::MatrixXd;

namespace util {

VectorXd vech(const MatrixXd& m);
MatrixXd invvech(const VectorXd& m);
MatrixXd invvech_triangular(const VectorXd& v);

}  // namespace util

#endif  // PYBNESIAN_VECH_OPS_HPP


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/integral_constant.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/core/datatype.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <cstddef>
#include <type_traits>
using namespace boost::hana;

int main() {
    //////////////////////////////////////////////////////////////////////////
    // IntegralConstant's API (like std::integral_constant)
    //////////////////////////////////////////////////////////////////////////
    {
        // operator()
        static_assert(size_t<0>() == 0, "");
        static_assert(size_t<1>() == 1, "");
        static_assert(int_<-3>() == -3, "");

        // decltype(operator())
        BOOST_HANA_CONSTANT_CHECK(decltype_(size_t<0>()) == type<std::size_t>);
        BOOST_HANA_CONSTANT_CHECK(decltype_(int_<-3>()) == type<int>);

        // conversions
        constexpr std::size_t a = size_t<0>, b = size_t<1>;
        static_assert(a == 0 && b == 1, "");

        constexpr int c = int_<0>, d = int_<-3>;
        static_assert(c == 0 && d == -3, "");

        // nested ::value
        static_assert(decltype(int_<1>)::value == 1, "");

        // nested ::type
        static_assert(std::is_same<
            decltype(int_<1>)::type,
            std::remove_cv_t<decltype(int_<1>)>
        >{}, "");

        // nested ::value_type
        static_assert(std::is_same<decltype(int_<1>)::value_type, int>{}, "");
    }

    //////////////////////////////////////////////////////////////////////////
    // Make sure we can inherit _integral_constant and retain the same
    // data type.
    //////////////////////////////////////////////////////////////////////////
    {
        struct derived : _integral_constant<int, 10> { };
        static_assert(std::is_same<datatype_t<derived>, IntegralConstant<int>>{}, "");
    }

    //////////////////////////////////////////////////////////////////////////
    // Extensions to std::integral_constant
    //////////////////////////////////////////////////////////////////////////
    {
        // times member function (the other ones are tested in the examples)
        {
            int counter = 0;
            int_<3>.times([&] { ++counter; });
            BOOST_HANA_RUNTIME_CHECK(counter == 3);
        }

        // Arithmetic operators
        {
            BOOST_HANA_CONSTANT_CHECK(+int_<1> == int_<1>);
            BOOST_HANA_CONSTANT_CHECK(-int_<1> == int_<-1>);
            BOOST_HANA_CONSTANT_CHECK(int_<1> + int_<2> == int_<3>);
            BOOST_HANA_CONSTANT_CHECK(int_<1> - int_<2> == int_<-1>);
            BOOST_HANA_CONSTANT_CHECK(int_<3> * int_<2> == int_<6>);
            BOOST_HANA_CONSTANT_CHECK(int_<6> / int_<3> == int_<2>);
            BOOST_HANA_CONSTANT_CHECK(int_<6> % int_<4> == int_<2>);
            BOOST_HANA_CONSTANT_CHECK(~int_<6> == int_<~6>);
            BOOST_HANA_CONSTANT_CHECK((int_<6> & int_<3>) == int_<6 & 3>);
            BOOST_HANA_CONSTANT_CHECK((int_<4> | int_<2>) == int_<4 | 2>);
            BOOST_HANA_CONSTANT_CHECK((int_<6> ^ int_<3>) == int_<6 ^ 3>);
            BOOST_HANA_CONSTANT_CHECK((int_<6> << int_<3>) == int_<(6 << 3)>);
            BOOST_HANA_CONSTANT_CHECK((int_<6> >> int_<3>) == int_<(6 >> 3)>);
        }

        // Comparison operators
        {
            BOOST_HANA_CONSTANT_CHECK(int_<0> == int_<0>);
            BOOST_HANA_CONSTANT_CHECK(int_<1> != int_<0>);
            BOOST_HANA_CONSTANT_CHECK(int_<0> < int_<1>);
            BOOST_HANA_CONSTANT_CHECK(int_<0> <= int_<1>);
            BOOST_HANA_CONSTANT_CHECK(int_<0> <= int_<0>);
            BOOST_HANA_CONSTANT_CHECK(int_<1> > int_<0>);
            BOOST_HANA_CONSTANT_CHECK(int_<1> >= int_<0>);
            BOOST_HANA_CONSTANT_CHECK(int_<0> >= int_<0>);
        }

        // Logical operators
        {
            BOOST_HANA_CONSTANT_CHECK(int_<3> || int_<0>);
            BOOST_HANA_CONSTANT_CHECK(int_<3> && int_<1>);
            BOOST_HANA_CONSTANT_CHECK(!int_<0>);
            BOOST_HANA_CONSTANT_CHECK(!!int_<3>);
        }

        // Creation with user-defined literals
        {
            using namespace boost::hana::literals;

            BOOST_HANA_CONSTANT_CHECK(0_c == llong<0>);
            BOOST_HANA_CONSTANT_CHECK(1_c == llong<1>);
            BOOST_HANA_CONSTANT_CHECK(12_c == llong<12>);
            BOOST_HANA_CONSTANT_CHECK(123_c == llong<123>);
            BOOST_HANA_CONSTANT_CHECK(1234567_c == llong<1234567>);
            BOOST_HANA_CONSTANT_CHECK(-34_c == llong<-34>);

            BOOST_HANA_CONSTANT_CHECK(decltype_(-1234_c) == decltype_(llong<-1234>));
            BOOST_HANA_CONSTANT_CHECK(-12_c < 0_c);
        }
    }
}


// g++ -O3 -DNDEBUG -I.. -L /usr/lib64/atlas/ benchBlasGemm.cpp -o benchBlasGemm -lrt -lcblas
// possible options:
//    -DEIGEN_DONT_VECTORIZE
//    -msse2

// #define EIGEN_DEFAULT_TO_ROW_MAJOR
#define _FLOAT

#include <iostream>

#include <Eigen/Core>
#include "BenchTimer.h"

// include the BLAS headers
extern "C" {
#include <cblas.h>
}
#include <string>

#ifdef _FLOAT
typedef float Scalar;
#define CBLAS_GEMM cblas_sgemm
#else
typedef double Scalar;
#define CBLAS_GEMM cblas_dgemm
#endif

typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> MyMatrix;
void bench_eigengemm(MyMatrix& mc, const MyMatrix& ma, const MyMatrix& mb, int nbloops);
void check_product(int M, int N, int K);
void check_product(void);

int main(int argc, char *argv[])
{
  // disable SSE exceptions
  #ifdef __GNUC__
  {
    int aux;
    asm(
    "stmxcsr   %[aux]           \n\t"
    "orl       $32832, %[aux]   \n\t"
    "ldmxcsr   %[aux]           \n\t"
    : : [aux] "m" (aux));
  }
  #endif

  int nbtries=1, nbloops=1, M, N, K;

  if (argc==2)
  {
    if (std::string(argv[1])=="check")
      check_product();
    else
      M = N = K = atoi(argv[1]);
  }
  else if ((argc==3) && (std::string(argv[1])=="auto"))
  {
    M = N = K = atoi(argv[2]);
    nbloops = 1000000000/(M*M*M);
    if (nbloops<1)
      nbloops = 1;
    nbtries = 6;
  }
  else if (argc==4)
  {
    M = N = K = atoi(argv[1]);
    nbloops = atoi(argv[2]);
    nbtries = atoi(argv[3]);
  }
  else if (argc==6)
  {
    M = atoi(argv[1]);
    N = atoi(argv[2]);
    K = atoi(argv[3]);
    nbloops = atoi(argv[4]);
    nbtries = atoi(argv[5]);
  }
  else
  {
    std::cout << "Usage: " << argv[0] << " size  \n";
    std::cout << "Usage: " << argv[0] << " auto size\n";
    std::cout << "Usage: " << argv[0] << " size nbloops nbtries\n";
    std::cout << "Usage: " << argv[0] << " M N K nbloops nbtries\n";
    std::cout << "Usage: " << argv[0] << " check\n";
    std::cout << "Options:\n";
    std::cout << "    size       unique size of the 2 matrices (integer)\n";
    std::cout << "    auto       automatically set the number of repetitions and tries\n";
    std::cout << "    nbloops    number of times the GEMM routines is executed\n";
    std::cout << "    nbtries    number of times the loop is benched (return the best try)\n";
    std::cout << "    M N K      sizes of the matrices: MxN  =  MxK * KxN (integers)\n";
    std::cout << "    check      check eigen product using cblas as a reference\n";
    exit(1);
  }

  double nbmad = double(M) * double(N) * double(K) * double(nbloops);

  if (!(std::string(argv[1])=="auto"))
    std::cout << M << " x " << N << " x " << K << "\n";

  Scalar alpha, beta;
  MyMatrix ma(M,K), mb(K,N), mc(M,N);
  ma = MyMatrix::Random(M,K);
  mb = MyMatrix::Random(K,N);
  mc = MyMatrix::Random(M,N);

  Eigen::BenchTimer timer;

  // we simply compute c += a*b, so:
  alpha = 1;
  beta = 1;

  // bench cblas
  // ROWS_A, COLS_B, COLS_A, 1.0,  A, COLS_A, B, COLS_B, 0.0, C, COLS_B);
  if (!(std::string(argv[1])=="auto"))
  {
    timer.reset();
    for (uint k=0 ; k<nbtries ; ++k)
    {
        timer.start();
        for (uint j=0 ; j<nbloops ; ++j)
              #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
              CBLAS_GEMM(CblasRowMajor, CblasNoTrans, CblasNoTrans, M, N, K, alpha, ma.data(), K, mb.data(), N, beta, mc.data(), N);
              #else
              CBLAS_GEMM(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, K, alpha, ma.data(), M, mb.data(), K, beta, mc.data(), M);
              #endif
        timer.stop();
    }
    if (!(std::string(argv[1])=="auto"))
      std::cout << "cblas: " << timer.value() << " (" << 1e-3*floor(1e-6*nbmad/timer.value()) << " GFlops/s)\n";
    else
        std::cout << M << " : " << timer.value() << " ; " << 1e-3*floor(1e-6*nbmad/timer.value()) << "\n";
  }

  // clear
  ma = MyMatrix::Random(M,K);
  mb = MyMatrix::Random(K,N);
  mc = MyMatrix::Random(M,N);

  // eigen
//   if (!(std::string(argv[1])=="auto"))
  {
      timer.reset();
      for (uint k=0 ; k<nbtries ; ++k)
      {
          timer.start();
          bench_eigengemm(mc, ma, mb, nbloops);
          timer.stop();
      }
      if (!(std::string(argv[1])=="auto"))
        std::cout << "eigen : " << timer.value() << " (" << 1e-3*floor(1e-6*nbmad/timer.value()) << " GFlops/s)\n";
      else
        std::cout << M << " : " << timer.value() << " ; " << 1e-3*floor(1e-6*nbmad/timer.value()) << "\n";
  }

  std::cout << "l1: " << Eigen::l1CacheSize() << std::endl;
  std::cout << "l2: " << Eigen::l2CacheSize() << std::endl;

  return 0;
}

using namespace Eigen;

void bench_eigengemm(MyMatrix& mc, const MyMatrix& ma, const MyMatrix& mb, int nbloops)
{
  for (uint j=0 ; j<nbloops ; ++j)
      mc.noalias() += ma * mb;
}

#define MYVERIFY(A,M) if (!(A)) { \
    std::cout << "FAIL: " << M << "\n"; \
  }
void check_product(int M, int N, int K)
{
  MyMatrix ma(M,K), mb(K,N), mc(M,N), maT(K,M), mbT(N,K), meigen(M,N), mref(M,N);
  ma = MyMatrix::Random(M,K);
  mb = MyMatrix::Random(K,N);
  maT = ma.transpose();
  mbT = mb.transpose();
  mc = MyMatrix::Random(M,N);

  MyMatrix::Scalar eps = 1e-4;

  meigen = mref = mc;
  CBLAS_GEMM(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, K, 1, ma.data(), M, mb.data(), K, 1, mref.data(), M);
  meigen += ma * mb;
  MYVERIFY(meigen.isApprox(mref, eps),". * .");

  meigen = mref = mc;
  CBLAS_GEMM(CblasColMajor, CblasTrans, CblasNoTrans, M, N, K, 1, maT.data(), K, mb.data(), K, 1, mref.data(), M);
  meigen += maT.transpose() * mb;
  MYVERIFY(meigen.isApprox(mref, eps),"T * .");

  meigen = mref = mc;
  CBLAS_GEMM(CblasColMajor, CblasTrans, CblasTrans, M, N, K, 1, maT.data(), K, mbT.data(), N, 1, mref.data(), M);
  meigen += (maT.transpose()) * (mbT.transpose());
  MYVERIFY(meigen.isApprox(mref, eps),"T * T");

  meigen = mref = mc;
  CBLAS_GEMM(CblasColMajor, CblasNoTrans, CblasTrans, M, N, K, 1, ma.data(), M, mbT.data(), N, 1, mref.data(), M);
  meigen += ma * mbT.transpose();
  MYVERIFY(meigen.isApprox(mref, eps),". * T");
}

void check_product(void)
{
  int M, N, K;
  for (uint i=0; i<1000; ++i)
  {
    M = internal::random<int>(1,64);
    N = internal::random<int>(1,768);
    K = internal::random<int>(1,768);
    M = (0 + M) * 1;
    std::cout << M << " x " << N << " x " << K << "\n";
    check_product(M, N, K);
  }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_POLYNOMIALS_FUNCTIONS_SIMD_COMMON_PLEVL_HPP_INCLUDED
#define NT2_POLYNOMIALS_FUNCTIONS_SIMD_COMMON_PLEVL_HPP_INCLUDED

#include <nt2/polynomials/functions/plevl.hpp>
#include <nt2/include/functions/simd/fma.hpp>
#include <nt2/include/functions/simd/tofloat.hpp>
#include <nt2/include/functions/simd/splat.hpp>
#include <nt2/include/functions/simd/plus.hpp>
#include <boost/fusion/adapted/array.hpp>
#include <nt2/sdk/meta/as_floating.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::plevl_, tag::cpu_
                            , (A0)(A1)(X)
                            , ((simd_<arithmetic_<A0>,X>))(fusion_sequence_<A1>)
                            )
  {

    typedef typename meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL(2)
    {
      return plevl(tofloat(a0), a1);
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is floating_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::plevl_, tag::cpu_
                            , (A0)(A1)(X)
                            , ((simd_<floating_<A0>,X>))(fusion_sequence_<A1>)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(2)
    {
      typename A1::const_iterator p = a1.begin();
      A0 ans = a0+nt2::splat<A0>(*p++);
      do
      ans = fma(ans, a0, nt2::splat<A0>(*p));
      while( ++p !=  a1.end());
      return ans;
    }
  };
} }

#endif