/*
 * Author: Mario Lüder
 * Date: Nov. 2018
 */

#ifndef UKF_HPP
#define UKF_HPP

#include <Eigen/Dense>

class UkfBase
{

};

template<typename PROCESSMODEL_T>
class Ukf
{
public:
   Ukf()
      : m_n_x(PROCESSMODEL_T::m_n_x)
      , m_n_x_aug(PROCESSMODEL_T::m_n_x_aug)
      , m_lambda(3 - PROCESSMODEL_T::m_n_x_aug)
      , m_weights(calculateWeights())
      , m_x(m_n_x)
      , m_P(m_n_x, m_n_x)
      , m_previousTimestampUs(0)
   {
      m_x.setZero();
      m_P.setIdentity();
   }

   ~Ukf() {}

   // delete this as this class as an read only interface
   Ukf(const Ukf & rhs) = delete;
   Ukf & operator=(Ukf & ukf) = delete;
   Ukf & operator=(Ukf && ukf) = delete;

   template<typename MEASUREMENT_T, typename MEASUREMENTMODEL_T>
   void processMeasurement(const MEASUREMENT_T & z_measurement, MEASUREMENTMODEL_T & measurementModel, int64_t timestamp_us)
   {
      const int64_t dt_us = setNewTime(timestamp_us);

      if (dt_us > 0)
      {
         // do update step
         // if some time passed since last measurement
         const double dt_s = dt_us / 1000000.0;	//dt - expressed in seconds

         // generate augumented sigma points from last state
         Eigen::MatrixXd XSigmaPoints;
         generateAugumentedSigmaPoints(m_x, m_P, m_std, XSigmaPoints);

         // predict sigma points
         PROCESSMODEL_T::predictSigmaPoints(XSigmaPoints, dt_s, m_XSigmaPointsPred);

         // predict object covariance and
         // predict object state
         m_x_pred.setZero(PROCESSMODEL_T::m_n_x);
         m_P_pred.setZero(PROCESSMODEL_T::m_n_x, PROCESSMODEL_T::m_n_x);

         predict<PROCESSMODEL_T>(m_XSigmaPointsPred, m_x_pred, m_P_pred);
      }

      Eigen::MatrixXd Z_pred;
      measurementModel.predictMeasurement(m_XSigmaPointsPred, Z_pred);

      Eigen::VectorXd z_pred;
      weightedMean(Z_pred, z_pred);

      Eigen::MatrixXd S_cov;
      measurementModel.predictCovar(*this, Z_pred, z_pred, S_cov);

      //
      // update
      //

      // Cross Correlation Matrix
      Eigen::MatrixXd Tc;
      calculateCrosscorrelationMatix<MEASUREMENTMODEL_T>(m_XSigmaPointsPred, Z_pred, m_x_pred, z_pred, Tc);

      // Kalman Gain
      Eigen::MatrixXd K;
      calculateKalmanGain(Tc, S_cov, K);

      // update state and covariance matrix
      Eigen::VectorXd z_residual = z_measurement.value - z_pred;
      MEASUREMENTMODEL_T::normalize(z_residual);

      updateState(K, z_residual, m_x_pred);
      updateCovariance(K, S_cov, m_P_pred);

      // compute NIS
      measurementModel.m_nis = computeNIS(z_residual, S_cov);

      std::swap(m_x_pred, m_x);
      std::swap(m_P_pred, m_P);
   }

   template<typename MODEL_T>
   void predictCovar(const Eigen::MatrixXd & SigmaPoints, const Eigen::VectorXd & vector, Eigen::MatrixXd & Covariance) const
   {
      Eigen::MatrixXd SigmaPointsDiff = SigmaPoints.colwise() - vector;

      for (long colIdx = 0; colIdx < PROCESSMODEL_T::m_nSigmaPoints; ++colIdx)
      {
         MODEL_T::normalize(colIdx, SigmaPointsDiff);

         Eigen::MatrixXd cov_pred_sigmaPoint = (SigmaPointsDiff.col(colIdx).array() * m_weights(colIdx)).matrix() *
               SigmaPointsDiff.col(colIdx).transpose();

         Covariance += cov_pred_sigmaPoint;
      }
   }

   // set standard deviation - process noise
   void setStd(const Eigen::VectorXd & std) { m_std = std; }

   // constant 2 * PI
   static constexpr double TWO_PI = 2 * M_PI;

private:
   //set state dimension
   const long m_n_x;

   //set augumented state dimension
   const long m_n_x_aug;

public:
   // Read only interface
   const Eigen::VectorXd & x = m_x;
   const Eigen::MatrixXd & P = m_P;

protected:
   void generateSigmaPoints(const Eigen::VectorXd &x_, const Eigen::MatrixXd & P_, Eigen::MatrixXd & XSigmaPoints)
   {
      //create sigma point matrix
      XSigmaPoints.setZero(m_n_x, 2 * m_n_x + 1);

      //calculate square root of P
      Eigen::MatrixXd A = P_.llt().matrixL();

      //set first column of sigma point matrix
      XSigmaPoints.col(0)  = x_;

      //set remaining sigma points
      for (int i = 0; i < m_n_x; i++)
      {
         XSigmaPoints.col(i+1)       = x_ + sqrt(m_lambda+m_n_x) * A.col(i);
         XSigmaPoints.col(i+1+m_n_x) = x_ - sqrt(m_lambda+m_n_x) * A.col(i);
      }
   }

   void generateAugumentedSigmaPoints(const Eigen::VectorXd &x_, const Eigen::MatrixXd & P_, const Eigen::VectorXd &std, Eigen::MatrixXd & XSigmaPoints)
   {
      //create augmented mean vector
      Eigen::VectorXd x_aug;
      x_aug.setZero(m_n_x_aug);
      x_aug.head(m_n_x) = x_;

      //create augmented state covariance
      Eigen::MatrixXd P_aug;
      P_aug.setZero(m_n_x_aug, m_n_x_aug);
      P_aug.topLeftCorner(m_n_x, m_n_x) = P_;

      for (long i = m_n_x; i < m_n_x_aug; ++i)
      {
         P_aug(i, i) = pow(std(i - m_n_x),2);
      }

      //create sigma point matrix
      XSigmaPoints.setZero(m_n_x_aug, 2 * m_n_x_aug + 1);

      //calculate square root of P
      Eigen::MatrixXd A = P_aug.llt().matrixL();

      //set first column of sigma point matrix
      XSigmaPoints.col(0)  = x_aug;

      //set remaining sigma points
      for (int i = 0; i < m_n_x_aug; i++)
      {
         XSigmaPoints.col(i+1)           = x_aug + sqrt(m_lambda+m_n_x_aug) * A.col(i);
         XSigmaPoints.col(i+1+m_n_x_aug) = x_aug - sqrt(m_lambda+m_n_x_aug) * A.col(i);
      }
   }

   inline void weightedMean(const Eigen::MatrixXd & XSigmaPoints, Eigen::VectorXd &x_pred)
   {
      x_pred = (XSigmaPoints.array().rowwise() * m_weights.transpose()).rowwise().sum();
   }

   template<typename MEASUREMENTMODEL_T>
   inline void calculateCrosscorrelationMatix(const Eigen::MatrixXd & XSigmaPointsPred, const Eigen::MatrixXd & ZSigmaPointsPred, const Eigen::VectorXd & x_mean, const Eigen::VectorXd & z_mean, Eigen::MatrixXd & T)
   {
      Eigen::MatrixXd X = (XSigmaPointsPred.colwise() - x_mean);
      // MEASUREMENTMODEL_T::normalize(X);
      Eigen::MatrixXd Z = (ZSigmaPointsPred.colwise() - z_mean);
      // MEASUREMENTMODEL_T::normalize(Z);
      const Eigen::MatrixXd X_weighted = X.array().rowwise() * m_weights.transpose();
      T = (X_weighted * Z.transpose());
   }

   inline void calculateKalmanGain(const Eigen::MatrixXd & T, const Eigen::MatrixXd & S, Eigen::MatrixXd & K)
   {
      K = T * S.inverse();
   }

   inline void updateState(const Eigen::MatrixXd & K, const Eigen::VectorXd & z_residual, Eigen::VectorXd & x_pred)
   {
      Eigen::VectorXd & x_update = x_pred;
      x_update = x_pred + K * z_residual;
   }

   inline void updateCovariance(const Eigen::MatrixXd & K, const Eigen::MatrixXd & S, Eigen::MatrixXd & P_pred) const
   {
      Eigen::MatrixXd & P_update = P_pred;
      P_update = P_pred - K * S * K.transpose();
   }

   template<typename MODEL_T>
   inline void predict(const Eigen::MatrixXd & XSigmaPoints, Eigen::VectorXd &x_pred, Eigen::MatrixXd & P_pred)
   {
      weightedMean(XSigmaPoints, x_pred);
      predictCovar<MODEL_T>(XSigmaPoints, x_pred, P_pred);
   }

   // set the new measurement time and compute the difference
   inline int64_t setNewTime(const int64_t timestamp_us)
   {
      //compute the time elapsed between the current and previous measurements
      int64_t dt = timestamp_us - m_previousTimestampUs;
      m_previousTimestampUs = timestamp_us;
      return dt;
   }

   inline double computeNIS(const Eigen::VectorXd z_diff, const Eigen::MatrixXd & S)
   {
      const double nis = z_diff.transpose() * S.inverse() * z_diff;
      return nis;
   }

   //define spreading parameter
   const double m_lambda;

   // weights for prediction
   const Eigen::ArrayXd m_weights;

   // set standard deviation - process noise
   Eigen::VectorXd m_std;

   // object state
   Eigen::VectorXd m_x;

   // object covariance matrix
   Eigen::MatrixXd m_P;

   // predicted object state
   Eigen::VectorXd m_x_pred;

   // predicted object covariance matrix
   Eigen::MatrixXd m_P_pred;

   // predicted Sigma Points
   Eigen::MatrixXd m_XSigmaPointsPred;

private:
   const Eigen::ArrayXd calculateWeights()
   {
      Eigen::ArrayXd weights(PROCESSMODEL_T::m_nSigmaPoints);

      weights(0) = m_lambda / (m_lambda + m_n_x_aug);

      for (int i = 1; i < PROCESSMODEL_T::m_nSigmaPoints; ++i)
      {
         weights(i) = 0.5 / (m_lambda + m_n_x_aug);
      }

      return  weights;
   }

   int64_t m_previousTimestampUs;
};

#endif // UKF_HPP


#include <algorithm>
#include <fstream>
#include <vector>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>
#include <Eigen/Core>
#include <Eigen/Householder>
#include <Eigen/SparseCore>
#include <Euclid/Descriptor/HKS.h>
#include <Euclid/Descriptor/Histogram.h>
#include <Euclid/IO/OffIO.h>
#include <Euclid/IO/PlyIO.h>
#include <Euclid/MeshUtil/CGALMesh.h>
#include <Euclid/Geometry/Spectral.h>
#include <Euclid/Util/Color.h>

#include <config.h>

using Kernel = CGAL::Simple_cartesian<double>;
using Point_3 = Kernel::Point_3;
using Mesh = CGAL::Surface_mesh<Point_3>;
using Mat = Eigen::MatrixXd;
using Vec = Eigen::VectorXd;
using SpMat = Eigen::SparseMatrix<double>;
using Arr = Eigen::ArrayXXd;

void eigs_to_color(const std::string& prefix,
                   const std::vector<double>& positions,
                   const std::vector<unsigned>& indices,
                   const Mat& eigenfunctions)
{
    auto nv = positions.size() / 3;
    for (int i = 0; i < 6; ++i) {
        std::vector<double> eigenvector(nv);
        Vec::Map(eigenvector.data(), nv) = eigenfunctions.col(i);
        std::vector<uint8_t> colors;
        Euclid::colormap(igl::COLOR_MAP_TYPE_JET, eigenvector, colors, true);
        std::string fout(TMP_DIR);
        fout.append(prefix)
            .append("_eigenfunction_")
            .append(std::to_string(i))
            .append(".ply");
        Euclid::write_ply<3>(
            fout, positions, nullptr, nullptr, &indices, &colors);
    }
}

void eigs_recon(const std::string& prefix,
                const std::vector<double>& positions,
                const std::vector<unsigned>& indices,
                const Mat& eigenfunctions)
{
    auto nv = positions.size() / 3;
    Eigen::Map<const Mat> xyz(positions.data(), 3, nv);
    EASSERT(xyz(1, 0) == positions[1]);
    EASSERT(xyz(0, 1) == positions[3]);
    Mat embedding = xyz * eigenfunctions;
    std::vector<int> neigs{ 5, 20, 50, 100, 200, 300 };
    for (auto ne : neigs) {
        Mat xyz_recon =
            embedding.leftCols(ne) * eigenfunctions.transpose().topRows(ne);
        std::vector<double> recon(nv * 3);
        Mat::Map(recon.data(), 3, nv) = xyz_recon;
        std::string fout(TMP_DIR);
        fout.append(prefix)
            .append("_recon_")
            .append(std::to_string(ne))
            .append(".ply");
        Euclid::write_ply<3>(fout, recon, nullptr, nullptr, &indices, nullptr);
    }
}

void hks_to_color(const std::vector<double>& positions,
                  const std::vector<unsigned>& indices,
                  const Arr& signatures,
                  int vidx)
{
    std::vector<double> distances(signatures.cols());
    for (int i = 0; i < signatures.cols(); ++i) {
        distances[i] = Euclid::chi2(signatures.col(vidx), signatures.col(i));
    }
    std::vector<uint8_t> colors;
    Euclid::colormap(
        igl::COLOR_MAP_TYPE_JET, distances, colors, true, false, true);
    std::string fout(TMP_DIR);
    fout.append("hks_").append(std::to_string(vidx)).append(".ply");
    Euclid::write_ply<3>(fout, positions, nullptr, nullptr, &indices, &colors);
}

void hks_to_csv(const Arr& signatures)
{
    std::string fout(TMP_DIR);
    fout.append("hks.csv");
    std::ofstream ofs(fout);
    for (int i = 0; i < signatures.rows(); ++i) {
        for (int j = 0; j < signatures.cols(); ++j) {
            ofs << signatures(i, j) << ",";
        }
        ofs << std::endl;
    }
}

int main()
{
    // read and build mesh
    std::vector<double> positions;
    std::vector<unsigned> indices;
    std::string fmesh(DATA_DIR);
    fmesh.append("bumpy.off");
    Euclid::read_off<3>(fmesh, positions, nullptr, &indices, nullptr);
    Mesh mesh;
    Euclid::make_mesh<3>(mesh, positions, indices);

    // solving eigenvalue problem
    int k = 300;
    Vec lambdas_lbo, lambdas_gl;
    Mat phis_lbo, phis_gl;
    Euclid::spectrum(
        mesh, k, lambdas_lbo, phis_lbo, Euclid::SpecOp::mesh_laplacian);
    Euclid::spectrum(
        mesh, k, lambdas_gl, phis_gl, Euclid::SpecOp::graph_laplacian);

    // eigenvectors of mesh laplacian need to be orthonormalized for recon
    phis_lbo = phis_lbo.householderQr().householderQ();

    // output eigenfunctions to colors
    eigs_to_color("lbo", positions, indices, phis_lbo);
    eigs_to_color("gl", positions, indices, phis_gl);

    // eigen embedding and reconstruction
    eigs_recon("lbo", positions, indices, phis_lbo);
    eigs_recon("gl", positions, indices, phis_gl);

    // use eigenstructures to compute heat kernel signature
    Euclid::HKS<Mesh> hks;
    hks.build(mesh, &lambdas_lbo, &phis_lbo);
    Eigen::ArrayXXd hks_sigs;
    hks.compute(hks_sigs, 100);
    hks_to_color(positions, indices, hks_sigs, 240); // tip
    hks_to_color(positions, indices, hks_sigs, 3);   // cube corner
    hks_to_csv(hks_sigs);
}


#include <inttypes.h>
#include <iostream>
#include <iomanip>
#include <cstdlib>
#include <cerrno>
#include <NTL/mat_GF2.h>
#include <NTL/GF2.h>
#include <NTL/matrix.h>
#include <vector>
#include "calc_tvalue.hpp"

using namespace std;
using namespace NTL;

//#define DEBUG

namespace {
    Mat<GF2> Ident;
    bool make_mat(Mat<GF2>& mat, uint64_t num);
    size_t calc_order(Mat<GF2>& mat, size_t max);
}

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

    if (argc < 4) {
        cout << argv[0] << " s m max_tvalue" << endl;
        return -1;
    }
    errno = 0;
    int s = strtoul(argv[1], NULL, 10);
    int m = strtoul(argv[2], NULL, 10);
    int max_tvalue = strtoul(argv[3], NULL, 10);
    if (errno) {
        cout << " s, m and max_tvalue should be numbers" << endl;
        return -1;
    }
    cout << "# s = " << dec << s << endl;
    cout << "# m = " << dec << m << endl;
    cout << "# max_tvalue = " << dec << max_tvalue << endl;
    ident(Ident, m);
#if defined(DEBUG)
    cout << "main step 0.1" << endl;
#endif
    int64_t max = 1 << (m * m);
    Mat<GF2> mat;
    mat.SetDims(m, m);
    Mat<GF2> array[s];
#if defined(DEBUG)
    cout << "main step 0.2" << endl;
#endif
    vector<vector<int> >to;
    to.resize(max_tvalue + 1);
#if defined(DEBUG)
    cout << "main step 1" << endl;
#endif
    for (int i = 0; i <= max_tvalue; i++) {
        to[i].resize(max);
        for (int64_t j = 0; j < max; j++) {
            to[i][j] = 0;
        }
    }
#if defined(DEBUG)
    cout << "main step 2" << endl;
#endif
    for (int64_t cnt = 1; cnt < max; cnt++) {
        if (!make_mat(mat, cnt)) {
            continue;
        }
        int order = calc_order(mat, max - 1);
        if (order < s) {
            continue;
        }
        array[0] = mat;
        for (int j = 1; j < s; j++) {
            array[j] = array[j - 1] * mat;
        }
        int tvalue = calc_tvalue(array, s, max_tvalue);
        if (tvalue > max_tvalue || tvalue < 0) {
            continue;
        }
        to[tvalue][order] += 1;
        if (tvalue == 0 && order == max -1) {
            cout << "found!" << endl;
            cout << "cnt = " << dec << cnt << endl;
            cout << "tvalue = " << dec << tvalue << endl;
            cout << "order = " << dec << order << endl;
            cout << mat;
        }
    }
    cout << "(tvalue, order) = cnt" << endl;
    for (int i = 0; i <= max_tvalue; i++) {
        for (int64_t j = 0; j < max; j++) {
            if (to[i][j] == 0) {
                continue;
            }
            cout << "(" << dec << i << ","
                 << j << ") = " << to[i][j] << endl;
        }
    }
    return 0;
}

namespace {
    bool make_mat(Mat<GF2>& mat, uint64_t num)
    {
#if defined(DEBUG)
        cout << "start make_mat" << endl;
#endif
        //cout << "num = " << dec << num << endl;
        int m = mat.NumRows();
        const uint64_t mask = ~UINT64_C(0) >> (64 - m);
        for (int i = 0; i < m; i++) {
            uint64_t row = num & mask;
            if (row == 0) {
#if defined(DEBUG)
                cout << "end make_mat" << endl;
#endif
                return false;
            }
            for (int j = 0; j < m; j++) {
                //cout << "mat.put(" << i << "," << j << "," << (row & 1) << ")"
                //     << endl;
                mat.put(i, j, row & 1);
                row = row >> 1;
            }
            num = num >> m;
        }
        //cout << "make_mat 1:" << mat << endl;
        Mat<GF2> w = mat;
        if (gauss(w) != m) {
#if defined(DEBUG)
            cout << "end make_mat" << endl;
#endif
            return false;
        }
        //cout << "make_mat 2:\n" << mat << endl;
#if defined(DEBUG)
        cout << "end make_mat" << endl;
#endif
        return true;
    }

    size_t calc_order(Mat<GF2>& mat, size_t max)
    {
        Mat<GF2> w = mat;
        for (size_t i = 1; i <= max; i++) {
            w = w * mat;
            if (w == mat) {
                return i;
            }
        }
        return max;
    }
}


#include "drakeGeometryUtil.h"
#include <iostream>
#include <cmath>
#include <limits>
#include <stdexcept>
#include <Eigen/Sparse>
#include "expmap2quat.h"

using namespace Eigen;

double angleDiff(double phi1, double phi2)
{
  double d = phi2-phi1;
  if(d>0.0)
  {
    d = fmod(d+M_PI,2*M_PI)-M_PI;
  }
  else
  {
    d = fmod(d-M_PI,2*M_PI)+M_PI;
  }
  return d;
}

Vector4d quatConjugate(const Eigen::Vector4d& q)
{
  Vector4d q_conj;
  q_conj << q(0), -q(1), -q(2), -q(3);
  return q_conj;
}

Eigen::Matrix4d dquatConjugate()
{
  Matrix4d dq_conj = Matrix4d::Identity();
  dq_conj(1, 1) = -1.0;
  dq_conj(2, 2) = -1.0;
  dq_conj(3, 3) = -1.0;
  return dq_conj;
}

Eigen::Vector4d quatProduct(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2)
{
  double w1 = q1(0);
  double w2 = q2(0);
  const auto& v1 = q1.tail<3>();
  const auto& v2 = q2.tail<3>();
  Vector4d r;
  r << w1 * w2 - v1.dot(v2), v1.cross(v2) + w1 * v2 + w2 * v1;
  return r;
}

Eigen::Matrix<double, 4, 8> dquatProduct(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2)
{
  double w1 = q1(0);
  double w2 = q2(0);
  const auto& v1 = q1.tail<3>();
  const auto& v2 = q2.tail<3>();

  Matrix<double, 4, 8> dr;
  dr.row(0) << w2, -v2.transpose(), w1, -v1.transpose();
  dr.row(1) << q2(1), q2(0), q2(3), -q2(2), q1(1), q1(0), -q1(3), q1(2);
  dr.row(2) << q2(2), -q2(3), q2(0), q2(1), q1(2), q1(3), q1(0), -q1(1);
  dr.row(3) << q2(3), q2(2), -q2(1), q2(0), q1(3), -q1(2), q1(1), q1(0);
  return dr;
}

Eigen::Vector3d quatRotateVec(const Eigen::Vector4d& q, const Eigen::Vector3d& v)
{
  Vector4d v_quat;
  v_quat << 0, v;
  Vector4d q_times_v = quatProduct(q, v_quat);
  Vector4d q_conj = quatConjugate(q);
  Vector4d v_rot = quatProduct(q_times_v, q_conj);
  Vector3d r = v_rot.bottomRows<3>();
  return r;
}

Eigen::Matrix<double, 3, 7> dquatRotateVec(const Eigen::Vector4d& q, const Eigen::Vector3d& v)
{
  Matrix<double, 4, 7> dq;
  dq << Matrix4d::Identity(), MatrixXd::Zero(4, 3);
  Matrix<double, 4, 7> dv = Matrix<double, 4, 7>::Zero();
  dv.bottomRightCorner<3, 3>() = Matrix3d::Identity();
  Matrix<double, 8, 7> dqdv;
  dqdv << dq, dv;

  Vector4d v_quat;
  v_quat << 0, v;
  Vector4d q_times_v = quatProduct(q, v_quat);
  Matrix<double, 4, 8> dq_times_v_tmp = dquatProduct(q, v_quat);
  Matrix<double, 4, 7> dq_times_v = dq_times_v_tmp * dqdv;

  Matrix<double, 4, 7> dq_conj = dquatConjugate() * dq;
  Matrix<double, 8, 7> dq_times_v_dq_conj;
  dq_times_v_dq_conj << dq_times_v, dq_conj;
  Matrix<double, 4, 8> dv_rot_tmp = dquatProduct(q_times_v, quatConjugate(q));
  Matrix<double, 4, 7> dv_rot = dv_rot_tmp * dq_times_v_dq_conj;
  Eigen::Matrix<double, 3, 7> dr = dv_rot.bottomRows(3);
  return dr;
}

Eigen::Vector4d quatDiff(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2)
{
  return quatProduct(quatConjugate(q1), q2);
}

Eigen::Matrix<double, 4, 8> dquatDiff(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2)
{
  auto dr = dquatProduct(quatConjugate(q1), q2);
  dr.block<4, 3>(0, 1) = -dr.block<4, 3>(0, 1);
  return dr;
}

double quatDiffAxisInvar(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2, const Eigen::Vector3d& u)
{
  Vector4d r = quatDiff(q1, q2);
  double e = -2.0 + 2 * r(0) * r(0) + 2 * pow(u(0) * r(1) + u(1) * r(2) + u(2) * r(3), 2);
  return e;
}

Eigen::Matrix<double, 1, 11> dquatDiffAxisInvar(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2, const Eigen::Vector3d& u)
{
  Vector4d r = quatDiff(q1, q2);
  Matrix<double, 4, 8> dr = dquatDiff(q1, q2);
  Matrix<double, 1, 11> de;
  const auto& rvec = r.tail<3>();
  de << 4.0 * r(0) * dr.row(0) + 4.0 * u.transpose() * rvec *u.transpose() * dr.block<3, 8>(1, 0), 4.0 * u.transpose() * rvec * rvec.transpose();
  return de;
}

double quatNorm(const Eigen::Vector4d& q)
{
  return std::acos(q(0));
}

Vector4d uniformlyRandomAxisAngle(std::default_random_engine& generator)
{
  std::normal_distribution<double> normal;
  std::uniform_real_distribution<double> uniform(-M_PI, M_PI);
  double angle = uniform(generator);
  Vector3d axis = Vector3d(normal(generator), normal(generator), normal(generator));
  axis.normalize();
  Vector4d a;
  a << axis, angle;
  return a;
}

Vector4d uniformlyRandomQuat(std::default_random_engine& generator)
{
  return axis2quat(uniformlyRandomAxisAngle(generator));
}

Eigen::Matrix3d uniformlyRandomRotmat(std::default_random_engine& generator)
{
  return axis2rotmat(uniformlyRandomAxisAngle(generator));
}

Eigen::Vector3d uniformlyRandomRPY(std::default_random_engine& generator)
{
  return axis2rpy(uniformlyRandomAxisAngle(generator));
}

template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, 3, 1> quat2rpy(const Eigen::MatrixBase<Derived>& q)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
  auto q_normalized = q.normalized();
  auto w = q_normalized(0);
  auto x = q_normalized(1);
  auto y = q_normalized(2);
  auto z = q_normalized(3);

  Eigen::Matrix<typename Derived::Scalar, 3, 1> ret;
  ret << std::atan2(2.0*(w*x + y*z), w*w + z*z -(x*x +y*y)),
      std::asin(2.0*(w*y - z*x)),
      std::atan2(2.0*(w*z + x*y), w*w + x*x-(y*y+z*z));
  return ret;
}

template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, 3, 3> quat2rotmat(const Eigen::MatrixBase<Derived>& q)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
  auto q_normalized = q.normalized();
  auto w = q_normalized(0);
  auto x = q_normalized(1);
  auto y = q_normalized(2);
  auto z = q_normalized(3);

  Eigen::Matrix<typename Derived::Scalar, 3, 3> M;
  M.row(0) << w * w + x * x - y * y - z * z, 2.0 * x * y - 2.0 * w * z, 2.0 * x * z + 2.0 * w * y;
  M.row(1) << 2.0 * x * y + 2.0 * w * z, w * w + y * y - x * x - z * z, 2.0 * y * z - 2.0 * w * x;
  M.row(2) << 2.0 * x * z - 2.0 * w * y, 2.0 * y * z + 2.0 * w * x, w * w + z * z - x * x - y * y;

  return M;
}

template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, 4, 1> quat2axis(const Eigen::MatrixBase<Derived>& q)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
  auto q_normalized = q.normalized();
  auto s = std::sqrt(1.0 - q_normalized(0) * q_normalized(0)) + std::numeric_limits<typename Derived::Scalar>::epsilon();
  Eigen::Matrix<typename Derived::Scalar, 4, 1> a;

  a << q_normalized.template tail<3>() / s, 2.0 * std::acos(q_normalized(0));
  return a;
}

template <typename Derived>
Eigen::Vector4d axis2quat(const Eigen::MatrixBase<Derived>& a)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
  auto axis = a.template head<3>();
  auto angle = a(3);
  auto arg = 0.5 * angle;
  auto c = std::cos(arg);
  auto s = std::sin(arg);
  Eigen::Vector4d ret;
  ret << c, s * axis;
  return ret;
}

template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, 3, 3> axis2rotmat(const Eigen::MatrixBase<Derived>& a)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
  const auto& axis = (a.template head<3>())/(a.template head<3>()).norm();
  const auto& theta = a(3);
  auto x = axis(0);
  auto y = axis(1);
  auto z = axis(2);
  auto ctheta = std::cos(theta);
  auto stheta = std::sin(theta);
  auto c = 1 - ctheta;
  Eigen::Matrix<typename Derived::Scalar, 3, 3> R;
  R <<
      ctheta + x * x * c , x * y * c - z * stheta, x * z * c + y * stheta,
      y * x * c + z * stheta, ctheta + y * y * c, y * z * c - x * stheta,
      z * x * c - y * stheta, z * y * c + x * stheta, ctheta + z * z * c;

  return R;
}

template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, 3, 1> axis2rpy(const Eigen::MatrixBase<Derived>& a)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
  return quat2rpy(axis2quat(a));
}

template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, 4, 1> rotmat2axis(const Eigen::MatrixBase<Derived>& R)
{
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3, 3);

  typename Derived::Scalar theta = std::acos((R.trace() - 1.0) / 2.0);
  Vector4d a;
  if (theta > std::numeric_limits<typename Derived::Scalar>::epsilon()) {
    a << R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1), theta;
    a.head<3>() *= 1.0 / (2.0 * std::sin(theta));
  }
  else {
    a << 1.0, 0.0, 0.0, 0.0;
  }
  return a;
}

template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, 4, 1> rotmat2quat(const Eigen::MatrixBase<Derived>& M)
{
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3, 3);
  using namespace std;

  Matrix<typename Derived::Scalar, 4, 3> A;
  A.row(0) << 1.0, 1.0, 1.0;
  A.row(1) << 1.0, -1.0, -1.0;
  A.row(2) << -1.0, 1.0, -1.0;
  A.row(3) << -1.0, -1.0, 1.0;
  Matrix<typename Derived::Scalar, 4, 1> B = A * M.diagonal();
  typename Matrix<typename Derived::Scalar, 4, 1>::Index ind, max_col;
  typename Derived::Scalar val = B.maxCoeff(&ind, &max_col);

  typename Derived::Scalar w, x, y, z;
  switch (ind) {
  case 0: {
    // val = trace(M)
    w = sqrt(1.0 + val) / 2.0;
    typename Derived::Scalar w4 = w * 4.0;
    x = (M(2, 1) - M(1, 2)) / w4;
    y = (M(0, 2) - M(2, 0)) / w4;
    z = (M(1, 0) - M(0, 1)) / w4;
    break;
  }
  case 1: {
    // val = M(1,1) - M(2,2) - M(3,3)
    double s = 2.0 * sqrt(1.0 + val);
    w = (M(2, 1) - M(1, 2)) / s;
    x = 0.25 * s;
    y = (M(0, 1) + M(1, 0)) / s;
    z = (M(0, 2) + M(2, 0)) / s;
    break;
  }
  case 2: {
    //  % val = M(2,2) - M(1,1) - M(3,3)
    double s = 2.0 * (sqrt(1.0 + val));
    w = (M(0, 2) - M(2, 0)) / s;
    x = (M(0, 1) + M(1, 0)) / s;
    y = 0.25 * s;
    z = (M(1, 2) + M(2, 1)) / s;
    break;
  }
  default: {
    // val = M(3,3) - M(2,2) - M(1,1)
    double s = 2.0 * (sqrt(1.0 + val));
    w = (M(1, 0) - M(0, 1)) / s;
    x = (M(0, 2) + M(2, 0)) / s;
    y = (M(1, 2) + M(2, 1)) / s;
    z = 0.25 * s;
    break;
  }
  }

  Eigen::Matrix<typename Derived::Scalar, 4, 1> q;
  q << w, x, y, z;
  return q;
}

template<typename Derived>
Eigen::Matrix<typename Derived::Scalar, 3, 1> rotmat2rpy(const Eigen::MatrixBase<Derived>& R)
{
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3, 3);
  using namespace std;

  Eigen::Matrix<typename Derived::Scalar, 3, 1> rpy;
  rpy << atan2(R(2, 1), R(2, 2)), atan2(-R(2, 0), sqrt(pow(R(2, 1), 2.0) + pow(R(2, 2), 2.0))), atan2(R(1, 0), R(0, 0));
  return rpy;
}

template<typename Derived>
DLLEXPORT Eigen::Matrix<typename Derived::Scalar, Eigen::Dynamic, 1> rotmat2Representation(const Eigen::MatrixBase<Derived>& R, int rotation_type)
{
  typedef typename Derived::Scalar Scalar;
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ret;
  switch (rotation_type) {
  case 0:
    return Eigen::Matrix<Scalar, Eigen::Dynamic, 1>(0, 1);
  case 1:
    return rotmat2rpy(R);
  case 2:
    return rotmat2quat(R);
  default:
    throw std::runtime_error("rotation representation type not recognized");
  }
}

template<typename Scalar>
DLLEXPORT GradientVar<Scalar, Eigen::Dynamic, 1> rotmat2Representation(const GradientVar<Scalar, SPACE_DIMENSION, SPACE_DIMENSION>& R, int rotation_type)
{
  GradientVar<Scalar, Eigen::Dynamic, 1> ret(rotationRepresentationSize(rotation_type), 1, R.getNumVariables(), R.maxOrder());
  switch (rotation_type) {
  case 0:
    // empty matrix, already done
    break;
  case 1:
    ret.value() = rotmat2rpy(R.value());
    if (R.hasGradient()) {
      ret.gradient().value() = drotmat2rpy(R.value(), R.gradient().value());
    }
    break;
  case 2:
    ret.value() = rotmat2quat(R.value());
    if (R.hasGradient()) {
      ret.gradient().value() = drotmat2quat(R.value(), R.gradient().value());
    }
    break;
  default:
    throw std::runtime_error("rotation representation type not recognized");
  }
  return ret;
}

template <typename Derived>
GradientVar<typename Derived::Scalar, QUAT_SIZE, 1> expmap2quat(const Eigen::MatrixBase<Derived>& v, const int gradient_order)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3);
  GradientVar<typename Derived::Scalar, QUAT_SIZE, 1> ret(QUAT_SIZE, 1, EXPMAP_SIZE,gradient_order);
  auto theta = v.norm();
  if (theta < pow(std::numeric_limits<typename Derived::Scalar>::epsilon(),0.25)) {
    ret.value() = expmap2quatDegenerate(v, theta);
    if(gradient_order>0)
    {
      ret.gradient().value() = dexpmap2quatDegenerate(v, theta);
      if(gradient_order>1)
      {
        ret.gradient().gradient().value() = ddexpmap2quatDegenerate(v, theta);
        if(gradient_order>2)
        {
          throw std::runtime_error("expmap2quat does not support gradient order larger than 2");
        }
      }
    }
  } else {
    ret.value() = expmap2quatNonDegenerate(v, theta);
    if(gradient_order>0)
    {
      ret.gradient().value() = dexpmap2quatNonDegenerate(v, theta);
      if(gradient_order>1)
      {
        ret.gradient().gradient().value() = ddexpmap2quatNonDegenerate(v, theta);
        if(gradient_order>2)
        {
          throw std::runtime_error("expmap2quat does not support gradient order larger than 2");
        }
      }
    }
  }
  return ret;
}

DLLEXPORT int rotationRepresentationSize(int rotation_type)
{
  switch (rotation_type) {
  case 0:
    return 0;
    break;
  case 1:
    return 3;
    break;
  case 2:
    return 4;
    break;
  default:
    throw std::runtime_error("rotation representation type not recognized");
  }
}

template<typename Derived>
Eigen::Matrix<typename Derived::Scalar, 4, 1> rpy2axis(const Eigen::MatrixBase<Derived>& rpy)
{
  return quat2axis(rpy2quat(rpy));
}

template<typename Derived>
Eigen::Matrix<typename Derived::Scalar, 4, 1> rpy2quat(const Eigen::MatrixBase<Derived>& rpy)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3);
  auto rpy_2 = (rpy / 2.0).array();
  auto s = rpy_2.sin();
  auto c = rpy_2.cos();

  Vector4d q;
  q << c(0)*c(1)*c(2) + s(0)*s(1)*s(2),
        s(0)*c(1)*c(2) - c(0)*s(1)*s(2),
        c(0)*s(1)*c(2) + s(0)*c(1)*s(2),
        c(0)*c(1)*s(2) - s(0)*s(1)*c(2);

  q /= q.norm() + std::numeric_limits<typename Derived::Scalar>::epsilon();
  return q;
}

template<typename Derived>
Eigen::Matrix<typename Derived::Scalar, 3, 3> rpy2rotmat(const Eigen::MatrixBase<Derived>& rpy)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3);
  auto rpy_array = rpy.array();
  auto s = rpy_array.sin();
  auto c = rpy_array.cos();

  Eigen::Matrix<typename Derived::Scalar, 3, 3> R;
  R.row(0) << c(2) * c(1), c(2) * s(1) * s(0) - s(2) * c(0), c(2) * s(1) * c(0) + s(2) * s(0);
  R.row(1) << s(2) * c(1), s(2) * s(1) * s(0) + c(2) * c(0), s(2) * s(1) * c(0) - c(2) * s(0);
  R.row(2) << -s(1), c(1) * s(0), c(1) * c(0);

  return R;
}

Matrix3d rotz(double theta) {
  // returns 3D rotation matrix (about the z axis)
  Matrix3d M;
  double c=cos(theta);
  double s=sin(theta);
  M << c,-s, 0,
     s, c, 0,
     0, 0, 1;
  return M;
}

void rotz(double theta, Matrix3d &M, Matrix3d &dM, Matrix3d &ddM)
{
  double c=cos(theta), s=sin(theta);
  M << c,-s,0, s,c,0, 0,0,1;
  dM << -s,-c,0, c,-s,0, 0,0,0;
  ddM << -c,s,0, -s,-c,0, 0,0,0;
}

template<typename Derived>
Eigen::Matrix<typename Derived::Scalar,9,3> drpy2rotmat(const Eigen::MatrixBase<Derived>& rpy)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3);
  auto rpy_array = rpy.array();
  auto s = rpy_array.sin();
  auto c = rpy_array.cos();

	Eigen::Matrix<typename Derived::Scalar, 9, 3> dR;
	dR.row(0) << 0, c(2)*-s(1), c(1)*-s(2);
	dR.row(1) << 0, -s(1)*s(2), c(2)*c(1);
	dR.row(2) << 0, -c(1), 0;
	dR.row(3) << c(2)*s(1)*c(0)-s(2)*-s(0), c(2)*c(1)*s(0), -s(2)*s(1)*s(0)-c(2)*c(0);
	dR.row(4) << s(2)*s(1)*c(0)+c(2)*-s(0), s(2)*c(1)*s(0), c(2)*s(1)*s(0)-s(2)*c(0);
	dR.row(5) << c(1)*c(0), -s(1)*s(0),0;
	dR.row(6) << c(2)*s(1)*-s(0)+s(2)*c(0), c(2)*c(1)*c(0), -s(2)*s(1)*c(0)+c(2)*s(0);
	dR.row(7) << s(2)*s(1)*-s(0)-c(2)*c(0), s(2)*c(1)*c(0), c(2)*s(1)*c(0)+s(2)*s(0);
	dR.row(8) << c(1)*-s(0), -s(1)*c(0), 0;

	return dR;
}

// NOTE: not reshaping second derivative to Matlab geval output format!
template <typename Derived>
void normalizeVec(
    const Eigen::MatrixBase<Derived>& x,
    typename Derived::PlainObject& x_norm,
    typename Gradient<Derived, Derived::RowsAtCompileTime, 1>::type* dx_norm,
    typename Gradient<Derived, Derived::RowsAtCompileTime, 2>::type* ddx_norm) {

  typename Derived::Scalar xdotx = x.squaredNorm();
  typename Derived::Scalar norm_x = std::sqrt(xdotx);
  x_norm = x / norm_x;

  if (dx_norm) {
    dx_norm->setIdentity(x.rows(), x.rows());
    (*dx_norm) -= x * x.transpose() / xdotx;
    (*dx_norm) /= norm_x;

    if (ddx_norm) {
      auto dx_norm_transpose = transposeGrad(*dx_norm, x.rows());
      auto ddx_norm_times_norm = -matGradMultMat(x_norm, x_norm.transpose(), (*dx_norm), dx_norm_transpose);
      auto dnorm_inv = -x.transpose() / (xdotx * norm_x);
      (*ddx_norm) = ddx_norm_times_norm / norm_x;
      auto temp = (*dx_norm) * norm_x;
      typename Derived::Index n = x.rows();
      for (int col = 0; col < n; col++) {
        auto column_as_matrix = (dnorm_inv(0, col) * temp);
        for (int row_block = 0; row_block < n; row_block++) {
          ddx_norm->block(row_block * n, col, n, 1) += column_as_matrix.col(row_block);
        }
      }
    }
  }
}

template <typename Derived>
typename Gradient<Matrix<typename Derived::Scalar, 3, 3>, QUAT_SIZE>::type dquat2rotmat(const Eigen::MatrixBase<Derived>& q)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, QUAT_SIZE);

  typename Gradient<Matrix<typename Derived::Scalar, 3, 3>, QUAT_SIZE>::type ret;
  typename Eigen::MatrixBase<Derived>::PlainObject qtilde;
  typename Gradient<Derived, QUAT_SIZE>::type dqtilde;
  normalizeVec(q, qtilde, &dqtilde);

  typedef typename Derived::Scalar Scalar;
  Scalar w=qtilde(0);
  Scalar x=qtilde(1);
  Scalar y=qtilde(2);
  Scalar z=qtilde(3);

  ret << w, x, -y, -z, z, y, x, w, -y, z, -w, x, -z, y, x, -w, w, -x, y, -z, x, w, z, y, y, z, w, x, -x, -w, z, y, w, -x, -y, z;
  ret *= 2.0;
  ret *= dqtilde;
  return ret;
}

template <typename DerivedR, typename DerivedDR>
typename Gradient<Eigen::Matrix<typename DerivedR::Scalar, RPY_SIZE, 1>, DerivedDR::ColsAtCompileTime>::type drotmat2rpy(
    const Eigen::MatrixBase<DerivedR>& R,
    const Eigen::MatrixBase<DerivedDR>& dR)
{
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<DerivedR>, SPACE_DIMENSION, SPACE_DIMENSION);
  EIGEN_STATIC_ASSERT(Eigen::MatrixBase<DerivedDR>::RowsAtCompileTime == RotmatSize, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typename DerivedDR::Index nq = dR.cols();
  typedef typename DerivedR::Scalar Scalar;
  typedef typename Gradient<Eigen::Matrix<Scalar, RPY_SIZE, 1>, DerivedDR::ColsAtCompileTime>::type ReturnType;
  ReturnType drpy(RPY_SIZE, nq);

  auto dR11_dq = getSubMatrixGradient<DerivedDR::ColsAtCompileTime>(dR, 0, 0, R.rows());
  auto dR21_dq = getSubMatrixGradient<DerivedDR::ColsAtCompileTime>(dR, 1, 0, R.rows());
  auto dR31_dq = getSubMatrixGradient<DerivedDR::ColsAtCompileTime>(dR, 2, 0, R.rows());
  auto dR32_dq = getSubMatrixGradient<DerivedDR::ColsAtCompileTime>(dR, 2, 1, R.rows());
  auto dR33_dq = getSubMatrixGradient<DerivedDR::ColsAtCompileTime>(dR, 2, 2, R.rows());

  Scalar sqterm = R(2,1) * R(2,1) + R(2,2) * R(2,2);

  using namespace std;
  // droll_dq
  drpy.row(0) = (R(2, 2) * dR32_dq - R(2, 1) * dR33_dq) / sqterm;

  // dpitch_dq
  Scalar sqrt_sqterm = sqrt(sqterm);
  drpy.row(1) = (-sqrt_sqterm * dR31_dq + R(2, 0) / sqrt_sqterm * (R(2, 1) * dR32_dq + R(2, 2) * dR33_dq)) / (R(2, 0) * R(2, 0) + R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2));

  // dyaw_dq
  sqterm = R(0, 0) * R(0, 0) + R(1, 0) * R(1, 0);
  drpy.row(2) = (R(0, 0) * dR21_dq - R(1, 0) * dR11_dq) / sqterm;
  return drpy;
}

template <typename DerivedR, typename DerivedDR>
typename Gradient<Eigen::Matrix<typename DerivedR::Scalar, QUAT_SIZE, 1>, DerivedDR::ColsAtCompileTime>::type drotmat2quat(
    const Eigen::MatrixBase<DerivedR>& R,
    const Eigen::MatrixBase<DerivedDR>& dR)
{
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<DerivedR>, SPACE_DIMENSION, SPACE_DIMENSION);
  EIGEN_STATIC_ASSERT(Eigen::MatrixBase<DerivedDR>::RowsAtCompileTime == RotmatSize, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  typedef typename DerivedR::Scalar Scalar;
  typedef typename Gradient<Eigen::Matrix<Scalar, QUAT_SIZE, 1>, DerivedDR::ColsAtCompileTime>::type ReturnType;
  typename DerivedDR::Index nq = dR.cols();

  auto dR11_dq = getSubMatrixGradient<DerivedDR::ColsAtCompileTime>(dR, 0, 0, R.rows());
  auto dR12_dq = getSubMatrixGradient<DerivedDR::ColsAtCompileTime>(dR, 0, 1, R.rows());
  auto dR13_dq = getSubMatrixGradient<DerivedDR::ColsAtCompileTime>(dR, 0, 2, R.rows());
  auto dR21_dq = getSubMatrixGradient<DerivedDR::ColsAtCompileTime>(dR, 1, 0, R.rows());
  auto dR22_dq = getSubMatrixGradient<DerivedDR::ColsAtCompileTime>(dR, 1, 1, R.rows());
  auto dR23_dq = getSubMatrixGradient<DerivedDR::ColsAtCompileTime>(dR, 1, 2, R.rows());
  auto dR31_dq = getSubMatrixGradient<DerivedDR::ColsAtCompileTime>(dR, 2, 0, R.rows());
  auto dR32_dq = getSubMatrixGradient<DerivedDR::ColsAtCompileTime>(dR, 2, 1, R.rows());
  auto dR33_dq = getSubMatrixGradient<DerivedDR::ColsAtCompileTime>(dR, 2, 2, R.rows());

  Matrix<Scalar, 4, 3> A;
  A.row(0) << 1.0, 1.0, 1.0;
  A.row(1) << 1.0, -1.0, -1.0;
  A.row(2) << -1.0, 1.0, -1.0;
  A.row(3) << -1.0, -1.0, 1.0;
  Matrix<Scalar, 4, 1> B = A * R.diagonal();
  typename Matrix<Scalar, 4, 1>::Index ind, max_col;
  Scalar val = B.maxCoeff(&ind, &max_col);

  ReturnType dq(QUAT_SIZE, nq);
  using namespace std;
  switch (ind) {
  case 0: {
    // val = trace(M)
    auto dvaldq = dR11_dq + dR22_dq + dR33_dq;
    auto dwdq = dvaldq / (4.0 * sqrt(1.0 + val));
    auto w = sqrt(1.0 + val) / 2.0;
    auto wsquare4 = 4.0 * w * w;
    dq.row(0) = dwdq;
    dq.row(1) = ((dR32_dq - dR23_dq) * w - (R(2, 1) - R(1, 2)) * dwdq) / wsquare4;
    dq.row(2) = ((dR13_dq - dR31_dq) * w - (R(0, 2) - R(2, 0)) * dwdq) / wsquare4;
    dq.row(3) = ((dR21_dq - dR12_dq) * w - (R(1, 0) - R(0, 1)) * dwdq) / wsquare4;
    break;
  }
  case 1: {
    // val = M(1,1) - M(2,2) - M(3,3)
    auto dvaldq = dR11_dq - dR22_dq - dR33_dq;
    auto s = 2.0 * sqrt(1.0 + val);
    auto ssquare = s * s;
    auto dsdq = dvaldq / sqrt(1.0 + val);
    dq.row(0) = ((dR32_dq - dR23_dq) * s - (R(2, 1) - R(1, 2)) * dsdq) / ssquare;
    dq.row(1) = .25 * dsdq;
    dq.row(2) = ((dR12_dq + dR21_dq) * s - (R(0, 1) + R(1, 0)) * dsdq) / ssquare;
    dq.row(3) = ((dR13_dq + dR31_dq) * s - (R(0, 2) + R(2, 0)) * dsdq) / ssquare;
    break;
  }
  case 2: {
    // val = M(2,2) - M(1,1) - M(3,3)
    auto dvaldq = -dR11_dq + dR22_dq - dR33_dq;
    auto s = 2.0 * (sqrt(1.0 + val));
    auto ssquare = s * s;
    auto dsdq = dvaldq / sqrt(1.0 + val);
    dq.row(0) = ((dR13_dq - dR31_dq) * s - (R(0, 2) - R(2, 0)) * dsdq) / ssquare;
    dq.row(1) = ((dR12_dq + dR21_dq) * s - (R(0, 1) + R(1, 0)) * dsdq) / ssquare;
    dq.row(2) = .25 * dsdq;
    dq.row(3) = ((dR23_dq + dR32_dq) * s - (R(1, 2) + R(2, 1)) * dsdq) / ssquare;
    break;
  }
  default: {
    // val = M(3,3) - M(2,2) - M(1,1)
    auto dvaldq = -dR11_dq - dR22_dq + dR33_dq;
    auto s = 2.0 * (sqrt(1.0 + val));
    auto ssquare = s * s;
    auto dsdq = dvaldq / sqrt(1.0 + val);
    dq.row(0) = ((dR21_dq - dR12_dq) * s - (R(1, 0) - R(0, 1)) * dsdq) / ssquare;
    dq.row(1) = ((dR13_dq + dR31_dq) * s - (R(0, 2) + R(2, 0)) * dsdq) / ssquare;
    dq.row(2) = ((dR23_dq + dR32_dq) * s - (R(1, 2) + R(2, 1)) * dsdq) / ssquare;
    dq.row(3) = .25 * dsdq;
    break;
  }
  }
  return dq;
}

template<typename Derived>
Eigen::Matrix<typename Derived::Scalar, 3, 3> vectorToSkewSymmetric(const Eigen::MatrixBase<Derived>& p)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, SPACE_DIMENSION);
  Eigen::Matrix<typename Derived::Scalar, 3, 3> ret;
  ret << 0.0, -p(2), p(1), p(2), 0.0, -p(0), -p(1), p(0), 0.0;
  return ret;
}

template <typename DerivedA, typename DerivedB>
Eigen::Matrix<typename DerivedA::Scalar, 3, Eigen::Dynamic> dcrossProduct(
    const Eigen::MatrixBase<DerivedA>& a,
    const Eigen::MatrixBase<DerivedB>& b,
    const typename Gradient<DerivedA, Eigen::Dynamic>::type& da,
    const typename Gradient<DerivedB, Eigen::Dynamic>::type& db)
{
  Eigen::Matrix<typename DerivedA::Scalar, 3, Eigen::Dynamic> ret(3, da.cols());
  ret.noalias() = da.colwise().cross(b);
  ret.noalias() -= db.colwise().cross(a);
  return ret;
}

template <typename DerivedQ, typename DerivedM, typename DerivedDM>
void angularvel2quatdotMatrix(const Eigen::MatrixBase<DerivedQ>& q,
    Eigen::MatrixBase<DerivedM>& M,
    Eigen::MatrixBase<DerivedDM>* dM)
{
  // note: not normalizing to match MATLAB implementation
  M.resize(QUAT_SIZE, SPACE_DIMENSION);
  M.row(0) << -q(1), -q(2), -q(3);
  M.row(1) << q(0), q(3), -q(2);
  M.row(2) << -q(3), q(0), q(1);
  M.row(3) << q(2), -q(1), q(0);
  M *= 0.5;

  if (dM) {
    (*dM) << 0.0, -0.5, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.5, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, -0.5, 0.0, 0.0, 0.0, 0.0, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0, -0.5, 0.0, 0.0, 0.0, 0.0, 0.0, -0.5, 0.0, 0.0, -0.5, 0.0, 0.0, 0.5, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0;
  }
}

template<typename DerivedQ, typename DerivedM>
void quatdot2angularvelMatrix(const Eigen::MatrixBase<DerivedQ>& q, Eigen::MatrixBase<DerivedM>& M, typename Gradient<DerivedM, QUAT_SIZE, 1>::type* dM)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<DerivedQ>, QUAT_SIZE);
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<DerivedM>, SPACE_DIMENSION, QUAT_SIZE);

  typename DerivedQ::PlainObject qtilde;
  if (dM) {
    typename Gradient<DerivedQ, QUAT_SIZE>::type dqtilde;
    normalizeVec(q, qtilde, &dqtilde);
    (*dM) << 0.0, -2.0, 0.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, 0.0, -2.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, 0.0, -2.0, 2.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, -2.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0;
    (*dM) *= dqtilde;
  } else {
    normalizeVec(q, qtilde);
  }
  M << -qtilde(1), qtilde(0), -qtilde(3), qtilde(2), -qtilde(2), qtilde(3), qtilde(0), -qtilde(1), -qtilde(3), -qtilde(2), qtilde(1), qtilde(0);
  M *= 2.0;
}

template<typename DerivedRPY, typename DerivedPhi, typename DerivedDPhi, typename DerivedDDPhi>
void angularvel2rpydotMatrix(const Eigen::MatrixBase<DerivedRPY>& rpy,
    typename Eigen::MatrixBase<DerivedPhi>& phi,
    typename Eigen::MatrixBase<DerivedDPhi>* dphi,
    typename Eigen::MatrixBase<DerivedDDPhi>* ddphi)
{
  phi.resize(RPY_SIZE, SPACE_DIMENSION);

  typedef typename DerivedRPY::Scalar Scalar;
  Scalar p = rpy(1);
  Scalar y = rpy(2);

  using namespace std;
  Scalar sy = sin(y);
  Scalar cy = cos(y);
  Scalar sp = sin(p);
  Scalar cp = cos(p);
  Scalar tp = sp / cp;

  phi << cy / cp, sy / cp, 0.0, -sy, cy, 0.0, cy * tp, tp * sy, 1.0;
  if (dphi) {
    dphi->resize(phi.size(), RPY_SIZE);
    Scalar sp2 = sp * sp;
    Scalar cp2 = cp * cp;
    (*dphi) << 0.0, (cy * sp) / cp2, -sy / cp, 0.0, 0.0, -cy, 0.0, cy + (cy * sp2) / cp2, -(sp * sy) / cp, 0.0, (sp * sy) / cp2, cy / cp, 0.0, 0.0, -sy, 0.0, sy + (sp2 * sy) / cp2, (cy * sp) / cp, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;

    if (ddphi) {
      ddphi->resize(dphi->size(), RPY_SIZE);
      Scalar cp3 = cp2 * cp;
      (*ddphi) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -(cy * (cp2 - 2.0)) / cp3, (sp * sy) / (sp2 - 1.0), 0.0, 0.0, 0.0, 0.0, (2.0 * cy * sp) / cp3, sy / (sp2 - 1.0), 0.0, (2.0 * sy - cp2 * sy)
          / cp3, (cy * sp) / cp2, 0.0, 0.0, 0.0, 0.0, (2.0 * sp * sy) / cp3, cy / cp2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (sp * sy) / (sp2 - 1.0), -cy / cp, 0.0, 0.0, sy, 0.0, sy / (sp2 - 1.0), -(cy * sp) / cp, 0.0, (cy * sp) / cp2, -sy / cp, 0.0, 0.0, -cy, 0.0, cy / cp2, -(sp * sy)
          / cp, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
    }
  }
}

template<typename Derived>
DLLEXPORT GradientVar<typename Derived::Scalar, Eigen::Dynamic, SPACE_DIMENSION> angularvel2RepresentationDotMatrix(
    int rotation_type, const Eigen::MatrixBase<Derived>& qrot, int gradient_order)
{
  // note: gradients w.r.t. qrot
  GradientVar<typename Derived::Scalar, Eigen::Dynamic, SPACE_DIMENSION> ret(qrot.rows(), SPACE_DIMENSION, qrot.rows(), gradient_order);
  switch (rotation_type) {
  case 0:
    // done
    break;
  case 1: {
    if (gradient_order > 1) {
      angularvel2rpydotMatrix(qrot, ret.value(), &ret.gradient().value(), &ret.gradient().gradient().value());
    }
    else if (gradient_order > 0) {
      angularvel2rpydotMatrix(qrot, ret.value(), &ret.gradient().value(), (MatrixXd*) nullptr);
    }
    else {
      angularvel2rpydotMatrix(qrot, ret.value(), (MatrixXd*) nullptr, (MatrixXd*) nullptr);
    }
    break;
  }
  case 2: {
    if (gradient_order > 1) {
      ret.gradient().gradient().value().setZero();
    }
    if (gradient_order > 0) {
      angularvel2quatdotMatrix(qrot, ret.value(), &ret.gradient().value());
    }
    else {
      angularvel2quatdotMatrix(qrot, ret.value(), (MatrixXd*) nullptr);
    }
    break;
  }
  default:
    throw std::runtime_error("rotation representation type not recognized");
  }
  return ret;
}

template<typename DerivedRPY, typename DerivedE>
void rpydot2angularvelMatrix(const Eigen::MatrixBase<DerivedRPY>& rpy,
		Eigen::MatrixBase<DerivedE>& E,
		typename Gradient<DerivedE,RPY_SIZE,1>::type* dE)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<DerivedRPY>, RPY_SIZE);
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<DerivedE>, SPACE_DIMENSION,RPY_SIZE);
  typedef typename DerivedRPY::Scalar Scalar;
  Scalar p = rpy(1);
  Scalar y = rpy(2);
  Scalar sp = sin(p);
  Scalar cp = cos(p);
  Scalar sy = sin(y);
  Scalar cy = cos(y);

  using namespace std;
  E << cp*cy, -sy, 0.0, cp*sy, cy, 0.0, -sp, 0.0, 1.0;
  if(dE)
  {
    (*dE)<< 0.0, -sp*cy, -cp*sy, 0.0, -sp*sy, cp*cy, 0.0, -cp, 0.0, 0.0, 0.0, -cy, 0.0, 0.0, -sy, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
  }
}

template<typename DerivedM>
typename TransformSpatial<DerivedM>::type transformSpatialMotion(
    const Eigen::Transform<typename DerivedM::Scalar, 3, Eigen::Isometry>& T,
    const Eigen::MatrixBase<DerivedM>& M) {
  Eigen::Matrix<typename DerivedM::Scalar, TWIST_SIZE, DerivedM::ColsAtCompileTime> ret(TWIST_SIZE, M.cols());
  ret.template topRows<3>().noalias() = T.linear() * M.template topRows<3>();
  ret.template bottomRows<3>().noalias() = -ret.template topRows<3>().colwise().cross(T.translation());
  ret.template bottomRows<3>().noalias() += T.linear() * M.template bottomRows<3>();
  return ret;
}

template<typename DerivedF>
typename TransformSpatial<DerivedF>::type transformSpatialForce(
    const Eigen::Transform<typename DerivedF::Scalar, 3, Eigen::Isometry>& T,
    const Eigen::MatrixBase<DerivedF>& F) {
  Eigen::Matrix<typename DerivedF::Scalar, TWIST_SIZE, DerivedF::ColsAtCompileTime> ret(TWIST_SIZE, F.cols());
  ret.template bottomRows<3>().noalias() = T.linear() * F.template bottomRows<3>().eval();
  ret.template topRows<3>() = -ret.template bottomRows<3>().colwise().cross(T.translation());
  ret.template topRows<3>().noalias() += T.linear() * F.template topRows<3>();
  return ret;
}

template<typename DerivedI>
GradientVar<typename DerivedI::Scalar, TWIST_SIZE, TWIST_SIZE> transformSpatialInertia(
    const Eigen::Transform<typename DerivedI::Scalar, SPACE_DIMENSION, Eigen::Isometry>& T_current_to_new,
    const typename Gradient<typename Eigen::Transform<typename DerivedI::Scalar, SPACE_DIMENSION, Eigen::Isometry>::MatrixType, Eigen::Dynamic>::type* dT_current_to_new,
    const Eigen::MatrixBase<DerivedI>& I)
{
  int gradient_order;
  typename DerivedI::Index nq;
  if (dT_current_to_new) {
    gradient_order = 1;
    nq = dT_current_to_new->cols();
  }
  else {
    nq = 0;
    gradient_order = 0;
  }

  GradientVar<typename DerivedI::Scalar, TWIST_SIZE, TWIST_SIZE> ret(TWIST_SIZE, TWIST_SIZE, nq, gradient_order);
  auto I_half_transformed = transformSpatialForce(T_current_to_new, I);

  ret.value() = transformSpatialForce(T_current_to_new, I_half_transformed.transpose());

  if (gradient_order > 0) {
    auto dI = Eigen::Matrix<typename DerivedI::Scalar, DerivedI::SizeAtCompileTime, Eigen::Dynamic>::Zero(I.size(), nq).eval(); // TODO: would be better not to evaluate and make another explicit instantiation
    auto dI_half_transformed = dTransformSpatialForce(T_current_to_new, I, *dT_current_to_new, dI);
    auto dI_half_transformed_transpose = transposeGrad(dI_half_transformed, I_half_transformed.rows());
    ret.gradient().value() = dTransformSpatialForce(T_current_to_new, I_half_transformed.transpose(), *dT_current_to_new, dI_half_transformed_transpose);
  }
  return ret;
}

template<typename DerivedA, typename DerivedB>
typename TransformSpatial<DerivedB>::type crossSpatialMotion(
  const Eigen::MatrixBase<DerivedA>& a,
  const Eigen::MatrixBase<DerivedB>& b) {
  typename TransformSpatial<DerivedB>::type ret(TWIST_SIZE, b.cols());
  ret.template topRows<3>() = -b.template topRows<3>().colwise().cross(a.template topRows<3>());
  ret.template bottomRows<3>() = -b.template topRows<3>().colwise().cross(a.template bottomRows<3>());
  ret.template bottomRows<3>() -= b.template bottomRows<3>().colwise().cross(a.template topRows<3>());
  return ret;
}

template<typename DerivedA, typename DerivedB>
typename TransformSpatial<DerivedB>::type crossSpatialForce(
  const Eigen::MatrixBase<DerivedA>& a,
  const Eigen::MatrixBase<DerivedB>& b) {
  typename TransformSpatial<DerivedB>::type ret(TWIST_SIZE, b.cols());
  ret.template topRows<3>() = -b.template topRows<3>().colwise().cross(a.template topRows<3>());
  ret.template topRows<3>() -= b.template bottomRows<3>().colwise().cross(a.template bottomRows<3>());
  ret.template bottomRows<3>() = -b.template bottomRows<3>().colwise().cross(a.template topRows<3>());
  return ret;
}

template<typename DerivedA, typename DerivedB>
Eigen::Matrix<typename DerivedA::Scalar, TWIST_SIZE, Eigen::Dynamic> dCrossSpatialMotion(
  const Eigen::MatrixBase<DerivedA>& a,
  const Eigen::MatrixBase<DerivedB>& b,
  const typename Gradient<DerivedA, Eigen::Dynamic>::type& da,
  const typename Gradient<DerivedB, Eigen::Dynamic>::type& db) {
  Eigen::Matrix<typename DerivedA::Scalar, TWIST_SIZE, Eigen::Dynamic> ret(TWIST_SIZE, da.cols());
  ret.row(0) = -da.row(2)*b[1] + da.row(1)*b[2] - a[2]*db.row(1) + a[1]*db.row(2);
  ret.row(1) =  da.row(2)*b[0] - da.row(0)*b[2] + a[2]*db.row(0) - a[0]*db.row(2);
  ret.row(2) = -da.row(1)*b[0] + da.row(0)*b[1] - a[1]*db.row(0) + a[0]*db.row(1);
  ret.row(3) = -da.row(5)*b[1] + da.row(4)*b[2] - da.row(2)*b[4] + da.row(1)*b[5] - a[5]*db.row(1) + a[4]*db.row(2) - a[2]*db.row(4) + a[1]*db.row(5);
  ret.row(4) =  da.row(5)*b[0] - da.row(3)*b[2] + da.row(2)*b[3] - da.row(0)*b[5] + a[5]*db.row(0) - a[3]*db.row(2) + a[2]*db.row(3) - a[0]*db.row(5);
  ret.row(5) = -da.row(4)*b[0] + da.row(3)*b[1] - da.row(1)*b[3] + da.row(0)*b[4] - a[4]*db.row(0) + a[3]*db.row(1) - a[1]*db.row(3) + a[0]*db.row(4);
  return ret;
}

template<typename DerivedA, typename DerivedB>
Eigen::Matrix<typename DerivedA::Scalar, TWIST_SIZE, Eigen::Dynamic> dCrossSpatialForce(
  const Eigen::MatrixBase<DerivedA>& a,
  const Eigen::MatrixBase<DerivedB>& b,
  const typename Gradient<DerivedA, Eigen::Dynamic>::type& da,
  const typename Gradient<DerivedB, Eigen::Dynamic>::type& db) {
  Eigen::Matrix<typename DerivedA::Scalar, TWIST_SIZE, Eigen::Dynamic> ret(TWIST_SIZE, da.cols());
  ret.row(0) =  da.row(2)*b[1] - da.row(1)*b[2] + da.row(5)*b[4] - da.row(4)*b[5] + a[2]*db.row(1) - a[1]*db.row(2) + a[5]*db.row(4) - a[4]*db.row(5);
  ret.row(1) = -da.row(2)*b[0] + da.row(0)*b[2] - da.row(5)*b[3] + da.row(3)*b[5] - a[2]*db.row(0) + a[0]*db.row(2) - a[5]*db.row(3) + a[3]*db.row(5);
  ret.row(2) =  da.row(1)*b[0] - da.row(0)*b[1] + da.row(4)*b[3] - da.row(3)*b[4] + a[1]*db.row(0) - a[0]*db.row(1) + a[4]*db.row(3) - a[3]*db.row(4);
  ret.row(3) =  da.row(2)*b[4] - da.row(1)*b[5] + a[2]*db.row(4) - a[1]*db.row(5);
  ret.row(4) = -da.row(2)*b[3] + da.row(0)*b[5] - a[2]*db.row(3) + a[0]*db.row(5);
  ret.row(5) =  da.row(1)*b[3] - da.row(0)*b[4] + a[1]*db.row(3) - a[0]*db.row(4);
  ret = -ret;
  return ret;
}

template<typename DerivedS, typename DerivedQdotToV>
typename DHomogTrans<DerivedQdotToV>::type dHomogTrans(
    const Eigen::Transform<typename DerivedQdotToV::Scalar, 3, Eigen::Isometry>& T,
    const Eigen::MatrixBase<DerivedS>& S,
    const Eigen::MatrixBase<DerivedQdotToV>& qdot_to_v) {
  const int nq_at_compile_time = DerivedQdotToV::ColsAtCompileTime;
  typename DerivedQdotToV::Index nq = qdot_to_v.cols();
  auto qdot_to_twist = (S * qdot_to_v).eval();

  const int numel = HOMOGENEOUS_TRANSFORM_SIZE;
  Eigen::Matrix<typename DerivedQdotToV::Scalar, numel, nq_at_compile_time> ret(numel, nq);

  const auto& Rx = T.linear().col(0);
  const auto& Ry = T.linear().col(1);
  const auto& Rz = T.linear().col(2);

  const auto& qdot_to_omega_x = qdot_to_twist.row(0);
  const auto& qdot_to_omega_y = qdot_to_twist.row(1);
  const auto& qdot_to_omega_z = qdot_to_twist.row(2);

  ret.template middleRows<3>(0) = -Rz * qdot_to_omega_y + Ry * qdot_to_omega_z;
  ret.row(3).setZero();

  ret.template middleRows<3>(4) = Rz * qdot_to_omega_x - Rx * qdot_to_omega_z;
  ret.row(7).setZero();

  ret.template middleRows<3>(8) = -Ry * qdot_to_omega_x + Rx * qdot_to_omega_y;
  ret.row(11).setZero();

  ret.template middleRows<3>(12) = T.linear() * qdot_to_twist.bottomRows(3);
  ret.row(15).setZero();

  return ret;
}

template<typename DerivedDT>
typename DHomogTrans<DerivedDT>::type dHomogTransInv(
    const Eigen::Transform<typename DerivedDT::Scalar, 3, Eigen::Isometry>& T,
    const Eigen::MatrixBase<DerivedDT>& dT) {
  typename DerivedDT::Index nq = dT.cols();

  const auto& R = T.linear();
  const auto& p = T.translation();

  std::array<int, 3> rows = {0, 1, 2};
  std::array<int, 3> R_cols = {0, 1, 2};
  std::array<int, 1> p_cols = {3};

  auto dR = getSubMatrixGradient<Eigen::Dynamic>(dT, rows, R_cols, T.Rows);
  auto dp = getSubMatrixGradient<Eigen::Dynamic>(dT, rows, p_cols, T.Rows);

  auto dinvT_R = transposeGrad(dR, R.rows());
  auto dinvT_p = (-R.transpose() * dp - matGradMult(dinvT_R, p)).eval();

  const int numel = HOMOGENEOUS_TRANSFORM_SIZE;
  Eigen::Matrix<typename DerivedDT::Scalar, numel, DerivedDT::ColsAtCompileTime> ret(numel, nq);
  setSubMatrixGradient<Eigen::Dynamic>(ret, dinvT_R, rows, R_cols, T.Rows);
  setSubMatrixGradient<Eigen::Dynamic>(ret, dinvT_p, rows, p_cols, T.Rows);

  // zero out gradient of elements in last row:
  const int last_row = 3;
  for (int col = 0; col < T.HDim; col++) {
    ret.row(last_row + col * T.Rows).setZero();
  }

  return ret;
}

template <typename Scalar, typename DerivedX, typename DerivedDT, typename DerivedDX>
typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime, 1>::type dTransformSpatialMotion(
    const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
    const Eigen::MatrixBase<DerivedX>& X,
    const Eigen::MatrixBase<DerivedDT>& dT,
    const Eigen::MatrixBase<DerivedDX>& dX) {
  assert(dT.cols() == dX.cols());
  typename DerivedDT::Index nq = dT.cols();

  const auto& R = T.linear();
  const auto& p = T.translation();

  std::array<int, 3> rows = {0, 1, 2};
  std::array<int, 3> R_cols = {0, 1, 2};
  std::array<int, 1> p_cols = {3};

  auto dR = getSubMatrixGradient<Eigen::Dynamic>(dT, rows, R_cols, T.Rows);
  auto dp = getSubMatrixGradient<Eigen::Dynamic>(dT, rows, p_cols, T.Rows);

  typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime, 1>::type ret(X.size(), nq);
  std::array<int, 3> Xomega_rows = {0, 1, 2};
  std::array<int, 3> Xv_rows = {3, 4, 5};
  for (int col = 0; col < X.cols(); col++) {
    auto Xomega_col = X.template block<3, 1>(0, col);
    auto Xv_col = X.template block<3, 1>(3, col);

    auto RXomega_col = (R * Xomega_col).eval();

    std::array<int, 1> col_array = {col};
    auto dXomega_col = getSubMatrixGradient<Eigen::Dynamic>(dX, Xomega_rows, col_array, X.rows());
    auto dXv_col = getSubMatrixGradient<Eigen::Dynamic>(dX, Xv_rows, col_array, X.rows());

    auto domega_part_col = (R * dXomega_col + matGradMult(dR, Xomega_col)).eval();
    auto dv_part_col = (R * dXv_col + matGradMult(dR, Xv_col)).eval();
    dv_part_col += dp.colwise().cross(RXomega_col);
    dv_part_col -= domega_part_col.colwise().cross(p);

    setSubMatrixGradient<Eigen::Dynamic>(ret, domega_part_col, Xomega_rows, col_array, X.rows());
    setSubMatrixGradient<Eigen::Dynamic>(ret, dv_part_col, Xv_rows, col_array, X.rows());
  }
  return ret;
}

template <typename Scalar, typename DerivedX, typename DerivedDT, typename DerivedDX>
typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime>::type dTransformSpatialForce(
    const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
    const Eigen::MatrixBase<DerivedX>& X,
    const Eigen::MatrixBase<DerivedDT>& dT,
    const Eigen::MatrixBase<DerivedDX>& dX) {
  assert(dT.cols() == dX.cols());
  typename DerivedDT::Index nq = dT.cols();

  const auto& R = T.linear();
  const auto& p = T.translation();

  std::array<int, 3> rows = {0, 1, 2};
  std::array<int, 3> R_cols = {0, 1, 2};
  std::array<int, 1> p_cols = {3};

  auto dR = getSubMatrixGradient<Eigen::Dynamic>(dT, rows, R_cols, T.Rows);
  auto dp = getSubMatrixGradient<Eigen::Dynamic>(dT, rows, p_cols, T.Rows);

  typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime>::type ret(X.size(), nq);
  std::array<int, 3> Xomega_rows = {0, 1, 2};
  std::array<int, 3> Xv_rows = {3, 4, 5};
  for (int col = 0; col < X.cols(); col++) {
    auto Xomega_col = X.template block<3, 1>(0, col);
    auto Xv_col = X.template block<3, 1>(3, col);

    auto RXv_col = (R * Xv_col).eval();

    std::array<int, 1> col_array = {col};
    auto dXomega_col = getSubMatrixGradient<Eigen::Dynamic>(dX, Xomega_rows, col_array, X.rows());
    auto dXv_col = getSubMatrixGradient<Eigen::Dynamic>(dX, Xv_rows, col_array, X.rows());

    auto domega_part_col = (R * dXomega_col).eval();
    domega_part_col += matGradMult(dR, Xomega_col);
    auto dv_part_col = (R * dXv_col).eval();
    dv_part_col += matGradMult(dR, Xv_col);
    domega_part_col += dp.colwise().cross(RXv_col);
    domega_part_col -= dv_part_col.colwise().cross(p);

    setSubMatrixGradient<Eigen::Dynamic>(ret, domega_part_col, Xomega_rows, col_array, X.rows());
    setSubMatrixGradient<Eigen::Dynamic>(ret, dv_part_col, Xv_rows, col_array, X.rows());
  }
  return ret;
}

template<typename Scalar >
DLLEXPORT void cylindrical2cartesian(const Matrix<Scalar,3,1> &m_cylinder_axis, const Matrix<Scalar,3,1> &m_cylinder_x_dir, const Matrix<Scalar,3,1> &cylinder_origin, const Matrix<Scalar,6,1> &x_cylinder, const Matrix<Scalar,6,1> &v_cylinder, Matrix<Scalar,6,1> &x_cartesian, Matrix<Scalar,6,1> &v_cartesian, Matrix<Scalar,6,6> &J, Matrix<Scalar,6,1> &Jdotv)
{
  Matrix<Scalar,3,1> cylinder_axis = m_cylinder_axis/m_cylinder_axis.norm();
  Matrix<Scalar,3,1> cylinder_x_dir = m_cylinder_x_dir/m_cylinder_x_dir.norm();
  Matrix<Scalar,3,3> R_cylinder2cartesian;
  R_cylinder2cartesian.col(0) = cylinder_x_dir;
  R_cylinder2cartesian.col(1) = cylinder_axis.cross(cylinder_x_dir);
  R_cylinder2cartesian.col(2) = cylinder_axis;
  double radius = x_cylinder(0);
  double theta = x_cylinder(1);
  double c_theta = cos(theta);
  double s_theta = sin(theta);
  double height = x_cylinder(2);
  double radius_dot = v_cylinder(0);
  double theta_dot = v_cylinder(1);
  double height_dot = v_cylinder(2);
  Matrix<Scalar,3,1> x_pos_cartesian;
  x_pos_cartesian << radius*c_theta, radius*s_theta, height;
  x_pos_cartesian = R_cylinder2cartesian*x_pos_cartesian+cylinder_origin;
  Matrix<Scalar,3,1> v_pos_cartesian;
  v_pos_cartesian << radius*-s_theta*theta_dot+radius_dot*c_theta, radius*c_theta*theta_dot+radius_dot*s_theta, height_dot;
  v_pos_cartesian = R_cylinder2cartesian*v_pos_cartesian;
  Vector3d x_rpy_cylinder = x_cylinder.block(3,0,3,1);
  Matrix<Scalar,3,3> R_tangent = rpy2rotmat(x_rpy_cylinder);
Matrix<Scalar,3,3> R_tangent2cylinder;
  Matrix<Scalar,3,3> dR_tangent2cylinder;
  Matrix<Scalar,3,3> ddR_tangent2cylinder;
  rotz(theta-M_PI/2,R_tangent2cylinder,dR_tangent2cylinder, ddR_tangent2cylinder);
  Matrix<Scalar,3,3> dR_tangent2cylinder_dtheta = dR_tangent2cylinder;
  Matrix<Scalar,3,3> R_cylinder = R_tangent2cylinder*R_tangent;
  Matrix<Scalar,3,3> R_cartesian = R_cylinder2cartesian*R_cylinder;
  Matrix<Scalar,3,1> x_rpy_cartesian = rotmat2rpy(R_cartesian);
  x_cartesian.block(0,0,3,1) = x_pos_cartesian;
  x_cartesian.block(3,0,3,1) = x_rpy_cartesian;
  v_cartesian.block(0,0,3,1) = v_pos_cartesian;
  v_cartesian.block(3,0,3,1) = theta_dot*R_cylinder2cartesian.col(2)+R_cylinder2cartesian*R_tangent2cylinder*v_cylinder.block(3,0,3,1);
  J = Matrix<Scalar,6,6>::Zero();
  J.block(0,0,3,1) << c_theta,s_theta,0;
  J.block(0,1,3,1) << radius*-s_theta,radius*c_theta,0;
  J.block(0,2,3,1) << 0,0,1;
  J.block(0,0,3,3) = R_cylinder2cartesian*J.block(0,0,3,3);
  J.block(3,1,3,1) = R_cylinder2cartesian.col(2);
  J.block(3,3,3,3) = R_cylinder2cartesian*R_tangent2cylinder;
  Matrix<Scalar,3,3> dJ1_dradius = Matrix<Scalar,3,3>::Zero();
  dJ1_dradius(0,1) = -s_theta;
  dJ1_dradius(1,1) = c_theta;
  Matrix<Scalar,3,3> dJ1_dtheta = Matrix<Scalar,3,3>::Zero();
  dJ1_dtheta(0,0) = -s_theta;
  dJ1_dtheta(0,1) = -radius*c_theta;
  dJ1_dtheta(1,0) = c_theta;
  dJ1_dtheta(1,1) = -radius*s_theta;
  Jdotv.block(0,0,3,1) = R_cylinder2cartesian*(dJ1_dradius*radius_dot+dJ1_dtheta*theta_dot)*v_cylinder.block(0,0,3,1);
  Jdotv.block(3,0,3,1) = R_cylinder2cartesian*dR_tangent2cylinder_dtheta*theta_dot*v_cylinder.block(3,0,3,1);
}

template <typename Scalar>
DLLEXPORT  void cartesian2cylindrical(const Eigen::Matrix<Scalar,3,1> &m_cylinder_axis, const Eigen::Matrix<Scalar,3,1> &m_cylinder_x_dir, const Eigen::Matrix<Scalar,3,1> & cylinder_origin, const Eigen::Matrix<Scalar,6,1> &x_cartesian, const Eigen::Matrix<Scalar,6,1> &v_cartesian, Eigen::Matrix<Scalar,6,1> &x_cylinder, Eigen::Matrix<Scalar,6,1> &v_cylinder, Eigen::Matrix<Scalar,6,6> &J, Eigen::Matrix<Scalar,6,1> &Jdotv )
{
  Matrix<Scalar,3,1> cylinder_axis = m_cylinder_axis/m_cylinder_axis.norm();
  Matrix<Scalar,3,1> cylinder_x_dir = m_cylinder_x_dir/m_cylinder_x_dir.norm();
  Matrix<Scalar,3,3> R_cylinder2cartesian;
  R_cylinder2cartesian.col(0) = cylinder_x_dir;
  R_cylinder2cartesian.col(1) = cylinder_axis.cross(cylinder_x_dir);
  R_cylinder2cartesian.col(2) = cylinder_axis;
  Matrix<Scalar,3,3> R_cartesian2cylinder = R_cylinder2cartesian.transpose();
  Matrix<Scalar,3,1> x_pos_cylinder = R_cartesian2cylinder*(x_cartesian.block(0,0,3,1)-cylinder_origin);
  Matrix<Scalar,3,1> v_pos_cylinder = R_cartesian2cylinder*v_cartesian.block(0,0,3,1);
  double radius = sqrt(pow(x_pos_cylinder(0),2)+pow(x_pos_cylinder(1),2));
  double radius_dot = (x_pos_cylinder(0)*v_pos_cylinder(0)+x_pos_cylinder(1)*v_pos_cylinder(1))/radius;
  double theta = atan2(x_pos_cylinder(1),x_pos_cylinder(0));
  double radius_square = pow(radius,2);
  double radius_cubic = pow(radius,3);
  double radius_quad = pow(radius,4);
  double theta_dot = (-x_pos_cylinder(1)*v_pos_cylinder(0)+x_pos_cylinder(0)*v_pos_cylinder(1))/radius_square;
  double height = x_pos_cylinder(2);
  double height_dot = v_pos_cylinder(2);
  x_cylinder(0) = radius;
  x_cylinder(1) = theta;
  x_cylinder(2) = height;
  v_cylinder(0) = radius_dot;
  v_cylinder(1) = theta_dot;
  v_cylinder(2) = height_dot;
  Matrix<Scalar,3,3> R_tangent2cylinder;
  Matrix<Scalar,3,3> dR_tangent2cylinder;
  Matrix<Scalar,3,3> ddR_tangent2cylinder;
  rotz(theta-M_PI/2,R_tangent2cylinder,dR_tangent2cylinder, ddR_tangent2cylinder);
  Matrix<Scalar,3,3> R_cylinder2tangent = R_tangent2cylinder.transpose();
  Vector3d x_rpy_cartesian = x_cartesian.block(3,0,3,1);
  Matrix<Scalar,3,3> R_cartesian = rpy2rotmat(x_rpy_cartesian);
  x_cylinder.block(3,0,3,1) = rotmat2rpy(R_cylinder2tangent*R_cartesian2cylinder*R_cartesian);
  J = Matrix<Scalar,6,6>::Zero();
  Matrix<Scalar,6,6> Jdot = Matrix<Scalar,6,6>::Zero();
  J(0,0) = x_pos_cylinder(0)/radius;
  J(0,1) = x_pos_cylinder(1)/radius;
  J(1,0) = -x_pos_cylinder(1)/radius_square;
  J(1,1) = x_pos_cylinder(0)/radius_square;
  J(2,2) = 1.0;
  J.block(0,0,3,3) = J.block(0,0,3,3)*R_cartesian2cylinder;
  Jdot(0,0) = pow(x_pos_cylinder(1),2)/radius_cubic*v_pos_cylinder(0)-x_pos_cylinder(0)*x_pos_cylinder(1)/radius_cubic*v_pos_cylinder(1);
  Jdot(0,1) = -x_pos_cylinder(0)*x_pos_cylinder(1)/radius_cubic*v_pos_cylinder(0)+pow(x_pos_cylinder(0),2)/radius_cubic*v_pos_cylinder(1);
  Jdot(1,0) = 2*x_pos_cylinder(0)*x_pos_cylinder(1)/radius_quad*v_pos_cylinder(0)+(pow(x_pos_cylinder(1),2)-pow(x_pos_cylinder(0),2))/radius_quad*v_pos_cylinder(1);
  Jdot(1,1) = (pow(x_pos_cylinder(1),2)-pow(x_pos_cylinder(0),2))/radius_quad*v_pos_cylinder(0)-2*x_pos_cylinder(0)*x_pos_cylinder(1)/radius_quad*v_pos_cylinder(1);
  Jdot.block(0,0,3,3) = Jdot.block(0,0,3,3)*R_cartesian2cylinder;
  v_cylinder.block(3,0,3,1) = R_cylinder2tangent*R_cartesian2cylinder*v_cartesian.block(3,0,3,1)-theta_dot*R_cylinder2tangent.col(2);
  J.block(3,0,3,3) = R_cylinder2tangent.col(2)*-J.block(1,0,1,3);
  J.block(3,3,3,3) = R_cylinder2tangent*R_cartesian2cylinder;
  Jdot.block(3,0,3,3) = dR_tangent2cylinder.row(2).transpose()*-J.block(1,0,1,3)*theta_dot+R_cylinder2tangent.col(2)*-Jdot.block(1,0,1,3);
  Jdot.block(3,3,3,3) = dR_tangent2cylinder.transpose()*theta_dot*R_cartesian2cylinder;
  Jdotv = Jdot*v_cartesian;
}

DLLEXPORT GradientVar<double,3,1> quat2expmap(const Ref<const Vector4d> &q, int gradient_order)
{
  double t = sqrt(1-q(0)*q(0));
  bool is_degenerate=(t*t<std::numeric_limits<double>::epsilon());
  double s = is_degenerate?2.0:2.0*acos(q(0))/t;
  GradientVar<double,3,1> ret(3,1,4,gradient_order);
  ret.value() = s*q.tail(3);
  if(gradient_order>0)
  {
    ret.gradient().value() = Matrix<double,3,4>::Zero();
    double dsdq1 = is_degenerate?0.0: (-2*t+2*acos(q(0))*q(0))/pow(t,3);
    ret.gradient().value().col(0) = q.tail(3)*dsdq1;
    ret.gradient().value().block(0,1,3,3) = Matrix3d::Identity()*s;
  }
  else if(gradient_order>1)
  {
    throw std::runtime_error("gradient_order>1 is not supported in quat2expmap");
  }
  return ret;
}

DLLEXPORT GradientVar<double,3,1> flipExpmap(const Ref<const Vector3d> &expmap, int gradient_order)
{
  if(gradient_order>1)
  {
    throw std::runtime_error("gradient_order>1 is not supported in flipExpmap");
  }
  double expmap_norm = expmap.norm();
  bool is_degenerate=(expmap_norm<std::numeric_limits<double>::epsilon());
  GradientVar<double,3,1> ret(3,1,3,gradient_order);
  Matrix3d eye3 = Matrix3d::Identity();
  if(is_degenerate)
  {
    ret.value() = expmap;
    if(gradient_order>0)
    {
      ret.gradient().value() = eye3;
    }
  }
  else
  {
    ret.value() = expmap-expmap/expmap_norm*2*M_PI;
    if(gradient_order>0)
    {
      ret.gradient().value() = eye3-(expmap_norm*expmap_norm*eye3-expmap*expmap.transpose())/pow(expmap_norm,3)*2*M_PI;
    }
  }
  return ret;
}

DLLEXPORT GradientVar<double, 3,1> unwrapExpmap(const Ref<const Vector3d> & expmap1, const Ref<const Vector3d> &expmap2, int gradient_order)
{
  auto expmap2_flip = flipExpmap(expmap2,gradient_order);
  double distance1 = (expmap1-expmap2).squaredNorm();
  double distance2 = (expmap1-expmap2_flip.value()).squaredNorm();
  if(distance1>distance2)
  {
    return expmap2_flip;
  }
  else
  {
    GradientVar<double,3,1> ret(3,1,3,gradient_order);
    ret.value() = expmap2;
    if(gradient_order>0)
    {
      ret.gradient().value() = Matrix3d::Identity();
    }
    return ret;
  }
}

void quat2expmapSequence(const Ref<const Matrix<double,4,Dynamic>> &quat, const Ref<const Matrix<double,4,Dynamic>> &quat_dot, Ref<Matrix<double,3,Dynamic>> expmap, Ref<Matrix<double,3,Dynamic>> expmap_dot)
{
  int N = quat.cols();
  if(quat_dot.cols() != N)
  {
    throw std::runtime_error("quat_dot must have the same number of columns as quat in quat2expmapSequence");
  }
  expmap.resize(3,N);
  expmap_dot.resize(3,N);
  for(int i = 0;i<N;i++)
  {
    auto expmap_grad = quat2expmap(quat.col(i),1);
    expmap.col(i) = expmap_grad.value();
    expmap_dot.col(i) = expmap_grad.gradient().value()*quat_dot.col(i);
    if(i>=1)
    {
      auto unwrap_grad = unwrapExpmap(expmap.col(i-1),expmap.col(i),1);
      expmap.col(i) = unwrap_grad.value();
      expmap_dot.col(i) = unwrap_grad.gradient().value()*expmap_dot.col(i);
    }
  }
}

// explicit instantiations
template DLLEXPORT void normalizeVec(
    const MatrixBase< Vector3d >& x,
    Vector3d& x_norm,
    Gradient<Vector3d, 3, 1>::type*,
    Gradient<Vector3d, 3, 2>::type*);

template DLLEXPORT void normalizeVec(
    const MatrixBase< Vector4d >& x,
    Vector4d& x_norm,
    Gradient<Vector4d, 4, 1>::type*,
    Gradient<Vector4d, 4, 2>::type*);

template DLLEXPORT void normalizeVec(
    const MatrixBase< Map<Vector4d> >& x,
    Vector4d& x_norm,
    Gradient<Vector4d, 4, 1>::type*,
    Gradient<Vector4d, 4, 2>::type*);

template DLLEXPORT void normalizeVec(
    const MatrixBase< Eigen::Block<Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > const, 4, 1, false> >& x,
    Vector4d& x_norm,
    Gradient<Vector4d, 4, 1>::type*,
    Gradient<Vector4d, 4, 2>::type*);

template DLLEXPORT Vector4d quat2axis(const MatrixBase<Vector4d>&);
template DLLEXPORT Matrix3d quat2rotmat(const MatrixBase<Vector4d>& q);
template DLLEXPORT Matrix3d quat2rotmat(const MatrixBase<Eigen::Block<Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > const, 4, 1, false> >& q);
template DLLEXPORT Vector3d quat2rpy(const MatrixBase<Vector4d>&);

template DLLEXPORT Vector4d axis2quat(const MatrixBase<Vector4d>&);
template DLLEXPORT Matrix3d axis2rotmat(const MatrixBase<Vector4d>&);
template DLLEXPORT Vector3d axis2rpy(const MatrixBase<Vector4d>&);

template DLLEXPORT Vector4d rotmat2axis(const MatrixBase<Matrix3d>&);
template DLLEXPORT Vector4d rotmat2quat(const MatrixBase<Matrix3d>&);
template DLLEXPORT Vector3d rotmat2rpy(const MatrixBase<Matrix3d>&);

template DLLEXPORT Eigen::Matrix<Eigen::Block<Eigen::Matrix<double, 4, 4, 0, 4, 4>, 3, 3, false>::Scalar, -1, 1, 0, -1, 1> rotmat2Representation<Eigen::Block<Eigen::Matrix<double, 4, 4, 0, 4, 4>, 3, 3, false> >(Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, 4, 4, 0, 4, 4>, 3, 3, false> > const&, int);

template DLLEXPORT GradientVar<double, Eigen::Dynamic, 1> rotmat2Representation(
    const GradientVar<double, SPACE_DIMENSION, SPACE_DIMENSION>& R,
    int rotation_type);

template DLLEXPORT GradientVar<double, QUAT_SIZE, 1> expmap2quat(const MatrixBase<Vector3d>& v, const int gradient_order);
template DLLEXPORT GradientVar<double, QUAT_SIZE, 1> expmap2quat(const MatrixBase<Map<Vector3d>>& v, const int gradient_order);

template DLLEXPORT Vector4d rpy2axis(const Eigen::MatrixBase<Vector3d>&);
template DLLEXPORT Vector4d rpy2quat(const Eigen::MatrixBase<Vector3d>&);
template DLLEXPORT Matrix3d rpy2rotmat(const Eigen::MatrixBase<Vector3d>&);
template DLLEXPORT Matrix3d rpy2rotmat(const Eigen::MatrixBase<Eigen::Block<Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > const, 3, 1, false>>&);

template DLLEXPORT Matrix<double,9,3> drpy2rotmat(const Eigen::MatrixBase<Vector3d>&);
template DLLEXPORT Matrix<double,9,3> drpy2rotmat(const Eigen::MatrixBase<Eigen::Block<Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > const, 3, 1, false>>&);

template DLLEXPORT Vector4d quat2axis(const MatrixBase< Map<Vector4d> >&);
template DLLEXPORT Matrix3d quat2rotmat(const MatrixBase< Map<Vector4d> >& q);
template DLLEXPORT Vector3d quat2rpy(const MatrixBase< Map<Vector4d> >&);

template DLLEXPORT Vector4d axis2quat(const MatrixBase< Map<Vector4d> >&);
template DLLEXPORT Matrix3d axis2rotmat(const MatrixBase< Map<Vector4d> >&);
template DLLEXPORT Vector3d axis2rpy(const MatrixBase< Map<Vector4d> >&);

template DLLEXPORT Vector4d rotmat2axis(const MatrixBase< Map<Matrix3d> >&);
template DLLEXPORT Vector4d rotmat2quat(const MatrixBase< Map<Matrix3d> >&);
template DLLEXPORT Vector3d rotmat2rpy(const MatrixBase< Map<Matrix3d> >&);

template DLLEXPORT Vector4d rpy2axis(const Eigen::MatrixBase< Map<Vector3d> >&);
template DLLEXPORT Vector4d rpy2quat(const Eigen::MatrixBase< Map<Vector3d> >&);
template DLLEXPORT Matrix3d rpy2rotmat(const Eigen::MatrixBase< Map<Vector3d> >&);
template DLLEXPORT Matrix<double,9,3> drpy2rotmat(const Eigen::MatrixBase< Map<Vector3d> >&);

template DLLEXPORT Eigen::Matrix<double, TWIST_SIZE, Eigen::Dynamic> transformSpatialMotion(
    const Eigen::Isometry3d&,
    const Eigen::MatrixBase< Eigen::Matrix<double, TWIST_SIZE, Eigen::Dynamic> >&);

template DLLEXPORT Eigen::Matrix<double, TWIST_SIZE, 1> transformSpatialMotion(
    const Eigen::Isometry3d&,
    const Eigen::MatrixBase< Eigen::Matrix<double, TWIST_SIZE, 1> >&);

template DLLEXPORT TransformSpatial< MatrixXd >::type transformSpatialMotion<MatrixXd>(
    const Eigen::Isometry3d&,
    const Eigen::MatrixBase< MatrixXd >&);

template DLLEXPORT TransformSpatial<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 6, -1, false> >::type transformSpatialMotion(
    const Eigen::Isometry3d& T,
    const Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 6, -1, false> >& M);

template DLLEXPORT TransformSpatial< Matrix<double, TWIST_SIZE, Eigen::Dynamic> >::type transformSpatialForce<Matrix<double, TWIST_SIZE, Eigen::Dynamic>>(
    const Eigen::Isometry3d&,
    const Eigen::MatrixBase< Matrix<double, TWIST_SIZE, Eigen::Dynamic> >&);

template DLLEXPORT TransformSpatial< MatrixXd >::type transformSpatialForce<MatrixXd>(
    const Eigen::Isometry3d&,
    const Eigen::MatrixBase< MatrixXd >&);

template DLLEXPORT TransformSpatial<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 6, -1, true>>::type transformSpatialForce(
    const Eigen::Isometry3d&,
    const Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 6, -1, true> >&);

template DLLEXPORT TransformSpatial<Eigen::Matrix<double, 6, 1, 0, 6, 1> >::type transformSpatialForce(
    const Eigen::Isometry3d&,
    const Eigen::MatrixBase<Eigen::Matrix<double, 6, 1, 0, 6, 1> >&);

template DLLEXPORT TransformSpatial<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1> const, 6, 1, true> >::type transformSpatialForce(
    const Eigen::Transform<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1> const, 6, 1, true>::Scalar, 3, 1, 0> &,
    const Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1> const, 6, 1, true> > &);

template DLLEXPORT TransformSpatial<Map<Matrix<double, 6, 1, 0, 6, 1> const, 0, Stride<0, 0> > >::type transformSpatialForce<Map<Matrix<double, 6, 1, 0, 6, 1> const, 0, Stride<0, 0> >>(
    const Eigen::Isometry3d&,
    const Eigen::MatrixBase<Map<Matrix<double, 6, 1, 0, 6, 1> const, 0, Stride<0, 0> > >&);

template DLLEXPORT GradientVar<double, TWIST_SIZE, TWIST_SIZE> transformSpatialInertia(
    const Eigen::Transform<double, SPACE_DIMENSION, Eigen::Isometry>& T_current_to_new,
    const Gradient<Eigen::Transform<double, SPACE_DIMENSION, Eigen::Isometry>::MatrixType, Eigen::Dynamic>::type* dT_current_to_new,
    const Eigen::MatrixBase< Eigen::Matrix<double, TWIST_SIZE, TWIST_SIZE> >& I);

template DLLEXPORT GradientVar<double, TWIST_SIZE, TWIST_SIZE> transformSpatialInertia(
    const Eigen::Transform<double, SPACE_DIMENSION, Eigen::Isometry>& T_current_to_new,
    const Gradient<Eigen::Transform<double, SPACE_DIMENSION, Eigen::Isometry>::MatrixType, Eigen::Dynamic>::type* dT_current_to_new,
    const Eigen::MatrixBase< Eigen::MatrixXd >& I);

template DLLEXPORT Eigen::Matrix<double, TWIST_SIZE, Eigen::Dynamic> dCrossSpatialMotion(
  const Eigen::MatrixBase<Eigen::Matrix<double, 6, 1, 0, 6, 1> >& a,
  const Eigen::MatrixBase<Eigen::Matrix<double, 6, 1, 0, 6, 1> >& b,
  const Gradient<Eigen::Matrix<double, 6, 1, 0, 6, 1>, Eigen::Dynamic>::type& da,
  const Gradient<Eigen::Matrix<double, 6, 1, 0, 6, 1>, Eigen::Dynamic>::type& db);

template DLLEXPORT Eigen::Matrix<double, TWIST_SIZE, Eigen::Dynamic> dCrossSpatialForce(
  const Eigen::MatrixBase<Eigen::Matrix<double, 6, 1, 0, 6, 1> >& a,
  const Eigen::MatrixBase<Eigen::Matrix<double, 6, 1, 0, 6, 1> >& b,
  const Gradient<Eigen::Matrix<double, 6, 1, 0, 6, 1>, Eigen::Dynamic>::type& da,
  const Gradient<Eigen::Matrix<double, 6, 1, 0, 6, 1>, Eigen::Dynamic>::type& db);

template DLLEXPORT Gradient<Matrix3d, QUAT_SIZE>::type dquat2rotmat(const Eigen::MatrixBase<Vector4d>&);
template DLLEXPORT Gradient<Matrix3d, QUAT_SIZE>::type dquat2rotmat(const Eigen::MatrixBase< Map<Vector4d> >&);
template DLLEXPORT Gradient<Matrix3d, QUAT_SIZE>::type dquat2rotmat(const Eigen::MatrixBase<Eigen::Block<Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > const, 4, 1, false> >&);

template DLLEXPORT Gradient<Vector3d, Dynamic>::type drotmat2rpy(
    const Eigen::MatrixBase<Matrix3d>&,
    const Eigen::MatrixBase< Matrix<double, RotmatSize, Dynamic> >&);

template DLLEXPORT Gradient<Vector3d, 6>::type drotmat2rpy(
    const Eigen::MatrixBase<Matrix3d>&,
    const Eigen::MatrixBase< Matrix<double, RotmatSize, 6> >&);

template DLLEXPORT Gradient<Vector4d, Dynamic>::type drotmat2quat(
    const Eigen::MatrixBase<Matrix3d>&,
    const Eigen::MatrixBase< Matrix<double, RotmatSize, Dynamic> >&);

template DLLEXPORT
Eigen::Matrix<double, 3, 3> vectorToSkewSymmetric(const Eigen::MatrixBase<Eigen::Vector3d>&);

template DLLEXPORT Eigen::Matrix<double, 3, Eigen::Dynamic> dcrossProduct(
    const Eigen::MatrixBase<Vector3d>& a,
    const Eigen::MatrixBase<Vector3d>& b,
    const Gradient<Vector3d, Eigen::Dynamic>::type& da,
    const Gradient<Vector3d, Eigen::Dynamic>::type& db);

template DLLEXPORT Eigen::Matrix<double, 3, Eigen::Dynamic> dcrossProduct(
    const Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, 3, -1, 0, 3, -1>, 3, 1, true>>& a,
    const Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 3, 1, false>>& b,
    const Gradient<Eigen::Block<Eigen::Matrix<double, 3, -1, 0, 3, -1>, 3, 1, true>, Eigen::Dynamic>::type& da,
    const Gradient<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 3, 1, false>, Eigen::Dynamic>::type& db);

template DLLEXPORT Eigen::Matrix<double, 3, Eigen::Dynamic> dcrossProduct(
    const Eigen::MatrixBase< Eigen::Block<Eigen::Matrix<double, 3, -1, 0, 3, -1>, 3, 1, true> >& a,
    const Eigen::MatrixBase< Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 3, 1, false> >& b,
    const Gradient< Eigen::Block<Eigen::Matrix<double, 3, -1, 0, 3, -1>, 3, 1, true>, Eigen::Dynamic>::type& da,
    const Gradient< Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 3, 1, false>, Eigen::Dynamic>::type& db);

template DLLEXPORT Eigen::Matrix<double, 3, Eigen::Dynamic> dcrossProduct(
    const Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1> const, 3, 1, false>>& a,
    const Eigen::MatrixBase<Eigen::Block<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 3, -1, false>, 3, 1, true>>& b,
    const Gradient<Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1> const, 3, 1, false>, Eigen::Dynamic>::type& da,
    const Gradient<Eigen::Block<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 3, -1, false>, 3, 1, true>, Eigen::Dynamic>::type& db);

template DLLEXPORT Eigen::Matrix<double, 3, Eigen::Dynamic> dcrossProduct(
    const Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1> const, 3, 1, false>>& a,
    const Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, 3, -1, 0, 3, -1>, 3, 1, true>>& b,
    const Gradient<Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1> const, 3, 1, false>, Eigen::Dynamic>::type& da,
    const Gradient<Eigen::Block<Eigen::Matrix<double, 3, -1, 0, 3, -1>, 3, 1, true>, Eigen::Dynamic>::type& db);

template DLLEXPORT Eigen::Matrix<double, 3, Eigen::Dynamic> dcrossProduct(
    const Eigen::MatrixBase<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true>>& a,
    const Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1>>& b,
    const Gradient<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true>, Eigen::Dynamic>::type& da,
    const Gradient<Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Dynamic>::type& db);

template DLLEXPORT Eigen::Matrix<double, 3, Eigen::Dynamic> dcrossProduct(
    const Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1>, 3, 1, false>>& a,
    const Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1>>& b,
    const Gradient<Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1>, 3, 1, false>, Eigen::Dynamic>::type& da,
    const Gradient<Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Dynamic>::type& db);

template DLLEXPORT Eigen::Matrix<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true>::Scalar, 3, -1, 0, 3, -1> dcrossProduct<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true>,
    Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 3, 1, false> >(Eigen::MatrixBase<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 3, 1, false> > const&,
    Gradient<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true>, -1, 1>::type const&, Gradient<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 3, 1, false>, -1, 1>::type const&);

template DLLEXPORT Eigen::Matrix<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true>::Scalar, 3, -1, 0, 3, -1> dcrossProduct<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true>,
    Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 3, 1, false> >(Eigen::MatrixBase<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true> > const&,
    Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 3, 1, false> > const&, Gradient<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true>, -1, 1>::type const&,
    Gradient<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 3, 1, false>, -1, 1>::type const&);

template DLLEXPORT Eigen::Matrix<double, 3, -1, 0, 3, -1> dcrossProduct<Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1> const, 3, 1, false>, Eigen::Block<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 3, 1, false>, 3, 1, true> >(
    Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1> const, 3, 1, false> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 3, 1, false>, 3, 1, true> > const&,
    Gradient<Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1> const, 3, 1, false>, -1, 1>::type const&, Gradient<Eigen::Block<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 3, 1, false>, 3, 1, true>, -1, 1>::type const&);

template DLLEXPORT Eigen::Matrix<double, 3, -1, 0, 3, -1> dcrossProduct<Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1> const, 3, 1, false>, Eigen::Block<Eigen::Matrix<double, 3, 1, 0, 3, 1>, 3, 1, true> >(
    Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1> const, 3, 1, false> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, 3, 1, 0, 3, 1>, 3, 1, true> > const&, Gradient<Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1> const, 3, 1, false>, -1, 1>::type const&,
    Gradient<Eigen::Block<Eigen::Matrix<double, 3, 1, 0, 3, 1>, 3, 1, true>, -1, 1>::type const&);

template DLLEXPORT Eigen::Matrix<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true>::Scalar, 3, -1, 0, 3, -1> dcrossProduct<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true>,
    Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 3, 1, false> >(Eigen::MatrixBase<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 3, 1, false> > const&,
    Gradient<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true>, -1, 1>::type const&, Gradient<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 3, 1, false>, -1, 1>::type const&);

template DLLEXPORT Eigen::Matrix<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true>::Scalar, 3, -1, 0, 3, -1> dcrossProduct<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true>,
    Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 3, 1, false> >(Eigen::MatrixBase<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 3, 1, false> > const&,
    Gradient<Eigen::Block<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 3, -1, false>, 3, 1, true>, -1, 1>::type const&, Gradient<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 3, 1, false>, -1, 1>::type const&);

template DLLEXPORT DHomogTrans<MatrixXd>::type dHomogTrans(
    const Isometry3d&,
    const MatrixBase< Matrix<double, TWIST_SIZE, Dynamic> >&,
    const MatrixBase< MatrixXd >&);

template DLLEXPORT DHomogTrans<Matrix<double, HOMOGENEOUS_TRANSFORM_SIZE, Dynamic>>::type dHomogTransInv(
    const Isometry3d&,
    const MatrixBase< Matrix<double, HOMOGENEOUS_TRANSFORM_SIZE, Dynamic> >&);

template DLLEXPORT Gradient< Matrix<double, TWIST_SIZE, Dynamic>, Dynamic, 1>::type dTransformSpatialMotion(
    const Isometry3d&,
    const MatrixBase< Matrix<double, TWIST_SIZE, Dynamic> >&,
    const MatrixBase< Matrix<double, HOMOGENEOUS_TRANSFORM_SIZE, Dynamic> >&,
    const MatrixBase<MatrixXd>&);

template DLLEXPORT Gradient< Matrix<double, TWIST_SIZE, 1>, Dynamic, 1>::type dTransformSpatialMotion(
    const Isometry3d&,
    const MatrixBase< Matrix<double, TWIST_SIZE, 1> >&,
    const MatrixBase< Matrix<double, HOMOGENEOUS_TRANSFORM_SIZE, Dynamic> >&,
    const MatrixBase< Matrix<double, TWIST_SIZE, Eigen::Dynamic> >&);

template DLLEXPORT TransformSpatial<Eigen::Matrix<double, TWIST_SIZE, 1>>::type crossSpatialMotion(
  const Eigen::MatrixBase<Eigen::Matrix<double, TWIST_SIZE, 1> >& a,
  const Eigen::MatrixBase<Eigen::Matrix<double, TWIST_SIZE, 1> >& b);

template DLLEXPORT TransformSpatial<Eigen::Matrix<double, TWIST_SIZE, TWIST_SIZE>>::type crossSpatialMotion(
  const Eigen::MatrixBase<Eigen::Matrix<double, TWIST_SIZE, 1> >& a,
  const Eigen::MatrixBase<Eigen::Matrix<double, TWIST_SIZE, TWIST_SIZE> >& b);

template DLLEXPORT TransformSpatial<Eigen::Matrix<double, TWIST_SIZE, Eigen::Dynamic>>::type crossSpatialMotion(
  const Eigen::MatrixBase<Eigen::Matrix<double, TWIST_SIZE, 1> >& a,
  const Eigen::MatrixBase<Eigen::Matrix<double, TWIST_SIZE, Eigen::Dynamic> >& b);

template DLLEXPORT TransformSpatial<Eigen::Matrix<double, TWIST_SIZE, 1>>::type crossSpatialForce(
  const Eigen::MatrixBase<Eigen::Matrix<double, TWIST_SIZE, 1> >& a,
  const Eigen::MatrixBase<Eigen::Matrix<double, TWIST_SIZE, 1> >& b);

template DLLEXPORT TransformSpatial<Eigen::Matrix<double, TWIST_SIZE, TWIST_SIZE>>::type crossSpatialForce(
  const Eigen::MatrixBase<Eigen::Matrix<double, TWIST_SIZE, 1> >& a,
  const Eigen::MatrixBase<Eigen::Matrix<double, TWIST_SIZE, TWIST_SIZE> >& b);

template DLLEXPORT Gradient< Matrix<double, TWIST_SIZE, Dynamic>, Dynamic, 1>::type dTransformSpatialForce(
    const Isometry3d&,
    const MatrixBase< Matrix<double, TWIST_SIZE, Dynamic> >&,
    const MatrixBase< Matrix<double, HOMOGENEOUS_TRANSFORM_SIZE, Dynamic> >&,
    const MatrixBase<MatrixXd>&);

template DLLEXPORT Gradient< Matrix<double, TWIST_SIZE, Dynamic>, Dynamic, 1>::type dTransformSpatialForce(
    const Isometry3d&,
    const MatrixBase< Matrix<double, TWIST_SIZE, Dynamic> >&,
    const MatrixBase<MatrixXd>&,
    const MatrixBase<MatrixXd>&);

template DLLEXPORT Gradient<Eigen::Matrix<double, 6, 1, 0, 6, 1>, Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 6, -1, true>::ColsAtCompileTime>::type dTransformSpatialForce(
    const Isometry3d& T,
    const Eigen::MatrixBase<Eigen::Matrix<double, 6, 1, 0, 6, 1> >& X,
    const Eigen::MatrixBase<Eigen::Matrix<double, 16, -1, 0, 16, -1> >& dT,
    const Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, 6, -1, 0, 6, -1>, 6, -1, true> >& dX);

template DLLEXPORT Gradient<Eigen::Matrix<double, 6, 1, 0, 6, 1>, Eigen::Matrix<double, 6, -1, 0, 6, -1>::ColsAtCompileTime>::type dTransformSpatialForce(
    const Isometry3d& T,
    const Eigen::MatrixBase<Eigen::Matrix<double, 6, 1, 0, 6, 1>>& X,
    const Eigen::MatrixBase<Eigen::Matrix<double, 16, -1, 0, 16, -1>>& dT,
    const Eigen::MatrixBase<Eigen::Matrix<double, 6, -1, 0, 6, -1>>& dX);

template DLLEXPORT  void cylindrical2cartesian(const Matrix<double,3,1> &cylinder_axis, const Matrix<double,3,1> &cylinder_x_dir, const Matrix<double,3,1> & cylinder_origin, const Matrix<double,6,1> &x_cylinder, const Matrix<double,6,1> &v_cylinder, Matrix<double,6,1> &x_cartesian, Matrix<double,6,1> &v_cartesian, Matrix<double,6,6> &J, Matrix<double,6,1> &Jdotv );

template DLLEXPORT  void cartesian2cylindrical(const Matrix<double,3,1> &cylinder_axis, const Matrix<double,3,1> &cylinder_x_dir, const Matrix<double,3,1> & cylinder_origin, const Matrix<double,6,1> &x_cartesian, const Matrix<double,6,1> &v_cartesian, Matrix<double,6,1> &x_cylinder, Matrix<double,6,1> &v_cylinder, Matrix<double,6,6> &J, Matrix<double,6,1> &Jdotv );

template DLLEXPORT void angularvel2quatdotMatrix(const Eigen::MatrixBase<Vector4d>& q,
    Eigen::MatrixBase< Matrix<double, QUAT_SIZE, SPACE_DIMENSION> >& M,
    Eigen::MatrixBase< Gradient<Matrix<double, QUAT_SIZE, SPACE_DIMENSION>, QUAT_SIZE, 1>::type>* dM);
template DLLEXPORT void angularvel2quatdotMatrix(const Eigen::MatrixBase<Map<Vector4d>>& q,
    Eigen::MatrixBase< Matrix<double, QUAT_SIZE, SPACE_DIMENSION> >& M,
    Eigen::MatrixBase< Gradient<Matrix<double, QUAT_SIZE, SPACE_DIMENSION>, QUAT_SIZE, 1>::type>* dM);
template DLLEXPORT void angularvel2quatdotMatrix(const Eigen::MatrixBase< Eigen::Block<Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > const, 4, 1, false> >& q,
    Eigen::MatrixBase< Matrix<double, QUAT_SIZE, SPACE_DIMENSION> >& M,
    Eigen::MatrixBase< Gradient<Matrix<double, QUAT_SIZE, SPACE_DIMENSION>, QUAT_SIZE, 1>::type>* dM);

template DLLEXPORT void angularvel2rpydotMatrix(const Eigen::MatrixBase<Vector3d>& rpy,
    Eigen::MatrixBase< Matrix<double, RPY_SIZE, SPACE_DIMENSION> >& phi,
    Eigen::MatrixBase< Gradient<Matrix<double, RPY_SIZE, SPACE_DIMENSION>, RPY_SIZE, 1>::type>* dphi,
    Eigen::MatrixBase< Gradient<Matrix<double, RPY_SIZE, SPACE_DIMENSION>, RPY_SIZE, 2>::type>* ddphi);

template DLLEXPORT void angularvel2rpydotMatrix(const Eigen::MatrixBase<Vector3d>& rpy,
    Eigen::MatrixBase< Matrix<double, RPY_SIZE, SPACE_DIMENSION> >& phi,
    Eigen::MatrixBase< Matrix<double, Eigen::Dynamic, Eigen::Dynamic> >* dphi,
    Eigen::MatrixBase< Matrix<double, Eigen::Dynamic, Eigen::Dynamic> >* ddphi);

template DLLEXPORT void rpydot2angularvelMatrix(const Eigen::MatrixBase<Vector3d>& rpy,
    Eigen::MatrixBase<Eigen::Matrix<double, SPACE_DIMENSION, RPY_SIZE> >& E,
    Gradient<Matrix<double,SPACE_DIMENSION,RPY_SIZE>,RPY_SIZE,1>::type* dE);
template DLLEXPORT void rpydot2angularvelMatrix(const Eigen::MatrixBase<Map<Vector3d>>& rpy,
    Eigen::MatrixBase<Eigen::Matrix<double, SPACE_DIMENSION, RPY_SIZE> >& E,
    Gradient<Matrix<double,SPACE_DIMENSION,RPY_SIZE>,RPY_SIZE,1>::type* dE);
template DLLEXPORT void rpydot2angularvelMatrix(const Eigen::MatrixBase< Eigen::Block<Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > const, 3, 1, false> >& rpy,
    Eigen::MatrixBase<Eigen::Matrix<double, SPACE_DIMENSION, RPY_SIZE> >& E,
    Gradient<Matrix<double,SPACE_DIMENSION,RPY_SIZE>,RPY_SIZE,1>::type* dE);

template DLLEXPORT GradientVar<double, Eigen::Dynamic, SPACE_DIMENSION> angularvel2RepresentationDotMatrix(
    int rotation_type, const Eigen::MatrixBase<VectorXd>& qrot, int gradient_order);
template DLLEXPORT GradientVar<double, Eigen::Dynamic, SPACE_DIMENSION> angularvel2RepresentationDotMatrix(
    int rotation_type, const Eigen::MatrixBase< Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, -1, 1, false> >& qrot, int gradient_order);
template DLLEXPORT GradientVar<double, -1, 3> angularvel2RepresentationDotMatrix<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >(int, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> > const&, int);
template DLLEXPORT GradientVar<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, -1, 1, false>::Scalar, -1, 3> angularvel2RepresentationDotMatrix<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, -1, 1, false> >(int,
    Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, -1, 1, false> > const&, int);
template DLLEXPORT GradientVar<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, -1, 1, false>::Scalar, -1, 3> angularvel2RepresentationDotMatrix<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, -1, 1, false> >(int,
    Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, -1, 1, false> > const&, int);

template DLLEXPORT void quatdot2angularvelMatrix(const Eigen::MatrixBase<Vector4d>& q,
    Eigen::MatrixBase< Matrix<double, SPACE_DIMENSION, QUAT_SIZE> >& M,
    Gradient<Matrix<double, SPACE_DIMENSION, QUAT_SIZE>, QUAT_SIZE, 1>::type* dM);
template DLLEXPORT void quatdot2angularvelMatrix(const Eigen::MatrixBase<Map<Vector4d>>& q,
    Eigen::MatrixBase< Matrix<double, SPACE_DIMENSION, QUAT_SIZE> >& M,
    Gradient<Matrix<double, SPACE_DIMENSION, QUAT_SIZE>, QUAT_SIZE, 1>::type* dM);
template DLLEXPORT void quatdot2angularvelMatrix(const Eigen::MatrixBase< Eigen::Block<Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > const, 4, 1, false> >& q,
    Eigen::MatrixBase< Matrix<double, SPACE_DIMENSION, QUAT_SIZE> >& M,
    Gradient<Matrix<double, SPACE_DIMENSION, QUAT_SIZE>, QUAT_SIZE, 1>::type* dM);


/*
* compile with flags:   g++ test.cc softmax_classifier.cc   ../datasets/datasets.cc -std=c++14 -larmadillo -I ../../include/
* author: Yuzhen Liu
* Date: 2019.4.16 21:35
*/

#include <iostream>
#include <armadillo>
#include <cmath>
#include <factorization/fm.h>
#include <datasets/datasets.h>

using namespace std;
using namespace arma;

int main() {
    Datasets dataset = Datasets("iris");

    // Logistic classifier testing
    vec y = dataset.y.subvec(0, 99);
    mat x = dataset.x.submat(0, 0, 3, 99);
    uvec field(4, fill::ones);
    FM fm = FM();
    fm.train(x, field, y);
    // vec res = fm.predict(mat({{6.1, 2.9, 4.7, 1.4}, {5.1, 3.5, 1.4, 0.2}}).t());
    vec res = fm.predict(x, field);
    (res - y).print();

    return 0;
}


#include <iostream>
#include <NTL/lzz_p.h>
#include <NTL/lzz_pX.h>

using namespace NTL;

int main()
{
  int n = 24680;
  n -= 1;
  long mod = 1020202009;
  int fac_n = 1;
  for (int i = 2; i <= n; i++) {
    fac_n = (long)fac_n * i % mod;
  }
  zz_p::init(mod);
  zz_pX den;
  den.SetLength(n);
  int inv_f = InvMod(fac_n,mod);
  for (int i = n; i; i--) {
    switch (i % 4) {
      case 1: SetCoeff(den, i, mod-inv_f); break;
      case 3: SetCoeff(den, i, inv_f); break;
    }
    inv_f = (long)inv_f * i % mod;
  }
  SetCoeff(den,0, 1);
  zz_pX egf = InvTrunc(den,n+1);
  std::cout << zz_p(fac_n) * coeff(egf,n) << std::endl;
}


#include <iostream>
#include <cmath>

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

int main() {

  Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();

  Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1));
  std::cout << "rotation_matrix = \n" << rotation_vector.matrix() << std::endl << std::endl;

  rotation_matrix = rotation_vector.toRotationMatrix();

  Eigen::Vector3d v(1, 0, 0);
  Eigen::Vector3d v_rotated = rotation_vector * v;
  std::cout << "(1, 0, 0) after rotation = " << v_rotated.transpose() << std::endl << std::endl;

  v_rotated = rotation_matrix * v;
  std::cout << "(1, 0, 0) after rotation = " << v_rotated.transpose() << std::endl << std::endl;

  Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);
  std::cout << "yaw pitch roll = " << euler_angles.transpose() << std::endl << std::endl;

  Eigen::Isometry3d t = Eigen::Isometry3d::Identity();
  t.rotate(rotation_vector);
  t.pretranslate(Eigen::Vector3d(1, 3, 4));
  std::cout << "Transform matrix = \n" << t.matrix() << std::endl << std::endl;

  Eigen::Vector3d v_transformed = t * v;
  std::cout << "v transformed = " << v_transformed.transpose() << std::endl << std::endl;

  Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
  std::cout << "quaternion = \n" << q.coeffs() << std::endl << std::endl;

  q = Eigen::Quaterniond(rotation_matrix);
  std::cout << "quaternion = \n" << q.coeffs() << std::endl << std::endl;

  v_rotated = q * v;
  std::cout << "(1, 0, 0) after rotation = " << v_rotated.transpose() << std::endl << std::endl;

  return 0;
}