/*******************************************************************************
* Copyright 2016 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

/* Authors: Hye-Jong KIM */

#ifndef OMMATH_HPP_
#define OMMATH_HPP_

#include "../../include/open_manipulator/OMDebug.hpp"

#include <Eigen.h>        // Calls main Eigen matrix class library
#include <Eigen/LU>       // Calls inverse, determinant, LU decomp., etc.

#include <math.h>

#define DEG2RAD (M_PI / 180.0)
#define RAD2DEG (180.0 / M_PI)

class OMMath
{
 public:
  OMMath(){}
  ~OMMath(){}

  float sign(float number)
  {
    if (num >= 0.0)
    {
      return 1.0;
    }
    else
    {
      return -1.0;
    }
  }

  Eigen::Vector3f makeEigenVector3(float v1, float v2, float v3)
  {
    Eigen::Vector3f temp;
    temp << v1, v2, v3;
    return temp;
  }

  Eigen::Matrix3f makeEigenMatrix3(float m11, float m12, float m13, float m21, float m22, float m23, float m31, float m32, float m33)
  {
    Eigen::Matrix3f temp;
    temp << m11, m12, m13, m21, m22, m23, m31, m32, m33;
    return temp;
  }

  Eigen::Matrix3f makeRotationMatrix(float roll, float pitch, float yaw)
  {
    Eigen::Matrix3f rotation_matrix;
    Eigen::Matrix3f roll_matrix;
    Eigen::Matrix3f pitch_matrix;
    Eigen::Matrix3f yaw_matrix;

    roll_matrix  << 1.000,  0.000,     0.000,
                    0.000,  cos(roll), sin(roll),
                    0.000, -sin(roll), cos(roll);

    pitch_matrix << cos(pitch),  0.000, sin(pitch),
                    0.000,       1.000, 0.000,
                    -sin(pitch), 0.000, cos(pitch);

    yaw_matrix   << cos(yaw), -sin(yaw), 0.000,
                    sin(yaw), cos(yaw),  0.000,
                    0.000,    0.000,     1.000;

    rotation_matrix = roll_matrix * pitch_matrix * yaw_matrix;

    return rotation_matrix;
  }

  Eigen::Matrix3f makeRotationMatrix(Eigen::Vector3f rotation_vector)
  {
    Eigen::Matrix3f rotation_matrix;
    rotation_matrix = makeRotationMatrix(rotation_vector(0), rotation_vector(1), rotation_vector(2));
    return rotation_matrix;
  }

  Eigen::Vector3f makeRotationVector(Eigen::Matrix3f rotation_matrix)
  {
    Eigen::Matrix3f R = rotation_matrix;
    Eigen::Vector3f l = Eigen::Vector3f::Zero();
    Eigen::Vector3f rotation_vector = Eigen::Vector3f::Zero();

    float theta = 0.0;
    float diag  = 0.0;
    bool diagonal_matrix = true;

    l << R(2,1) - R(1,2),
         R(0,2) - R(2,0),
         R(1,0) - R(0,1);
    theta = atan2(l.norm(), R(0,0) + R(1,1) + R(2,2) - 1);
    diag  = R(0,0) + R(1,1) + R(2,2);

    for (int8_t i = 0; i < 3; i++)
    {
      for (int8_t j = 0; j < 3; j++)
      {
        if (i != j)
        {
          if (R(i, j) != 0)
          {
            diagonal_matrix = false;
          }
        }
      }
    }
    if (R == Eigen::Matrix3f::Identity())
    {
      rotation_vector = Eigen::Vector3f::Zero();
    }
    else if (diagonal_matrix == true)
    {
      rotation_vector << R(0,0) + 1, R(1,1) + 1, R(2,2) + 1;
      rotation_vector = rotation_vector * M_PI_2;
    }
    else
    {
      rotation_vector = theta * (l / l.norm());
    }
    return rotation_vector;
  }

  Eigen::Matrix3f skewSymmetricMatrix(Eigen::Vector3f v)
  {
    Eigen::Matrix3f skew_symmetric_matrix = Eigen::Matrix3f::Zero();

    skew_symmetric_matrix <<    0,  -v(2),  v(1),
                            v(2),      0, -v(0),
                            -v(1),   v(0),     0;

    return skew_symmetric_matrix;
  }

  Eigen::Matrix3f rodriguesRotationMatrix(Eigen::Vector3f axis, float angle)
  {
    Eigen::Matrix3f skew_symmetric_matrix = Eigen::Matrix3f::Zero();
    Eigen::Matrix3f rotation_matrix = Eigen::Matrix3f::Zero();
    Eigen::Matrix3f Identity_matrix = Eigen::Matrix3f::Identity();

    skew_symmetric_matrix = skewSymmetricMatrix(axis);
    rotation_matrix = Identity_matrix +
                      skew_symmetric_matrix * sin(angle) +
                      skew_symmetric_matrix * skew_symmetric_matrix * (1 - cos(angle));

    return rotation_matrix;
  }

  Eigen::Vector3f differentialPosition(Eigen::Vector3f desired_position, Eigen::Vector3f present_position)
  {
    Eigen::Vector3f differential_position;
    differential_position = desired_position - present_position;

    return differential_position;
  }

  Eigen::Vector3f differentialOrientation(Eigen::Matrix3f desired_orientation, Eigen::Matrix3f present_orientation)
  {
    Eigen::Vector3f differential_orientation;
    differential_orientation = present_orientation * makeRotationVector(present_orientation.transpose() * desired_orientation);

    return differential_orientation;
  }

  Eigen::VectorXf differentialPose(Eigen::Vector3f desired_position, Eigen::Vector3f present_position, Eigen::Matrix3f desired_orientation, Eigen::Matrix3f present_orientation)
  {
    Eigen::Vector3f differential_position;
    Eigen::Vector3f differential_orientation;
    Eigen::VectorXf  differential_pose(6);

    differential_position = differentialPosition(desired_position, present_position);
    differential_orientation = differentialOrientation(desired_orientation, present_orientation)
    differential_pose << differential_position(0),
                         differential_position(1),
                         differential_position(2),
                         differential_orientation(0),
                         differential_orientation(1),
                         differential_orientation(2);
    return differential_pose;
  }
};

#endif // OMMATH_HPP_


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file linear_algebra.hpp
 * \date January 2015
 * \author Jan Issac (jan.issac@gmail.com)
 * \author Manuel Wuthrich (manuel.wuthrich@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <cmath>
#include <vector>

namespace fl
{
/**
 * \ingroup linear_algebra
 *
 * \brief Blockweise matrix inversion using the Sherman-Morrision-Woodbury
 * indentity given that \f$\Sigma^{-1}_{aa}\f$ of
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1}
 * \f$
 *
 * is already available.
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1} =
 * \begin{pmatrix} \Lambda_{aa} & \Lambda_{ab} \\
 *                 \Lambda_{ba} & \Lambda_{bb} \end{pmatrix} = \Lambda
 * \f$
 *
 * \param [in]  A_inv   \f$ \Sigma^{-1}_{aa} \f$
 * \param [in]  B       \f$ \Sigma_{ab} \f$
 * \param [in]  C       \f$ \Sigma_{ba} \f$
 * \param [in]  D       \f$ \Sigma_{bb} \f$
 * \param [out] L_A     \f$ \Lambda_{aa} \f$
 * \param [out] L_B     \f$ \Lambda_{ab} \f$
 * \param [out] L_C     \f$ \Lambda_{ba} \f$
 * \param [out] L_D     \f$ \Lambda_{bb} \f$
 */
template <typename MatrixAInv,
          typename MatrixB,
          typename MatrixC,
          typename MatrixD,
          typename MatrixLA,
          typename MatrixLB,
          typename MatrixLC,
          typename MatrixLD>
inline void smw_inverse(const MatrixAInv& A_inv,
                        const MatrixB& B,
                        const MatrixC& C,
                        const MatrixD& D,
                        MatrixLA& L_A,
                        MatrixLB& L_B,
                        MatrixLC& L_C,
                        MatrixLD& L_D)
{
    auto&& AinvB = (A_inv * B).eval();

    L_D = (D - C * AinvB).inverse();
    L_C = -(L_D * C * A_inv).eval();
    L_A = A_inv - AinvB * L_C;
    L_B = L_C.transpose();
}

/**
 * \ingroup linear_algebra
 *
 * \brief Blockweise matrix inversion using the Sherman-Morrision-Woodbury
 * indentity given that \f$\Sigma^{-1}_{aa}\f$ of
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1}
 * \f$
 *
 * is already available.
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1} =
 * \begin{pmatrix} \Lambda_{aa} & \Lambda_{ab} \\
 *                 \Lambda_{ba} & \Lambda_{bb} \end{pmatrix} = \Lambda
 * \f$
 *
 * \param [in]  A_inv   \f$ \Sigma^{-1}_{aa} \f$
 * \param [in]  B       \f$ \Sigma_{ab} \f$
 * \param [in]  C       \f$ \Sigma_{ba} \f$
 * \param [in]  D       \f$ \Sigma_{bb} \f$
 * \param [out] L_A     \f$ \Lambda_{aa} \f$
 * \param [out] L_B     \f$ \Lambda_{ab} \f$
 * \param [out] L_C     \f$ \Lambda_{ba} \f$
 * \param [out] L_D     \f$ \Lambda_{bb} \f$
 * \param [out] L       \f$ \Lambda \f$
 */
template <typename MatrixAInv,
          typename MatrixB,
          typename MatrixC,
          typename MatrixD,
          typename MatrixLA,
          typename MatrixLB,
          typename MatrixLC,
          typename MatrixLD,
          typename ResultMatrix>
inline void smw_inverse(const MatrixAInv& A_inv,
                        const MatrixB& B,
                        const MatrixC& C,
                        const MatrixD& D,
                        MatrixLA& L_A,
                        MatrixLB& L_B,
                        MatrixLC& L_C,
                        MatrixLD& L_D,
                        ResultMatrix& L)
{
    smw_inverse(A_inv, B, C, D, L_A, L_B, L_C, L_D);

    L.resize(L_A.rows() + L_C.rows(), L_A.cols() + L_B.cols());

    L.block(0, 0, L_A.rows(), L_A.cols()) = L_A;
    L.block(0, L_A.cols(), L_B.rows(), L_B.cols()) = L_B;
    L.block(L_A.rows(), 0, L_C.rows(), L_C.cols()) = L_C;
    L.block(L_A.rows(), L_A.cols(), L_D.rows(), L_D.cols()) = L_D;
}

/**
 * \ingroup linear_algebra
 *
 * \brief Blockweise matrix inversion using the Sherman-Morrision-Woodbury
 * indentity given that \f$\Sigma^{-1}_{aa}\f$ of
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1}
 * \f$
 *
 * is already available.
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1} =
 * \begin{pmatrix} \Lambda_{aa} & \Lambda_{ab} \\
 *                 \Lambda_{ba} & \Lambda_{bb} \end{pmatrix} = \Lambda
 * \f$
 *
 * \param [in]  A_inv   \f$ \Sigma^{-1}_{aa} \f$
 * \param [in]  B       \f$ \Sigma_{ab} \f$
 * \param [in]  C       \f$ \Sigma_{ba} \f$
 * \param [in]  D       \f$ \Sigma_{bb} \f$
 * \param [out] L       \f$ \Lambda \f$
 */
template <typename MatrixAInv,
          typename MatrixB,
          typename MatrixC,
          typename MatrixD,
          typename ResultMatrix>
inline void smw_inverse(const MatrixAInv& A_inv,
                        const MatrixB& B,
                        const MatrixC& C,
                        const MatrixD& D,
                        ResultMatrix& L)
{
    MatrixAInv L_A;
    MatrixB L_B;
    MatrixC L_C;
    MatrixD L_D;

    smw_inverse(A_inv, B, C, D, L_A, L_B, L_C, L_D, L);
}

///**
// * \ingroup linear_algebra
// *
// * \brief Normalizes the values of input vector such that their sum is equal
// to
// * the specified \c sum. For instance, any convex combination requires that
// the
// * weights of the weighted sum sums up to 1.
// */
// template <typename T>
// inline std::vector<T> normalize(const std::vector<T>& input, T sum)
//{
//    T old_sum = 0;
//    for(size_t i = 0; i < input.size(); i++)
//    {
//        old_sum += input[i];
//    }
//    T factor = sum/old_sum;

//    std::vector<T> output(input.size());
//    for(size_t i = 0; i < input.size(); i++)
//    {
//        output[i] = factor*input[i];
//    }

//    return output;
//}

/**
 * \ingroup linear_algebra
 * \brief Constructs the quaternion matrix for the specified quaternion vetcor
 *
 * \param q_xyzw  Quaternion vector
 *
 * \return Matrix representation of the quaternion vector
 */
inline Eigen::Matrix<double, 4, 3> quaternion_matrix(
    const Eigen::Matrix<double, 4, 1>& q_xyzw)
{
    Eigen::Matrix<double, 4, 3> Q;
    Q << q_xyzw(3), q_xyzw(2), -q_xyzw(1), -q_xyzw(2), q_xyzw(3), q_xyzw(0),
        q_xyzw(1), -q_xyzw(0), q_xyzw(3), -q_xyzw(0), -q_xyzw(1), -q_xyzw(2);

    return 0.5 * Q;
}

/**
 * \ingroup linear_algebra
 * \todo DOC
 * \todo Implement numerically robust version and test it
 */
template <typename RegularMatrix, typename SquareRootMatrix>
void square_root(const RegularMatrix& regular_matrix,
                 SquareRootMatrix& square_root)
{
    square_root = regular_matrix.llt().matrixL();

//    typedef Eigen::Matrix<typename RegularMatrix::Scalar,
//                          RegularMatrix::SizeAtCompileTime,
//                          1> Vector;

//    Eigen::LDLT<RegularMatrix> ldlt;
//    ldlt.compute(regular_matrix);
//    Vector D_sqrt = ldlt.vectorD();
//    for (int i = 0; i < D_sqrt.rows(); ++i)
//    {
//        D_sqrt(i) = std::sqrt(std::fabs(D_sqrt(i)));
//    }

//    return ldlt.transpositionsP().transpose() * (Matrix)ldlt.matrixL() *
//           D_sqrt.asDiagonal();
}

/**
 * \ingroup linear_algebra
 * \todo DOC
 */
template <typename RegularMatrix>
void sqrt_diagonal(const RegularMatrix& regular_matrix,
                   RegularMatrix& square_root)
{
    square_root = regular_matrix;
    for (size_t i = 0; i < square_root.rows(); ++i)
    {
        square_root(i, i) = std::sqrt(square_root(i, i));
    }
}

/**
 * \ingroup linear_algebra
 * \brief Computes the square root of each element of the regular_matrix
 */
template <typename RegularMatrix, typename SquareRootVector>
void sqrt_diagonal_vector(const RegularMatrix& diagonal_vector,
                          SquareRootVector& square_root)
{
    square_root = diagonal_vector;
    for (size_t i = 0; i < square_root.rows(); ++i)
    {
        square_root(i, 0) = std::sqrt(square_root(i, 0));
    }
}

/**
 * \ingroup linear_algebra
 * \brief Inverts a diagonal matrix represented by a vector. The result is again
 *        a vector of the inverted diagonal elements.
 */
template <typename SrcDiagonalMatrix, typename DestDiagonalMatrix>
void invert_diagonal_vector(const SrcDiagonalMatrix& diagonal,
                            DestDiagonalMatrix& diagonal_inverse)
{
    diagonal_inverse = diagonal.array().cwiseInverse();

    //    diagonal_inverse.resize(diagonal.size());
    //    for (int i = 0; i < diagonal.size(); ++i)
    //    {
    //        diagonal_inverse(i) = 1./diagonal(i);
    //    }
}

/**
 * \ingroup linear_algebra
 *
 * \brief Computes the Frobenius norm of a given real matrix.
 *
 * The Frobenius norm is given by
 *
 * \f$
 *   \| A \|_F := \sqrt{\sum\limits_{i=1}^m \sum\limits_{j=1}^n |a_{ij} |^2 }
 * \f$
 */
template <typename Derived>
double frobenius_norm(const Eigen::MatrixBase<Derived>& a)
{
    return std::sqrt(a.array().abs().square().sum());
}

/**
 * \ingroup linear_algebra
 * \brief Checks whether two matrices of the same size are similiar w.r.t. some
 * \f$\epsilon\f$.
 *
 * The similarity check is performed based on the Frobenius Norm
 * (\ref frobenius_norm):
 *
 * For any real matrix set \f$M \subseteq \mathbb{R}^{m\times n} \f$
 *
 * \f$ \text{are_similar}: (M \times M) \mapsto \{0, 1\}\f$
 *
 * \f$ \text{are_similar}(a, b)
 *     := \mathbf{1}_{[0, \epsilon)}\left(\| a - b \|_F\right)\f$
 * with \f$ a, b \in M\f$ and the indicator function
 *
 * \f$ \mathbf{1}_{[0, \epsilon)}(x) :=
 *     \begin{cases} 1 & x \in [0, \epsilon)
 *                  \\ 0, & \text{otherwise} \end{cases}\f$
 */
// template <typename DerivedA, typename DerivedB>
// bool are_similar(const Eigen::MatrixBase<DerivedA>& a,
//                 const Eigen::MatrixBase<DerivedB>& b,
//                 const double epsilon = 1.e-6)
//{
//    return frobenius_norm(a - b) < epsilon;
//}

template <typename DerivedA, typename DerivedB>
bool are_similar(const Eigen::MatrixBase<DerivedA>& a,
                 const Eigen::MatrixBase<DerivedB>& b,
                 const double epsilon = 1.e-6)
{
    // return (a - b).cwiseAbs().maxCoeff() < epsilon;
    return frobenius_norm(a - b) < epsilon;
}

/**
 * \ingroup linear_algebra
 * \brief Robust decomposition of M = L*L^T for positive semidefinite matrices
 */
template <typename Scalar, int Size>
Eigen::Matrix<Scalar, Size, Size> matrix_sqrt(
    Eigen::Matrix<Scalar, Size, Size> M)
{
    typedef Eigen::Matrix<Scalar, Size, Size> Matrix;
    typedef Eigen::Matrix<Scalar, Size, 1> Vector;

    Eigen::LDLT<Matrix> ldlt;
    ldlt.compute(M);
    Vector D_sqrt = ldlt.vectorD();
    for (int i = 0; i < D_sqrt.rows(); ++i)
    {
        D_sqrt(i) = std::sqrt(std::fabs(D_sqrt(i)));
    }

    return ldlt.transpositionsP().transpose() * (Matrix)ldlt.matrixL() *
           D_sqrt.asDiagonal();
}

/**
 * \ingroup linear_algebra
 * \brief Checks wheather the specified dense matrix has a diagonal form
 *
 * \return true if diagonal, false otherwise
 */
template <typename Derived>
bool is_diagonal(const Eigen::MatrixBase<Derived>& m)
{
    Derived z = m;
    z -= z.diagonal().asDiagonal();

    return z.isZero();
}

/**
 * \ingroup linear_algebra
 * \brief Robust AX=B solver where X is a vector or a matrix
 */
template <typename MatrixA, typename VectorsB>
VectorsB solve(
    const Eigen::MatrixBase<MatrixA>& A,
    const VectorsB& B,
    typename std::enable_if<VectorsB::RowsAtCompileTime == 1>::type* = 0)
{
    // householderQr() was not providing always a solution
    // VectorsB x = A.householderQr().solve(B).eval();

    assert(A.cols() == B.rows());
    assert(A.cols() == 1);
    assert(A.rows() == 1);

    VectorsB x = B / A(0);
    return x;  // RVO
}

/**
 * \ingroup linear_algebra
 * \brief Robust AX=B solver where X is a vector or a matrix
 */
template <typename MatrixA, typename VectorsB>
VectorsB solve(
    const Eigen::MatrixBase<MatrixA>& A,
    const VectorsB& B,
    typename std::enable_if<(VectorsB::RowsAtCompileTime != 1)>::type* = 0)
{
    // householderQr() was not providing always a solution
    // VectorsB x = A.householderQr().solve(B).eval();

    assert(A.cols() == B.rows());

    VectorsB x = A.colPivHouseholderQr().solve(B).eval();
    return x;  // RVO
}
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef NL_PROBLEM_PENDULUM_HPP_
#define NL_PROBLEM_PENDULUM_HPP_

#include <cbr_math/math.hpp>

#include <Eigen/Dense>
#include <cstdint>
#include <functional>
#include <limits>

template<typename T1, typename T2>
auto pendulum_dynamics(const Eigen::MatrixBase<T1> & x, const Eigen::MatrixBase<T2> & u)
{
  // Define nx, nu
  constexpr auto nx = T1::RowsAtCompileTime;
  constexpr auto nu = T2::RowsAtCompileTime;

  // Initialize Parameters
  constexpr double l = 1.;
  constexpr double m = 1.;
  constexpr double b = 0.2;
  constexpr double gr = 9.81;

  using T = typename decltype(x * u.transpose())::EvalReturnType::Scalar;

  // xDot = f(x) + g(x)*u
  Eigen::Matrix<typename T1::Scalar, nx, 1> f;
  Eigen::Matrix<typename T1::Scalar, nx, nu> g;

  // Define States
  const auto & theta = x[0];
  const auto & thetaDot = x[1];

  // Dynamics
  f[0] = thetaDot;
  f[1] = -(gr / l) * sin(theta) - b / (m * l * l) * thetaDot;
  g[0] = 0.;
  g[1] = 1 / (m * l * l);

  return (f + g * u).eval();
}

struct NlOcpPendulum
{
  constexpr static std::size_t nx = 2;
  constexpr static std::size_t nu = 1;
  constexpr static bool xl_always_feasible = false;

  using state_t = Eigen::Matrix<double, nx, 1>;
  using deriv_t = Eigen::Matrix<double, nx, 1>;
  using input_t = Eigen::Matrix<double, nu, 1>;
  using Q_t = Eigen::Matrix<double, nx, nx>;
  using R_t = Eigen::Matrix<double, nu, nu>;

  const Q_t Q = (state_t() << 20.0, 1.0).finished().asDiagonal();
  const Q_t QT = (state_t() << 20.0, 1.0).finished().asDiagonal();
  const R_t R = (R_t() << 0.1).finished();

  template<typename T1, typename T2>
  auto get_f(const Eigen::MatrixBase<T1> & x, const Eigen::MatrixBase<T2> & u) const
  {
    static_assert(
      T1::RowsAtCompileTime == nx &&
      T1::ColsAtCompileTime == 1,
      "First argument must be an nx*1 Eigen Matrix");

    static_assert(
      T2::RowsAtCompileTime == nu &&
      T2::ColsAtCompileTime == 1,
      "Second argument must be an nu*1 Eigen Matrix");

    return pendulum_dynamics<T1, T2>(x, u);
  }

  void get_state_lb(double, Eigen::Ref<state_t> state_lb) const
  {
    const double kInfinity = std::numeric_limits<double>::infinity();
    state_lb <<
      -kInfinity,
      -cbr::deg2rad(200.0);
  }

  void get_state_ub(double, Eigen::Ref<state_t> state_ub) const
  {
    const double kInfinity = std::numeric_limits<double>::infinity();
    state_ub <<
      kInfinity,
      cbr::deg2rad(200.0);
  }

  void get_input_lb(double, Eigen::Ref<input_t> input_lb) const
  {
    input_lb << -0.1;
  }

  void get_input_ub(double, Eigen::Ref<input_t> input_ub) const
  {
    input_ub << 0.1;
  }

  const Q_t & get_Q(double) const {return Q;}
  const R_t & get_R(double) const {return R;}
  const Q_t & get_QT() const {return QT;}
};

#endif  // NL_PROBLEM2_HPP_


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/position_from_two_rays.h"

#include <Eigen/Core>
#include <Eigen/QR>
#include <glog/logging.h>
#include <vector>

namespace theia {

// We use the reprojection error constraint:
//
//   rotated_feature = [x - cx; y - cy] / (z - cz)
//
// where the 3D point is [x y z]^t and the unknown position is [cx cy cz].
// This constraint can be rearranged into a linear system:
//
//   [1  0  -u] * c = [x - u * z]
//   [0  1  -v]     = [y - v * z]
//
// where rotated_feature = [u v]. By stacking this constraint for both features
// we obtain a 4x3 linear system whose solution is the camera position.
bool PositionFromTwoRays(const Eigen::Vector2d& rotated_feature1,
                         const Eigen::Vector3d& point1,
                         const Eigen::Vector2d& rotated_feature2,
                         const Eigen::Vector3d& point2,
                         Eigen::Vector3d* position) {
  CHECK_NOTNULL(position);
  // Create the left hand side of the linear system above.
  Eigen::Matrix<double, 4, 3> lhs;
  lhs.block<2, 2>(0, 0).setIdentity();
  lhs.block<2, 2>(2, 0).setIdentity();
  lhs.block<2, 1>(0, 2) = -rotated_feature1;
  lhs.block<2, 1>(2, 2) = -rotated_feature2;

  // Create the right hand side of the linear system above.
  Eigen::Vector4d rhs;
  rhs.head<2>() = point1.head<2>() - rotated_feature1 * point1.z();
  rhs.tail<2>() = point2.head<2>() - rotated_feature2 * point2.z();

  Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 4, 3> > linear_solver(lhs);
  // If the linear solver is not well-conditioned then the position cannot be
  // reliably computed.
  if (linear_solver.rank() != 3) {
    return false;
  }

  *position = linear_solver.solve(rhs);
  return true;
}

}  // namespace theia


/*
 * Copyright 2009-2012 Karsten Ahnert
 * Copyright 2009-2012 Mario Mulansky
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/ublas/vector.hpp>

/* define ublas::vector<double> as resizeable
 * this is not neccessarily required because this definition already
 * exists in util/ublas_wrapper.hpp.
 * However, for completeness and educational purpose it is repeated here.
 */

//[ ublas_resizeable
typedef boost::numeric::ublas::vector< double > state_type;

namespace boost { namespace numeric { namespace odeint {

template<>
struct is_resizeable< state_type >
{
    typedef boost::true_type type;
    const static bool value = type::value;
};

} } }
//]

void lorenz( const state_type &x , state_type &dxdt , const double t )
{
    const double sigma( 10.0 );
    const double R( 28.0 );
    const double b( 8.0 / 3.0 );

    dxdt[0] = sigma * ( x[1] - x[0] );
    dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
    dxdt[2] = -b * x[2] + x[0] * x[1];
}

using namespace boost::numeric::odeint;

//[ublas_main
int main()
{
    state_type x(3);
    x[0] = 10.0; x[1] = 5.0 ; x[2] = 0.0;
    typedef runge_kutta4< state_type , double , state_type , double , vector_space_algebra > stepper;
    integrate_const( stepper() , lorenz , x ,
                     0.0 , 10.0 , 0.1 );
}
//]


#include <Eigen/Core>
#include <iostream>
#include <LBFGS.h>

using Eigen::VectorXd;
using Eigen::MatrixXd;
using namespace LBFGSpp;

class Rosenbrock
{
private:
    int n;
    ptrdiff_t ncalls;

public:
    Rosenbrock(int n_) : n(n_), ncalls(0) {}
    double operator()(const VectorXd& x, VectorXd& grad)
    {
//        std::cout << x << std::endl;
        ncalls += 1;

        double fx = 0.0;
        for(int i = 0; i < n; i += 2)
        {
            double t1 = 1.0 - x[i];
            double t2 = 10 * (x[i + 1] - x[i] * x[i]);
            grad[i + 1] = 20 * t2;
            grad[i]     = -2.0 * (x[i] * grad[i + 1] + t1);
            fx += t1 * t1 + t2 * t2;
        }
        assert( ! std::isnan(fx) );
        return fx;
    }

    const ptrdiff_t get_ncalls() {
      return ncalls;
    }
};

int main()
{
    LBFGSParam<double> param;
    param.    linesearch = LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE;
    param.max_linesearch = 128;

    LBFGSSolver<double, LineSearchBacktracking > solver_backtrack(param);
    LBFGSSolver<double, LineSearchBracketing   > solver_bracket  (param);
    LBFGSSolver<double, LineSearchNocedalWright> solver_nocedal  (param);

    const int tests_per_n = 1024;

    for( int n=2; n <= 24; n += 2 )
    {
        std::cout << "n = " << n << std::endl;
        Rosenbrock fun_backtrack(n),
                   fun_bracket  (n),
                   fun_nocedal  (n);
        int niter_backtrack = 0,
            niter_bracket   = 0,
            niter_nocedal   = 0;
        for( int test=0; test < tests_per_n; test++ )
        {
            VectorXd x, x0 = VectorXd::Random(n);

            double fx;

            x = x0; niter_backtrack += solver_backtrack.minimize(fun_backtrack, x, fx); assert( ( (x.array() - 1.0).abs() < 1e-4 ).all() );
            x = x0; niter_bracket   += solver_bracket  .minimize(fun_bracket  , x, fx); assert( ( (x.array() - 1.0).abs() < 1e-4 ).all() );
            x = x0; niter_nocedal   += solver_nocedal  .minimize(fun_nocedal  , x, fx); assert( ( (x.array() - 1.0).abs() < 1e-4 ).all() );
        }
        std::cout << "  Average #calls:" << std::endl;
        std::cout << "  LineSearchBacktracking : " << (fun_backtrack.get_ncalls() / tests_per_n) << " calls, " << (niter_backtrack / tests_per_n) << " iterations" << std::endl;
        std::cout << "  LineSearchBracketing   : " << (fun_bracket  .get_ncalls() / tests_per_n) << " calls, " << (niter_bracket   / tests_per_n) << " iterations" << std::endl;
        std::cout << "  LineSearchNocedalWright: " << (fun_nocedal  .get_ncalls() / tests_per_n) << " calls, " << (niter_nocedal   / tests_per_n) << " iterations" << std::endl;
    }

    return 0;
}


#include "derivatives.h"
#include <boost/math/tools/numerical_differentiation.hpp>
#include <nlopt.h>
#include <math.h>

using namespace arma;

double optim::compute_first_derivative(std::function<double(const double&)> f, const double & x)
{
	double error_estimate;
	return boost::math::tools::finite_difference_derivative<decltype(f),double,6>(f, x,&error_estimate);
}

vec optim::compute_gradient(ObjectiveFunction feval, const vec & x)
{
	vec g(x.size());
	for (arma::uword i = 0; i < g.size(); ++i) {
		auto f_i = [&](const double& s) {
			vec v = x;
			v[i] = s;
			return feval(v);
		};

		g[i] = compute_first_derivative(f_i, x[i]);
	}
	return g;
}

mat optim::compute_hessian(ObjectiveFunction feval, const vec & x)
{
	{
		using namespace std::placeholders;

		int N = x.size();
		mat H(N, N);
		auto fij = [&x,&feval](double i, double j, double xi, double xj) {
			vec v = x;
			double di = xi - x[i];
			double dj = xj - x[j];
			v[i] += di;
			v[j] += dj;
			return feval(v);
		};

		for (int i = 0; i < N; ++i) {

			for (int j = i; j < N; ++j) {

				auto di = [fij, i, j,x](const double xj) {return compute_first_derivative(std::bind(fij, i, j, _1, xj),x[i]); };
				auto dij = compute_first_derivative(di, x[j]);
				H(i, j) = dij;
				if (i != j) {
					H(j, i) = H(i, j);
				}
			}
		}
		return H;

	}
}

mat optim::compute_jacobian(ObjectiveVectorFunction feval, const vec & x)
{
	int N = x.size();
	mat J(N, N);
	for (int i = 0; i < N; ++i) {
		auto g = compute_gradient([&feval, i](decltype(x) v) {return feval(v)[i]; },x);
		for (int j = 0; j < N; ++j) {
			J(i, j) = g[j];
		}
	}
	return J;

}


//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2009
//
//============================================================================

#ifndef __Thea_Algorithms_LinearLeastSquares2_hpp__
#define __Thea_Algorithms_LinearLeastSquares2_hpp__

#include "../Common.hpp"
#include "../Array.hpp"
#include "../Line2.hpp"
#include "../MatVec.hpp"
#include "CentroidN.hpp"
#include "Iterators.hpp"
#include "PointTraitsN.hpp"
#include <Eigen/Eigenvalues>

namespace Thea {
namespace Algorithms {

/** Fitting linear models to 2D data by minimizing sum-of-squared-errors. */
template <typename T, typename Enable = void>
class /* THEA_API */ LinearLeastSquares2
{
  public:
    /**
     * Linear least-squares fitting of a line to a set of 2D objects. InputIterator must dereference to type T or pointer-to-T.
     *
     * @param begin The first object in the set.
     * @param end One position beyond the last object in the set.
     * @param line Used to return the best-fit line.
     * @param centroid If non-null, used to return the centroid of the objects, which is computed in the process of finding the
     *   best-fit line.
     *
     * @return The sum of squared fitting errors.
     */
    template <typename InputIterator>
    static double fitLine(InputIterator begin, InputIterator end, Line2 & line, Vector2 * centroid = nullptr);

}; // class LinearLeastSquares2

// Fitting lines to sets of objects that map to single points in 2-space.
template <typename T>
class /* THEA_API */ LinearLeastSquares2<T, typename std::enable_if< IsNonReferencedPointN<T, 2>::value >::type>
{
  public:
    template <typename InputIterator>
    static double fitLine(InputIterator begin, InputIterator end, Line2 & line, Vector2 * centroid = nullptr)
    {
      Vector2d center = CentroidN<T, 2>::compute(begin, end);
      Matrix2d m = Matrix2d::Zero();
      for (auto iter = makeRefIterator(begin); iter != makeRefIterator(end); ++iter)
      {
        Vector2d diff = Vector2d(PointTraitsN<T, 2>::getPosition(*iter)) - center;

        m(0, 0) += (diff.y() * diff.y());
        m(0, 1) -= (diff.x() * diff.y());
        m(1, 1) += (diff.x() * diff.x());
      }

      m(1, 0) = m(0, 1);

      Eigen::SelfAdjointEigenSolver eigensolver;
      if (eigensolver.computeDirect(m).info() != Eigen::Success)
        throw Error("LinearLeastSquares2: Could not eigensolve covariance matrix");

      // Eigenvalues are non-negative (covariance matrix is non-negative definite) and in increasing order
      line = Line2::fromPointAndDirection(Vector2(center), Vector2(eigensolver.eigenvectors().col(0)));
      if (centroid) *centroid = center;
      return eigensolver.eigenvalues()[0];
    }

}; // class LinearLeastSquares2<Point2>

} // namespace Algorithms
} // namespace Thea

#endif


//
//  holo_gain.cpp
//  autd3
//
//  Created by Seki Inoue on 7/6/16.
//
//

#include <iostream>
#include <string>
#include <map>
#include <complex>
#include <vector>
#include <boost/assert.hpp>
#include "privdef.hpp"
#include "autd3.hpp"
#include <boost/random.hpp>
#include <Eigen/Eigen>
#define _USE_MATH_DEFINES
#include <math.h>

#define REPEAT_SDP (10)
#define LAMBDA_SDP (0.8)

namespace hologainimpl {
    // TODO: use cache and interpolate. don't use exp
    std::complex<float> transfer(Eigen::Vector3f trans_pos, Eigen::Vector3f trans_norm, Eigen::Vector3f target_pos) {
        Eigen::Vector3f diff = trans_pos-target_pos;
        float dist = diff.norm();
        float cos = diff.dot(trans_norm)/dist/trans_norm.norm();

        //1.11   & 1.06  &  0.24 &  -0.12  &  0.035
        float directivity = 1.11 *sqrt((2*0+1)/(4*M_PI))*1 + 1.06*sqrt((2*1+1)/(4*M_PI))*cos + 0.24*sqrt((2*2+1)/(4*M_PI))/2*(3*cos*cos-1)  -0.12*sqrt((2*3+1)/(4*M_PI))/2*cos*(5*cos*cos-3) + 0.035*sqrt((2*4+1)/(4*M_PI))/8*(35*cos*cos*cos*cos-30*cos*cos+3);

        std::complex<float> g = directivity/dist*exp(std::complex<float>(-dist*1.15e-4, -2*M_PI/ULTRASOUND_WAVELENGTH*dist));
        return g;
    }

    void removeRow(Eigen::MatrixXcf& matrix, unsigned int rowToRemove)
    {
        unsigned int numRows = matrix.rows()-1;
        unsigned int numCols = matrix.cols();

        if( rowToRemove < numRows )
            matrix.block(rowToRemove,0,numRows-rowToRemove,numCols) = matrix.block(rowToRemove+1,0,numRows-rowToRemove,numCols);

        matrix.conservativeResize(numRows,numCols);
    }

    void removeColumn(Eigen::MatrixXcf& matrix, unsigned int colToRemove)
    {
        unsigned int numRows = matrix.rows();
        unsigned int numCols = matrix.cols()-1;

        if( colToRemove < numCols )
            matrix.block(0,colToRemove,numRows,numCols-colToRemove) = matrix.block(0,colToRemove+1,numRows,numCols-colToRemove);

        matrix.conservativeResize(numRows,numCols);
    }
}

autd::GainPtr autd::HoloGainSdp::Create(Eigen::MatrixX3f foci, Eigen::VectorXf amp) {
    std::shared_ptr<HoloGainSdp> ptr = std::shared_ptr<HoloGainSdp>(new HoloGainSdp());
    ptr->_foci = foci;
    ptr->_amp = amp;
    return ptr;
}

void autd::HoloGainSdp::build() {
    if (this->built()) return;
    if (this->geometry() == nullptr) BOOST_ASSERT_MSG(false, "Geometry is required to build Gain");

    //const double pinvtoler=1.e-7;
    const float alpha = 1e-3;

    const int M = (int)_foci.rows();
    const int N = (int)this->geometry()->numTransducers();

    Eigen::MatrixXcf P = Eigen::MatrixXcf::Zero(M, M);
    Eigen::VectorXcf p = Eigen::VectorXcf::Zero(M);
    Eigen::MatrixXcf B = Eigen::MatrixXcf(M,N);
    Eigen::VectorXcf q = Eigen::VectorXcf(N);

    boost::random::mt19937 rng( static_cast<unsigned int>(time(0)) );
    boost::random::uniform_real_distribution<> range(0,1);
    boost::random::variate_generator< boost::random::mt19937, boost::random::uniform_real_distribution<> > mt(rng, range);

    for (int i = 0; i < M; i++) {
        p(i) = _amp(i)*exp(std::complex<float>(0,2*M_PI*mt()));
        P(i,i) = _amp(i);

        Eigen::Vector3f q = _foci.row(i);
        for (int j = 0; j < N; j++) {
            B(i,j) = hologainimpl::transfer(
                        this->geometry()->position(j),
                        this->geometry()->direction(j),
                        q);
        }
    }

    Eigen::JacobiSVD<Eigen::MatrixXcf> svd(B, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::JacobiSVD<Eigen::MatrixXcf>::SingularValuesType singularValues_inv= svd.singularValues();
    for ( long i=0; i < singularValues_inv.size(); ++i) {
        singularValues_inv(i) =  singularValues_inv(i) / (singularValues_inv(i) *singularValues_inv(i)  + alpha*alpha);
    }
    Eigen::MatrixXcf pinvB = (svd.matrixV()*singularValues_inv.asDiagonal()*svd.matrixU().adjoint());

    Eigen::MatrixXcf MM = P*(Eigen::MatrixXcf::Identity(M, M) - B*pinvB)*P;
    Eigen::MatrixXcf X = Eigen::MatrixXcf::Identity(M, M);
    for (int i = 0; i < REPEAT_SDP; i++) {
        unsigned int ii = mt()*M;

        Eigen::MatrixXcf Xc = X;
        hologainimpl::removeRow(Xc, ii);
        hologainimpl::removeColumn(Xc, ii);
        Eigen::VectorXcf MMc = MM.col(ii);
        MMc.block(ii,0,MMc.rows()-1-ii,1) = MMc.block(ii+1,0,MMc.rows()-1-ii,1);
        MMc.conservativeResize(MMc.rows()-1, 1);

        Eigen::VectorXcf x = Xc*MMc;
        std::complex<float> gamma = x.adjoint()*MMc;
        if (gamma.real()>1e-9) {
            x = -x*sqrt(LAMBDA_SDP/gamma.real());
            X.block(ii,0,1,ii) = x.block(0,0,ii,1).adjoint().eval();
            X.block(ii,ii+1, 1, M-ii-1) = x.block(ii,0,M-1-ii,1).adjoint().eval();
            X.block(0,ii,ii,1) = x.block(0,0,ii,1).eval();
            X.block(ii+1,ii, M-ii-1, 1) = x.block(ii,0,M-1-ii,1).eval();
        }else {
            X.block(ii,0,1,ii) = Eigen::VectorXcf::Zero(ii).adjoint();
            X.block(ii,ii+1, 1, M-ii-1) = Eigen::VectorXcf::Zero(M-1).adjoint();
            X.block(0,ii,ii,1) = Eigen::VectorXcf::Zero(ii);
            X.block(ii+1,ii, M-ii-1, 1) = Eigen::VectorXcf::Zero(M-1);
        }

    }

    Eigen::ComplexEigenSolver<Eigen::MatrixXcf> ces(X);
    Eigen::VectorXcf evs = ces.eigenvalues();
    float abseiv = 0;
    int idx = 0;
    for (int j = 0; j < evs.rows(); j++) {
        float eiv = abs(evs(j));
        if (abseiv < eiv) {
            abseiv = eiv;
            idx = j;
        }
    }

    Eigen::VectorXcf u = ces.eigenvectors().col(idx);
    q = pinvB*P*u;

    this->_data.clear();
    const int ndevice = this->geometry()->numDevices();
    for (int i = 0; i < ndevice; i++) {
        this->_data[this->geometry()->deviceIdForDeviceIdx(i)].resize(NUM_TRANS_IN_UNIT);
    }

    //float maxCoeff = sqrt(q.cwiseAbs2().maxCoeff());
    for (int j = 0; j < N; j++) {
        float famp = 1.0;//abs(q(j))/maxCoeff;
        float fphase = arg(q(j)) / (2*M_PI) + 0.5;
        uint8_t amp = famp*255, phase = (1-fphase)*255;
        this->_data[this->geometry()->deviceIdForTransIdx(j)][j%NUM_TRANS_IN_UNIT] = ((uint16_t)amp << 8) + phase;
    }
}


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * estimate_plane.cpp
 *
 *  Created on: Dec 28, 2020
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab.
 */

#include "mrob/estimate_plane.hpp"
#include <Eigen/Eigenvalues>

using namespace mrob;

Mat41 estimate_plane_centered(const Eigen::Ref<const MatX> X);

Mat41 mrob::estimate_plane(const Eigen::Ref<const MatX> X)
{
    // Initialization
    assert(X.cols() == 3  && "Estimate_plane: Incorrect sizing, we expect Nx3");
    assert(X.rows() >= 3  && "Estimate_plane: Incorrect sizing, we expect at least 3 correspondences (not aligned)");

    // Plane estimation, centered approach
    return estimate_plane_centered(X);
}

//local function, we also will test the homogeneous plane estimation
Mat41 estimate_plane_centered(const Eigen::Ref<const MatX> X)
{
    uint_t N = X.rows();
    // Calculate center of points:
    Mat13 c =  X.colwise().sum();
    c /= (double)N;

    MatX qx = X.rowwise() - c;
    Mat3 C = qx.transpose() * qx;

    Eigen::SelfAdjointEigenSolver<Mat3> eigs;
    eigs.computeDirect(C);

    Mat31 normal = eigs.eigenvectors().col(0);

    matData_t d = - c*normal;

    Mat41 plane;
    plane << normal, d;
    // error = eigs.eigenvalues()(0);
    return plane;

}

Mat31 mrob::estimate_normal(const Eigen::Ref<const MatX> X)
{
    Mat41 res = estimate_plane(X);
    return res.head(3);
}

Mat31 mrob::estimate_centroid(const Eigen::Ref<const MatX> X)
{
    // Initialization
    assert(X.cols() == 3  && "Estimate_centroid: Incorrect sizing, we expect Nx3");
    assert(X.rows() >= 3  && "Estimate_centroid: Incorrect sizing, we expect at least 3 correspondences (not aligned)");

    uint_t N = X.rows();
    Mat13 c =  X.colwise().sum();
    c /= (double)N;
    return c;
}


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

using namespace Eigen;

int main() {
  Matrix2d a;
  a << 1, 2,
      3, 4;
  MatrixXd b(2, 2);
  b << 2, 3,
      1, 4;
  std::cout << "a + b =\n" << a + b << std::endl;
  std::cout << "a - b =\n" << a - b << std::endl;
  std::cout << "Doing a += b;" << std::endl;
  a += b;
  std::cout << "Now a =\n" << a << std::endl;
  Vector3d v(1, 2, 3);
  Vector3d w(1, 0, 0);
  std::cout << "-v + w - v =\n" << -v + w - v << std::endl;
}


#include <Eigen/Dense>
#include <iostream>
#include <vector>

using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::cout;
using std::endl;

MatrixXd CalculateJacobian(const VectorXd& x_state);

int main() {
  /**
   * Compute the Jacobian Matrix
   */

  // predicted state example
  // px = 1, py = 2, vx = 0.2, vy = 0.4
  VectorXd x_predicted(4);
  x_predicted << 1, 2, 0.2, 0.4;

  MatrixXd Hj = CalculateJacobian(x_predicted);

  cout << "Hj:" << endl << Hj << endl;

  return 0;
}

MatrixXd CalculateJacobian(const VectorXd& x_state) {

  MatrixXd Hj(3, 4);
  // recover state parameters
  float px = x_state(0);
  float py = x_state(1);
  float vx = x_state(2);
  float vy = x_state(3);

  // TODO: YOUR CODE HERE
  const float denom2 = px * px + py * py;
  const float denom = std::sqrt(denom2);
  const float denom3 = denom * denom2;

  // check division by zero
  if (std::abs(px - py) <= 1e-8) {
    cout << "CalculateJacobian () - Error - Division by Zero" << endl;
    return Hj;
  }

  // compute the Jacobian matrix
  Hj(0, 0) = px / denom;
  Hj(0, 1) = py / denom;
  Hj(0, 2) = Hj(0, 3) = 0.0f;
  Hj(1, 0) = -py / denom2;
  Hj(1, 1) = px / denom2;
  Hj(1, 2) = Hj(1, 3) = 0.0f;
  Hj(2, 0) = py * (vx * py - vy * px) / denom3;
  Hj(2, 1) = px * (vy * px - vx * py) / denom3;
  Hj(2, 2) = px / denom;
  Hj(2, 3) = py / denom;

  return Hj;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

/*
This program demonstrates the use of the fluid decomposition toolbox
to compute a summary of spectral features of an audio file
*/

#include <AudioFile/IAudioFile.h>
#include <Eigen/Core>
#include <algorithms/public/DCT.hpp>
#include <algorithms/public/Loudness.hpp>
#include <algorithms/public/MelBands.hpp>
#include <algorithms/public/STFT.hpp>
#include <algorithms/public/SpectralShape.hpp>
#include <algorithms/public/MultiStats.hpp>
#include <algorithms/public/YINFFT.hpp>
#include <data/FluidIndex.hpp>
#include <data/TensorTypes.hpp>
#include <cstdio>
#include <iomanip>
#include <iostream>

fluid::RealVector computeStats(fluid::RealMatrixView   matrix,
                               fluid::algorithm::MultiStats stats)
{
  fluid::index      dim = matrix.cols();
  fluid::RealMatrix tmp(dim, 7);
  fluid::RealVector result(dim * 7);
  stats.process(matrix.transpose(), tmp);
  for (int j = 0; j < dim; j++)
  {
    result(fluid::Slice(j * 7, 7)) = tmp.row(j);
  }
  return result;
}

void printRow(std::string name, fluid::RealVectorView vals)
{
  using namespace std;
  cout << setw(10) << name;
  for (auto v : vals) { cout << setw(10) << setprecision(5) << v; }
  cout << endl;
}

int main(int argc, char* argv[])
{
  using namespace fluid;
  using namespace fluid::algorithm;
  using fluid::index;
  using std::cout;
  using std::endl;
  using std::setw;

  if (argc <= 1)
  {
    cout << "usage: describe input_file.wav\n";
    return 1;
  }
  const char* inputFile = argv[1];

  HISSTools::IAudioFile file(inputFile);

  index nSamples = file.getFrames();
  auto  samplingRate = file.getSamplingRate();

  if (!file.isOpen())
  {
    cout << "Input file could not be opened\n";
    return -2;
  }

  index nBins = 513;
  index fftSize = 2 * (nBins - 1);
  index hopSize = 1024;
  index windowSize = 1024;
  index halfWindow = windowSize / 2;
  index nBands = 40;
  index nCoefs = 13;
  index minFreq = 20;
  index maxFreq = 5000;

  STFT          stft{windowSize, fftSize, hopSize};
  MelBands      bands{nBands, fftSize};
  DCT           dct{nBands, nCoefs};
  YINFFT        yin;
  SpectralShape shape;
  Loudness      loudness{windowSize};
  MultiStats    stats;

  bands.init(minFreq, maxFreq, nBands, nBins, samplingRate, windowSize);
  dct.init(nBands, nCoefs);
  stats.init(0, 0, 50, 100);
  loudness.init(windowSize, samplingRate);

  RealVector in(nSamples);
  file.readChannel(in.data(), nSamples, 0);
  RealVector padded(in.size() + windowSize + hopSize);
  index      nFrames = floor((padded.size() - windowSize) / hopSize);
  RealMatrix pitchMat(nFrames, 2);
  RealMatrix loudnessMat(nFrames, 2);
  RealMatrix mfccMat(nFrames, nCoefs);
  RealMatrix shapeMat(nFrames, 7);
  std::iota(padded.begin(), padded.end(), 0);
  padded(Slice(halfWindow, in.size())) = in;

  for (int i = 0; i < nFrames; i++)
  {
    ComplexVector  frame(nBins);
    RealVector     magnitude(nBins);
    RealVector     mels(nBands);
    RealVector     mfccs(nCoefs);
    RealVector     pitch(2);
    RealVector     shapeDesc(7);
    RealVector     loudnessDesc(2);
    RealVectorView window = padded(fluid::Slice(i * hopSize, windowSize));
    stft.processFrame(window, frame);
    stft.magnitude(frame, magnitude);
    bands.processFrame(magnitude, mels, false, false, true);
    dct.processFrame(mels, mfccs);
    mfccMat.row(i) = mfccs;
    yin.processFrame(magnitude, pitch, minFreq, maxFreq, samplingRate);
    pitchMat.row(i) = pitch;
    shape.processFrame(magnitude, shapeDesc, samplingRate);
    shapeMat.row(i) = shapeDesc;
    loudness.processFrame(window, loudnessDesc, true, true);
    loudnessMat.row(i) = loudnessDesc;
  }
  RealVector  pitchStats = computeStats(pitchMat, stats);
  RealVector  loudnessStats = computeStats(loudnessMat, stats);
  RealVector  shapeStats = computeStats(shapeMat, stats);
  RealVector  mfccStats = computeStats(mfccMat, stats);
  std::string colNames[] = {"Descriptor", "mean", "stdev", "skew",
                            "kurt",       "low",  "mid",   "high"};
  std::string shapeDescs[] = {"Centroid", "Spread",   "Skewness", "Kurtosis",
                              "Rolloff",  "Flatness", "Crest"};
  for (auto n : colNames) cout << setw(10) << n;
  cout << endl;
  printRow("Pitch", pitchStats(Slice(0, 7)));
  printRow("Pitch Conf.", pitchStats(Slice(7, 7)));
  printRow("Loudness", loudnessStats(Slice(0, 7)));
  printRow("True Peak", loudnessStats(Slice(7, 7)));
  for (int i = 0; i < 7; i++)
    printRow(shapeDescs[i], shapeStats(Slice(i * 7, 7)));
  for (int i = 0; i < 13; i++)
    printRow("MFCC" + std::to_string(i), mfccStats(Slice(i * 7, 7)));
  return 0;
}


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

#ifndef MTL_HESSIAN_MATRIX_UTILITIES_INCLUDE
#define MTL_HESSIAN_MATRIX_UTILITIES_INCLUDE

#include <cmath>
#include <iostream>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/matrix/inserter.hpp>
#include <boost/numeric/mtl/operation/entry_similar.hpp>

namespace mtl { namespace matrix {

/// Fills a matrix A with A[i][j] = factor * (i + j)
/** Intended for dense matrices.
    Works on sparse matrices with inserter but is very expensive. **/
template <typename Matrix, typename Value>
void hessian_setup(Matrix& A, Value factor)
{
    typedef typename Collection<Matrix>::value_type    value_type;
    typedef typename Collection<Matrix>::size_type     size_type;

    inserter<Matrix> ins(A, num_cols(A));

    for (size_type r= 0; r < num_rows(A); r++)
	for (size_type c= 0; c < num_cols(A); c++)
	    ins[r][c] << factor * (value_type(r) + value_type(c));
}

namespace impl {

    /*
    - Check matrix product C = A * B with:
      - A is MxN, B is NxL, C is MxL
      - with matrices a_ij = i+j, b_ij = 2(i+j);
      - c_ij = 1/3 N (1 - 3i - 3j + 6ij - 3N + 3iN + 3jN + 2N^2).

    */
    // Not really generic
    template <typename Value>
    double inline hessian_product_i_j (Value i, Value j, Value N)
    {
        return 1.0/3.0 * N * (1.0 - 3*i - 3*j + 6*i*j - 3*N + 3*i*N + 3*j*N + 2*N*N);
    }

    template <typename Value>
    inline bool similar_values(Value x, Value y)
    {
        using std::abs; using std::max;
        return abs(x - y) / max(abs(x), abs(y)) < 0.000001;
    }

    template <typename Matrix>
    void inline check_entry(Matrix const& C, unsigned long r, unsigned long c,
			    unsigned long reduced_dim, double factor)
    {
	if (!entry_similar(C, r, c, factor * hessian_product_i_j(r, c, reduced_dim), 0.00001)) {
	    std::cerr << "Result in C[" << r << "][" << c << "] should be "
		      << factor * hessian_product_i_j(r, c, reduced_dim)
		      << " but is " << C[r][c] << "\n";
	    MTL_THROW(unexpected_result());
	}
    }

} // impl

/// Check if matrix C is A * B with A and B set by hessian_setup
/** C has dimensions M x L and reduced_dim is N, see hessian_setup. **/
template <typename Matrix>
void check_hessian_matrix_product(Matrix const& C, typename Matrix::size_type reduced_dim, double factor= 1.0)
{
    if (num_rows(C) * num_cols(C) == 0) return; // otherwise out of range

    impl::check_entry(C, 0, 0, reduced_dim, factor);
    impl::check_entry(C, 0, num_cols(C)-1, reduced_dim, factor);
    impl::check_entry(C, num_rows(C)-1, 0, reduced_dim, factor);
    impl::check_entry(C, num_rows(C)-1, num_cols(C)-1, reduced_dim, factor);
    impl::check_entry(C, num_rows(C)/2, num_cols(C)/2, reduced_dim, factor);
}

} // namespace matrix;

using matrix::hessian_setup;
using matrix::check_hessian_matrix_product;

} // namespace mtl

#endif // MTL_HESSIAN_MATRIX_UTILITIES_INCLUDE


/*
    This file is part of beautiful-bullet.

    Copyright (c) 2021, 2022 Bernardo Fichera <bernardo.fichera@gmail.com>

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

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

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

#ifndef BEAUTIFUL_BULLET_TOOLS_MATH_HPP
#define BEAUTIFUL_BULLET_TOOLS_MATH_HPP

#include <Eigen/Geometry>

namespace beautiful_bullet {
    namespace tools {
        Eigen::Matrix<double, 6, 1> lieAlgebraSE3(const Eigen::Vector3d& position, const Eigen::Matrix3d& orientation)
        {
            Eigen::AngleAxisd aa(orientation);

            Eigen::Vector3d omega = aa.angle() * aa.axis();

            Eigen::Matrix3d omega_skew;
            omega_skew << 0, -omega(2), omega(1),
                omega(2), 0, -omega(0),
                -omega(1), omega(0), 0;

            double theta = omega.norm(), A = std::sin(theta) / theta, B = (1 - std::cos(theta)) / std::pow(theta, 2);

            return (Eigen::Matrix<double, 6, 1>() << (Eigen::Matrix3d::Identity() - 0.5 * omega_skew + (1 - 0.5 * A / B) / std::pow(theta, 2) * omega_skew * omega_skew) * position, omega).finished();
        }

        // Check here if this can be improved
        Eigen::MatrixXd pseudoInverse(const Eigen::MatrixXd& M)
        {
            double epsilon = std::numeric_limits<double>::epsilon();

            Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);

            Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType sing_vals = svd.singularValues();

            double tolerance = epsilon * std::max(M.cols(), M.rows()) * sing_vals.array().abs()(0);
            Eigen::MatrixXd S = Eigen::MatrixXd::Zero(M.rows(), M.cols());

            for (size_t i = 0; i < sing_vals.size(); i++)
                if (std::fabs(sing_vals(i)) > tolerance)
                    S(i, i) = 1.0 / sing_vals(i);

            return svd.matrixV() * S.transpose() * svd.matrixU().adjoint();
        }
    } // namespace tools

} // namespace beautiful_bullet

#endif // BEAUTIFUL_BULLET_TOOLS_MATH_HPP


#pragma once
#include <cstdint>
#include <NTL/ZZ.h>
#include <NTL/RR.h>
#include <NTL/matrix.h>

/* binomials are precomputed up to MAX_N-choose-MAX_T */
#define MAX_N 2000
#define MAX_T 300

#define LOW_K_MAX_N 10000
#define LOW_K_MAX_T 10

const NTL::RR nat_log_2 = NTL::log(NTL::RR(2));

static inline NTL::RR log2_RR(NTL::RR v){
    return NTL::log(v)/nat_log_2;
}

NTL::Mat<NTL::ZZ> binomial_table; /*contains all binomials up to MAX_N-choose-MAX_T */
NTL::Mat<NTL::ZZ> low_k_binomial_table; /*contains all binomials up to LOW_K_MAX_N-choose-LOW_K_MAX_T */

NTL::RR pi;
/*NOTE: NTL allows to access matrices as 1- based with Matlab notation */
void InitBinomials(){
    std::cerr << "Precomputing n-choose-t up to n: " << MAX_N <<
                                              " t: " << MAX_T << std::endl;
    binomial_table.SetDims(MAX_N+1,MAX_T+1);
    binomial_table[0][0] = NTL::ZZ(1);
    for (unsigned i = 1 ; i <= MAX_N; i++){
        binomial_table[i][0] = NTL::ZZ(1);
        binomial_table[i][1] = NTL::ZZ(i);
        for(unsigned j=2 ; (j <= i) && (j <= MAX_T) ; j++){
            binomial_table[i][j] = binomial_table[i][j-1] * NTL::ZZ(i-j+1) / NTL::ZZ(j);
        }
    }
    std::cerr << "Precomputing low n-choose-t up to n: " << LOW_K_MAX_N <<
                                                       " t: " << LOW_K_MAX_T << std::endl;
    low_k_binomial_table.SetDims(LOW_K_MAX_N+1,LOW_K_MAX_T+1);
    low_k_binomial_table[0][0] = NTL::ZZ(1);
    for (unsigned i = 0 ; i <= LOW_K_MAX_N; i++){
        low_k_binomial_table[i][0] = NTL::ZZ(1);
        low_k_binomial_table[i][1] = NTL::ZZ(i);
        for(unsigned j=2 ; (j <= i) && (j <= LOW_K_MAX_T) ; j++){
            low_k_binomial_table[i][j] = low_k_binomial_table[i][j-1] * NTL::ZZ(i-j+1) / NTL::ZZ(j);
        }
    }
    std::cerr << "done" << std::endl;
    pi = NTL::ComputePi_RR();
}

NTL::RR lnFactorial(NTL::RR n){
    /* log of Stirling series approximated to the fourth term
     * n log(n) - n + 1/2 log(2 \pi n) + log(- 139/(51840 n^3) +
     * + 1/(288 n^2) + 1/(12 n) + 1) */
    return n * NTL::log(n) - n + 0.5 * NTL::log(2*pi*n) +
           NTL::log( - NTL::RR(139)/(n*n*n * 51840) +
           NTL::RR(1)/(n*n*288) +
           NTL::RR(1)/(n*12) +
           1);
}

NTL::RR lnBinom(NTL::RR n, NTL::RR k){
    if ( (k == NTL::RR(0) ) || (k == n) ) {
        return NTL::RR(0);
    }
    return lnFactorial(n) - (lnFactorial(k) + lnFactorial(n-k) );
}

NTL::ZZ binomial_wrapper(long n, long k){
    if(k>n) return NTL::ZZ(0);
    /* employ memoized if available */
    if ((n <= MAX_N) && (k < MAX_T)){
        return binomial_table[n][k];
    }
    if ((n <= LOW_K_MAX_N) && (k < LOW_K_MAX_T)){
        return low_k_binomial_table[n][k];
    }

    /* shortcut computation for fast cases (k < 10) where
     * Stirling may not provide good approximations */
    if (k < 10) {
        NTL::ZZ result = NTL::ZZ(1);
        for(int i = 1 ; i <= k; i++){
            result = (result * (n+1-i))/i;
        }
        return result;
    }
    /*Fall back to Stirling*/
    return NTL::conv<NTL::ZZ>( NTL::exp( lnBinom(NTL::RR(n),NTL::RR(k)) ));
}


/**
 * @file
 * @author lkoppel
 * Defines an Eigen expression for the "cross-product" or "hat" matrix of a 3-vector
 */

#ifndef WAVE_GEOMETRY_CROSSMATRIX_HPP
#define WAVE_GEOMETRY_CROSSMATRIX_HPP

#include <Eigen/Geometry>

namespace wave {

/**
 * An Eigen expression for the skew-symmetric "cross-product" or "hat" matrix of a
 * 3-vector. This matrix applies the cross product:
 *
 * @f[ a \times b = \text{CrossMatrix}(a) b @f]
 *
 * In different works, `CrossMatrix(a)` might be written as @f$ a^{\times} @f$ or
 * @f$ \hat{a} @f$ .
 *
 * @tparam VecType the type of the vector expression
 */
// The implementation of an Eigen expression here was guided by
// https://eigen.tuxfamily.org/dox/TopicNewExpressionType.html
template <class VecType>
class CrossMatrix : public Eigen::Matrix<typename VecType::Scalar, 3, 3> {
 public:
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecType, 3);
    using MatrixType = Eigen::Matrix<typename VecType::Scalar, 3, 3>;
    using Nested = typename Eigen::internal::ref_selector<CrossMatrix>::type;
    using VecTypeNested = typename Eigen::internal::ref_selector<VecType>::type;

    using Scalar = typename VecType::Scalar;

    EIGEN_STRONG_INLINE
    explicit CrossMatrix(const VecType &vec)
        : MatrixType{(MatrixType{} << Scalar{0},
                      -vec(2),
                      vec(1),
                      vec(2),
                      Scalar{0},
                      -vec(0),
                      -vec(1),
                      vec(0),
                      Scalar{0})
                       .finished()},
          vec{vec} {}

    using NegativeReturnType = CrossMatrix<typename VecType::NegativeReturnType>;

    EIGEN_DEVICE_FUNC
    inline NegativeReturnType operator-() const {
        return NegativeReturnType{-this->vec};
    }

    VecTypeNested vec;
};

/** Produce a cross matrix*/
template <class VecType>
inline auto crossMatrix(const Eigen::MatrixBase<VecType> &vec) -> CrossMatrix<VecType> {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecType, 3);
    return CrossMatrix<VecType>(vec.derived());
}

template <class VecType>
inline auto crossMatrix(const Eigen::MatrixBase<VecType> &&vec) -> CrossMatrix<VecType> {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VecType, 3);
    return CrossMatrix<VecType>(std::move(vec.derived()));
}

// Forward declaration
template <typename Scalar, int N>
class IdentityMatrix;

}  // namespace wave

namespace Eigen {

/** Left-multiply a fixed-size 3-vector by a cross-matrix
 *
 * Performs a cross product
 */
template <
  typename VecType,
  typename OtherType,
  std::enable_if_t<OtherType::RowsAtCompileTime == 3 && OtherType::ColsAtCompileTime == 1,
                   int> = 0>
EIGEN_DEVICE_FUNC inline auto operator*(const wave::CrossMatrix<VecType> &crossMat,
                                        const Eigen::MatrixBase<OtherType> &rhs) {
    return crossMat.vec.cross(rhs);
}

/** Right-multiply a fixed-size 3-vector by a cross-matrix
 *
 * Performs a cross product
 */
template <
  typename VecType,
  typename OtherType,
  std::enable_if_t<OtherType::ColsAtCompileTime == 3 && OtherType::RowsAtCompileTime == 1,
                   int> = 0>
EIGEN_DEVICE_FUNC inline auto operator*(const Eigen::MatrixBase<OtherType> &lhs,
                                        const wave::CrossMatrix<VecType> &crossMat) {
    return lhs.cross(crossMat.vec);
}

/**
 * Left-multiply a 3*N matrix by a cross-matrix
 *
 * This operation is equivalent to taking the cross product of each row in the l.h.s.
 * matrix with the negative original vector
 *
 * Only enabled for static matrices
 */
template <
  typename VecType,
  typename OtherType,
  std::enable_if_t<OtherType::RowsAtCompileTime == 3 && OtherType::ColsAtCompileTime != 1,
                   int> = 0>
EIGEN_DEVICE_FUNC inline auto operator*(const wave::CrossMatrix<VecType> &crossMat,
                                        const Eigen::MatrixBase<OtherType> &rhs) {
    return rhs.colwise().cross(-crossMat.vec);
}

/**
 * Left-multiply a 3*N matrix by a cross-matrix of a negative
 *
 * This operation is equivalent to taking the cross product of each row in the l.h.s.
 * matrix with the original vector
 *
 * This special case cuts down one instruction(?), but more importantly simplifies the
 * expression graph output.
 */
template <
  typename VecType,
  typename OtherType,
  std::enable_if_t<OtherType::RowsAtCompileTime == 3 && OtherType::ColsAtCompileTime != 1,
                   int> = 0>
EIGEN_DEVICE_FUNC inline auto operator*(
  const wave::CrossMatrix<Eigen::CwiseUnaryOp<
    Eigen::internal::scalar_opposite_op<typename internal::traits<VecType>::Scalar>,
    VecType>> &crossMat,
  const Eigen::MatrixBase<OtherType> &rhs) {
    return rhs.derived().colwise().cross(crossMat.vec.nestedExpression());
}

/**
 * Right-multiply an N*3 matrix by a cross-matrix
 *
 * This operation is equivalent to taking the cross product of the original vector with
 * each column in the r.h.s. matrix, and negating it since a x b = -(b x a).
 *
 * Only enabled for static matrices
 */
template <
  typename OtherType,
  typename VecType,
  std::enable_if_t<OtherType::ColsAtCompileTime == 3 && OtherType::RowsAtCompileTime != 1,
                   int> = 0>
EIGEN_DEVICE_FUNC inline auto operator*(const Eigen::MatrixBase<OtherType> &lhs,
                                        const wave::CrossMatrix<VecType> &crossMat) {
    return lhs.rowwise().cross(crossMat.vec);
}

/**
 * Multiply two cross-matrices
 * (needed to disambiguate)
 */
template <typename Lhs, typename Rhs>
EIGEN_DEVICE_FUNC inline auto operator*(const wave::CrossMatrix<Lhs> &lhs,
                                        const wave::CrossMatrix<Rhs> &rhs) {
    return lhs.rowwise().cross(rhs.vec);
}

/**
 * Multiply an Identity expression by a CrossMatrix expression
 * (Provided to break tie between the other specializations)
 */
// @todo generalize - this won't scale well if we add more expressions with operator*
// @todo Check scalar mixing?
template <typename VecType, typename Scalar>
EIGEN_DEVICE_FUNC inline const wave::CrossMatrix<VecType> &operator*(
  const wave::IdentityMatrix<Scalar, 3> &, const wave::CrossMatrix<VecType> &cross) {
    return cross;
}

/**
 * Multiply an CrossMatrix expression by an Identity expression
 * (Provided to break tie between the other specializations)
 */
// @todo generalize - this won't scale well if we add more expressions with operator*
// @todo Check scalar mixing?
template <typename VecType, typename Scalar>
EIGEN_DEVICE_FUNC inline const wave::CrossMatrix<VecType> &operator*(
  const wave::CrossMatrix<VecType> &cross, const wave::IdentityMatrix<Scalar, 3> &) {
    return cross;
}

namespace internal {

// Static attributes of our CrossMatrix expression
// See https://eigen.tuxfamily.org/dox/TopicNewExpressionType.html
template <class VecType>
struct traits<::wave::CrossMatrix<VecType>> {
    using StorageKind = Eigen::Dense;
    using XprKind = Eigen::MatrixXpr;
    using StorageIndex = typename VecType::StorageIndex;
    using Scalar = typename VecType::Scalar;
    enum {
        Flags = Eigen::AutoAlign,
        RowsAtCompileTime = 3,
        ColsAtCompileTime = 3,
        MaxRowsAtCompileTime = 3,
        MaxColsAtCompileTime = 3
    };
};

}  // namespace internal
}  // namespace Eigen

#endif  // WAVE_GEOMETRY_CROSSMATRIX_HPP


/*
BKSVD implemented by C++ based on https://github.com/cpmusco/bksvd
Author: ItakEjgo@SUSTech
*/

#include <bits/stdc++.h>
#include <Eigen/Dense>
#include <random>
#include <ctime>

#define EIGEN_USE_MKL_ALL

using namespace std;
using namespace Eigen;

typedef Matrix<double, Dynamic, Dynamic, RowMajor> MatrixXd_row;
typedef Matrix<double, Dynamic, Dynamic, ColMajor> MatrixXd_col;

struct bksvd_output {
	MatrixXd U;		// an orthogonal matrix whose columns are approximate top k left singular vectors for A
	MatrixXd S;		// a diagonal matrix whose entries are A's approximate top k singular values
	MatrixXd V;		// an orthogonal matrix whose columns are approximate top k right singular vectors for A
};

static default_random_engine e(233666);
static normal_distribution<double> m_n(0, 1);

//	function bksvd

bksvd_output bksvd(MatrixXd &A, int k = 6, int iter = 3, int bsize = 6, bool center = false) {
	bksvd_output result;

	k = min(k, (int)min(A.rows(), A.cols()));
	bsize = k;

	if (k < 1 || iter < 1 || bsize < k) {
		cerr << "bksvd: BadInput, Oner or more inputs outside required range" << endl;
	}

	//	Calculate row mean if rows should be centered.
	MatrixXd u = MatrixXd::Zero(1, A.cols());
	if (center) {
		for (int i = 0; i < A.cols(); i++) {
			u(0, i) = A.col(i).mean();
		}
	}
	MatrixXd l = MatrixXd::Ones(A.rows(), 1);

	//	We want to iterate on the smaller dimension of A.
	int n, ind;
	if (A.rows() <= A.cols()) {
		n = A.rows();
		ind = 1;
	}
	else {
		n = A.cols();
		ind = 2;
	}
	bool tpose = false;
	if (ind == 1) {
		tpose = true;
		l = u.transpose(); u.setOnes(1, A.rows());
		A.transposeInPlace();
	}

	//	Allocate space for Krylov subspace.
	MatrixXd K = MatrixXd::Zero(A.cols(), bsize * iter);
	//	Random block initialization.
	MatrixXd block = MatrixXd::Zero(A.cols(), bsize).unaryExpr([](double dummy) {return m_n(e); });

	HouseholderQR<MatrixXd> qr;
	qr.compute(block);
	block.noalias() = qr.householderQ() * MatrixXd::Identity(A.cols(), bsize);

	//	Preallocate space for temporary products.
	MatrixXd T = MatrixXd::Zero(A.cols(), bsize);

	//	Construct and orthonormalize Krlov Subspace.
	//	Orthogonalize at each step using economy size QR decomposition
	for (int i = 1; i <= iter; i++) {
		T.noalias() = A * block;
		T.noalias() -= l * (u * block);
		//T = A * block - l * (u * block);
		block.noalias() = A.transpose() * T;
		block.noalias() -= u.transpose() * (l.transpose() * T);
		//block = A.transpose() * T - u.transpose() * (l.transpose() * T);
		HouseholderQR<MatrixXd> tmp_qr;
		tmp_qr.compute(block);
		block.noalias() = tmp_qr.householderQ() * MatrixXd::Identity(block.rows(), block.cols());;
		K.middleCols((i - 1)*bsize, bsize) = block;
	}

	//	Rayleigh-Ritz postprocessing with economy size dense SVD.
	HouseholderQR<MatrixXd> tmp_qr;

	tmp_qr.compute(K);
	MatrixXd Q = tmp_qr.householderQ() * MatrixXd::Identity(K.rows(), K.cols());

	T.noalias() = A * Q;
	T.noalias() -= l * (u * Q);
	//T = A * Q - l * (u * Q);

	BDCSVD<MatrixXd> svd(T, ComputeThinU | ComputeThinV);    //	For large scale Matrix
	//JacobiSVD<MatrixXd>svd(T, ComputeThinU | ComputeThinV); // For small scale Matrix

	MatrixXd Ut = svd.matrixU(), Vt = svd.matrixV();
	MatrixXd St = svd.singularValues().asDiagonal();
	result.S = St.block(0, 0, k, k);
	if (!tpose) {
		result.U = Ut.leftCols(k);
		result.V = Q * Vt.leftCols(k);
	}
	else {
		result.V = Ut.leftCols(k);
		result.U = Q * Vt.leftCols(k);
	}
	return result;
}

//	using reference

void bksvd(bksvd_output &result, MatrixXd &A, int k = 6, int iter = 3, int bsize = 6, bool center = false) {

	k = min(k, (int)min(A.rows(), A.cols()));
	bsize = k;

	if (k < 1 || iter < 1 || bsize < k) {
		cerr << "bksvd: BadInput, Oner or more inputs outside required range" << endl;
	}

	//	Calculate row mean if rows should be centered.
	MatrixXd u = MatrixXd::Zero(1, A.cols());
	if (center) {
		for (int i = 0; i < A.cols(); i++) {
			u(0, i) = A.col(i).mean();
		}
	}
	MatrixXd l = MatrixXd::Ones(A.rows(), 1);

	//	We want to iterate on the smaller dimension of A.
	int n, ind;
	if (A.rows() <= A.cols()) {
		n = A.rows();
		ind = 1;
	}
	else {
		n = A.cols();
		ind = 2;
	}
	bool tpose = false;
	if (ind == 1) {
		tpose = true;
		l = u.transpose(); u.setOnes(1, A.rows());
		A.transposeInPlace();
	}

	//	Allocate space for Krylov subspace.
	MatrixXd K = MatrixXd::Zero(A.cols(), bsize * iter);
	//	Random block initialization.
	MatrixXd block = MatrixXd::Zero(A.cols(), bsize).unaryExpr([](double dummy) {return m_n(e); });

	HouseholderQR<MatrixXd> qr;
	qr.compute(block);
	/*block.noalias() = qr.householderQ() * MatrixXd::Identity(A.cols(), bsize);*/
	block = qr.householderQ() * MatrixXd::Identity(A.cols(), bsize);

	//	Preallocate space for temporary products.
	MatrixXd T = MatrixXd::Zero(A.cols(), bsize);

	//	Construct and orthonormalize Krlov Subspace.
	//	Orthogonalize at each step using economy size QR decomposition
	for (int i = 1; i <= iter; i++) {
		/*T.noalias() = A * block;
		T.noalias() -= l * (u * block);*/
		T = A * block - l * (u * block);
		/*block.noalias() = A.transpose() * T;
		block.noalias() -= u.transpose() * (l.transpose() * T);*/
		block = A.transpose() * T - u.transpose() * (l.transpose() * T);
		HouseholderQR<MatrixXd> tmp_qr;
		tmp_qr.compute(block);
		/*block.noalias() = tmp_qr.householderQ() * MatrixXd::Identity(block.rows(), block.cols());*/
		block = tmp_qr.householderQ() * MatrixXd::Identity(block.rows(), block.cols());
		K.middleCols((i - 1)*bsize, bsize) = block;
	}

	//	Rayleigh-Ritz postprocessing with economy size dense SVD.
	HouseholderQR<MatrixXd> tmp_qr;

	tmp_qr.compute(K);
	MatrixXd Q = tmp_qr.householderQ() * MatrixXd::Identity(K.rows(), K.cols());

	/*T.noalias() = A * Q;
	T.noalias() -= l * (u * Q);*/
	T = A * Q - l * (u * Q);

	BDCSVD<MatrixXd> svd(T, ComputeThinU | ComputeThinV);    //	For large scale Matrix
	//JacobiSVD<MatrixXd>svd(T, ComputeThinU | ComputeThinV); // For small scale Matrix

	MatrixXd Ut = svd.matrixU(), Vt = svd.matrixV();
	MatrixXd St = svd.singularValues().asDiagonal();
	result.S = St.block(0, 0, k, k);
	if (!tpose) {
		result.U = Ut.leftCols(k);
		result.V = Q * Vt.leftCols(k);
	}
	else {
		result.V = Ut.leftCols(k);
		result.U = Q * Vt.leftCols(k);
	}
}

//	function sisvd

bksvd_output sisvd(MatrixXd A, int k = 6, int iter = 3, int bsize = 6, bool center = false) {
	bksvd_output result;

	static default_random_engine e((unsigned int)time(0));
	static normal_distribution<double> m_n(0, 1);

	k = min(k, (int)min(A.rows(), A.cols()));
	bsize = k;
	if (k < 1 || iter < 1 || bsize < k) {
		cerr << "sisvd:BadInput, one or more inputs outside required range" << endl;
	}

	//	Calculate row mean if rows should be centered.
	MatrixXd u = MatrixXd::Zero(1, A.cols());
	if (center) {
		for (int i = 0; i < A.cols(); i++) {
			u(0, i) = A.col(i).mean();
		}
	}
	MatrixXd l = MatrixXd::Ones(A.rows(), 1);

	//	We want to iterate on the smaller dimension of A.
	int n, ind;
	if (A.rows() <= A.cols()) {
		n = A.rows();
		ind = 1;
	}
	else {
		n = A.cols();
		ind = 2;
	}
	bool tpose = false;
	if (ind == 1) {
		tpose = true;
		l = u.transpose(); u.setOnes(1, A.rows());
		A.transposeInPlace();
	}

	//	Random block initialization.
	MatrixXd block = MatrixXd::Zero(A.cols(), bsize).unaryExpr([](double dummy) {return m_n(e); });

	HouseholderQR<MatrixXd> qr;
	qr.compute(block);
	block = qr.householderQ() * MatrixXd::Identity(A.cols(), bsize);

	//	Preallocate space for temporary products.
	MatrixXd T = MatrixXd::Zero(A.cols(), bsize);

	//	Run power iteration, orthogonalizing at each step using economy size QR.
	for (int i = 1; i <= iter; i++) {
		T = A * block - l * (u * block);
		block = A.transpose() * T - u.transpose() * (l.transpose() * T);
		HouseholderQR<MatrixXd> tmp_qr;
		tmp_qr.compute(block);
		block = tmp_qr.householderQ() * MatrixXd::Identity(block.rows(), block.cols());
	}

	//	Rayleigh-Ritz postprocessing with economy size dense SVD.
	T = A * block - l * (u * block);
	//JacobiSVD<MatrixXd>svd(T, ComputeFullU | ComputeFullV);
	BDCSVD<MatrixXd> svd(T, ComputeThinU | ComputeThinV);
	MatrixXd Ut = svd.matrixU(), Vt = svd.matrixV();
	MatrixXd St = svd.singularValues().asDiagonal();
	result.S = St.block(0, 0, k, k);
	if (!tpose) {
		result.U = Ut.leftCols(k);
		result.V = block * Vt.leftCols(k);
	}
	else {
		result.V = Ut.leftCols(k);
		result.U = block * Vt.leftCols(k);
	}
	return result;
}

int main(){
	//ofstream fout;
	//fout.open("Matrix_A.txt");
	//MatrixXd A = MatrixXd::Zero(10000, 161).unaryExpr([](double dummy) {return m_n(e); });
	////MatrixXd_row A = MatrixXd::Zero(1000, 1000).unaryExpr([](double dummy) {return m_n(e); });
	//fout << A << endl;

	MatrixXd A = MatrixXd::Zero(10000, 161);
	ifstream fin; fin.open("MatrixA.txt");
	double val;
	for (int i = 0; i < 10000; i++) {
		for (int j = 0; j < 161; j++) {
			fin >> val;
			A(i, j) = val;
		}
	}

	MatrixXd B = A;
	bksvd_output result1, result2;
	clock_t start, end;
	start = clock();
	//result1 = bksvd(A,10);
	end = clock();
	printf("Time used = :%.5f second(s)\n", (double)(end - start) / CLOCKS_PER_SEC);

	start = clock();
	bksvd(result2, A, 10);
	end = clock();
	printf("Time used = :%.5f second(s)\n", (double)(end - start) / CLOCKS_PER_SEC);

	return 0;
}


#include "epipolar.h"
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>
#include "permutation_sampler.h"
#include "srrg_geometry/geometry3d.h"

namespace srrg2_core{
  namespace epipolar {
    using Matrix3_2f = Eigen::Matrix<float, 3, 2>;
    using Matrix9f   = Eigen::Matrix<float,9,9>;
    using Matrix3_9f = Eigen::Matrix<float, 3, 9>;
    using Vector9f   = Eigen::Matrix<float, 9, 1>;

    float triangulatePoint(Vector3f& p,
                           const Line3f& a,
                           const Line3f& b,
                           Vector2f& s) {
      const float epsilon=1e-9;
      Matrix3_2f D;
      D.col(0)=-a.direction;
      D.col(1)=b.direction;
      Matrix2f DtD=D.transpose()*D;
      if (fabs(DtD.determinant())<=epsilon)
        return -3.;
      s=DtD.inverse()*D.transpose()*(a.point-b.point);
      if (s(0)<0)
        return -1.;
      if (s(1)<0)
        return -2;
      Vector3f pa=a.point+a.direction*s(0);
      Vector3f pb=b.point+b.direction*s(1);
      p=0.5*(pa+pb);
      return (pa-pb).norm();
    }

    float triangulatePoint(Vector3f& p,
                           const Line3f& a,
                           const Line3f& b) {
      Vector2f s;
      return triangulatePoint(p,a,b,s);
    }

    int triangulateDirections(Vector3fVector& points,
                              std::vector<float>& results,
                              const Isometry3f& iso1,
                              const Isometry3f& iso2,
                              const Vector3fVector& directions1,
                              const Vector3fVector& directions2){
      assert(directions1.size()==directions2.size() && "directions size mismatch");
      size_t size=directions1.size();
      points.resize(size);
      results.resize(size);
      Line3f l1(iso1.translation(), Vector3f::Zero());
      Line3f l2(iso2.translation(), Vector3f::Zero());
      int good=0;
      for (size_t i=0; i<size; ++i) {
        l1.direction = iso1.linear()*directions1[i];
        l2.direction = iso2.linear()*directions2[i];
        results[i]=triangulatePoint(points[i], l1, l2);
        if (results[i]>=0)
          ++good;
      }
      return good;
    }

    void projectPointsOnSphere(Vector3fVector& dirs,
                               const Isometry3f& iso,
                               const Vector3fVector& points){
      dirs.resize(points.size());
      Isometry3f invT=iso.inverse();
      for (size_t i=0; i<points.size(); ++i) {
        dirs[i]=(invT*points[i]);
        dirs[i].normalize();
      }
    }

    Matrix3f iso2essential(const Eigen::Isometry3f& iso){
      const Vector3f t=iso.translation();
      return iso.linear().transpose()*geometry3d::skew(t);
    }

    void essential2iso(Isometry3f iso[2],
                       const Matrix3f& E) {
      Matrix3f W;
      W << 0, -1, 0,
        1, 0, 0,
        0, 0, 1;
      Eigen::JacobiSVD<Matrix3f> svd(E, Eigen::ComputeFullU|Eigen::ComputeFullV);
      Eigen::Matrix3f R=svd.matrixV()*W*svd.matrixU().transpose();
      if(R.determinant()<0) {
        svd=Eigen::JacobiSVD<Matrix3f>(-E, Eigen::ComputeFullU|Eigen::ComputeFullV);
        R=svd.matrixV()*W*svd.matrixU().transpose();
      }
      iso[0].setIdentity();
      iso[0].linear()=R;
      Matrix3f t_cross=R*E;
      iso[0].translation()=Vector3f(t_cross(2,1)-t_cross(1,2),
                                    t_cross(0,2)-t_cross(2,0),
                                    t_cross(1,0)-t_cross(0,1));

      iso[1].setIdentity();
      R=svd.matrixV()*W.transpose()*svd.matrixU().transpose();
      iso[1].linear()=R;
      t_cross=R*E;
      iso[1].translation()=Vector3f(t_cross(2,1)-t_cross(1,2),
                                    t_cross(0,2)-t_cross(2,0),
                                    t_cross(1,0)-t_cross(0,1));
    }

    float eightPointEstimate(Matrix3f& dest,
                             const Vector3fVector& dirs1,
                             const Vector3fVector& dirs2) {
      using Matrix9f=Eigen::Matrix<float, 9,9>;
      using Vector9f=Eigen::Matrix<float, 9,1>;
      Matrix9f H;
      H.setZero();
      assert(dirs1.size()==dirs2.size() && "dir size mismatch");
      for (size_t i=0; i<dirs1.size(); ++i) {
        const Vector3f& d1=dirs1[i];
        const Vector3f& d2=dirs2[i];
        Vector9f a;
        int k=0;
        for (int c=0; c<3; ++c)
          for (int r=0; r<3; ++r, ++k) {
            a[k]=d1(c)*d2(r);
          }
        H.noalias()+=a*a.transpose();
      }
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix<float, 9,9> > eig(H);

      // std::cerr << "H: " << std::endl;
      // std::cerr << H << std::endl;

      // std::cerr << "eigenvectors: " << std::endl;
      // std::cerr << eig.eigenvectors() << std::endl;

      // std::cerr << "eigenvalues: " << std::endl;
      // std::cerr << eig.eigenvalues() << std::endl;
      int k=0;
      for (int c=0; c<3; ++c){
        for (int r=0; r<3; ++r, ++k) {
          dest(r,c)=eig.eigenvectors()(k,0);
        }
      }
      return eig.eigenvalues()(0);
    }

    float estimateTransformFromDirections(Isometry3f& iso,
                                          const Vector3fVector& dirs1,
                                          const Vector3fVector& dirs2) {
      assert(dirs1.size()==dirs2.size() && "dir size mismatch");
      Matrix3f E;
      float lambda=eightPointEstimate(E, dirs1, dirs2);
      Isometry3f iso_v[2];
      essential2iso(iso_v,E);
      Vector3fVector temp_pts(dirs1.size());
      std::vector<float> temp_errors;
      int best_points=0;
      for (int sol=0; sol<2; ++sol) {
        Isometry3f temp_iso=iso_v[sol];
        int temp_good = triangulateDirections(temp_pts,
                                              temp_errors,
                                              Isometry3f::Identity(),
                                              temp_iso,
                                              dirs1, dirs2);
        if (temp_good>best_points) {
          iso=temp_iso;
        }
        temp_iso.translation()=-temp_iso.translation();
        temp_good = triangulateDirections(temp_pts,
                                          temp_errors,
                                          Isometry3f::Identity(),
                                          temp_iso,
                                          dirs1, dirs2);
        if (temp_good>best_points) {
          iso=temp_iso;
        }
      }
      return lambda;
    }

    int scoreEssential(std::vector<float>& errors,
                       const Matrix3f& E,
                       const Vector3fVector& dirs1,
                       const Vector3fVector& dirs2,
                       const float inlier_threshold) {
      int num_inliers=0;
      assert(dirs1.size()==dirs2.size());
      const size_t s=dirs1.size();
      errors.resize(s);
      for (size_t i=0; i<s; ++i) {
        errors[i]=dirs2[i].transpose()*E*dirs1[i];
        if (fabs(errors[i])<inlier_threshold)
          ++num_inliers;
      }
      return num_inliers;
    }

    int estimateEssentialRANSACEightPts(Matrix3f& E,
                                        std::vector<float>& errors,
                                        const Vector3fVector& dirs1,
                                        const Vector3fVector& dirs2,
                                        float inlier_threshold,
                                        int num_rounds,
                                        const std::vector<double>& weights_) {
      assert(dirs1.size()==dirs2.size() && "dir size mismatch");
      size_t s=dirs1.size();
      assert(s>=8 && "not enough correspondences for 8 pt estimate");
      int best_num_inliers=0;
      Vector3fVector dirs1_current(8);
      Vector3fVector dirs2_current(8);
      std::vector<float> errors_current(s);
      std::vector<double> weights(s, 1.f);
      if (weights_.size()) {
        assert(weights.size()==s && "weights size mismatch");
        weights=weights_;
      }
      PermutationSampler sampler(weights);
      for (int round_num=0; round_num<num_rounds; ++round_num) {
        // std::cerr << "RANSAC:, round: " << round_num << "[ ";
        for (size_t i=0; i<8; ++i) {
          double r=drand48()*sampler.getSum();
          int idx=sampler.sampleWithRemoval(r);
          dirs1_current[i]=dirs1[idx];
          dirs2_current[i]=dirs2[idx];
          // std::cerr << idx << " ";
        }
        Matrix3f E_current;
        E_current.setZero();
        eightPointEstimate(E_current, dirs1_current, dirs2_current);
        int current_inliers=scoreEssential(errors_current, E_current, dirs1, dirs2, inlier_threshold);
        // std::cerr << "], inliers: " << current_inliers << std::endl;
        if (current_inliers>best_num_inliers) {
          E=E_current;
          errors=errors_current;
          best_num_inliers=current_inliers;
        }
        sampler.recoverWeights();
      }
      return best_num_inliers;
    }
    /*
    void estimateRotation(Matrix3f R,
                          Vector3fVector& dirs1,
                          Vector3fVector& dirs2) {
      assert (dirs1.size()==dirs2.size() &&" dirs size differ");
      assert (dirs1.size()>3 &&" too few points");
      Matrix9f H;
      H.setZero();
      Vector9f b;
      b.setZero();
      Vector9f x;
      x.setZero();
      Matrix3_9f J;
      J.setZero();
      Vector3f e;
      for (size_t i=0; i<dirs1.size(); ++i) {
        const Vector3f& d1=dirs1[i];
        const Vector3f& d2=dirs2[i];
        J.block<3,1>(0,0)=d1.transpose();
        J.block<3,1>(1,3)=d1.transpose();
        J.block<3,1>(2,6)=d1.transpose();
        H+=J.transpose()*J;
        e=-d2;
      }

    }

    float  estimateRotationTwoPts(Matrix3f& R,
                            const Vector3fVector& dirs1,
                            const Vector3fVector& dirs2,
                            const float s_min,
                            const float epsilon) {
      using namespace std;
      R.setIdentity();
      assert (dirs1.size()==dirs2.size() &&" dirs size differ");
      assert (dirs1.size()>= 2 &&" need at least 2 dirs");
      Vector3f v1=dirs1[0].cross(dirs1[1]);
      Vector3f v2=dirs2[0].cross(dirs2[1]);
      R1.col(0)=dirs1[0];
      R2.col(0)=dirs2[0];
      // construct two rotation matrices
      float n1=v1.norm();
      float n2=v2.norm();
      if(n1<s_min || n2<s_min) {
        return -1;
      }
      if (fabs(n1-n2)>epsilon) {
        return -2;
      }
      Matrix9f
      float e=0;
      for (size_t i=0; i<2; ++i) {
        e+=(R*dirs1[i]-dirs2[i]).squaredNorm();
      }
      return e;
    }
    */
  }
}


//
//  cvx_gl_homography.cpp
//  CalibMeMatching
//
//  Created by jimmy on 2017-07-30.
//  Copyright (c) 2017 Nowhere Planet. All rights reserved.
//

#include "cvx_gl_homography.h"
#include <iostream>

// Eigen
#include <Eigen/Geometry>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

using std::cout;
using std::endl;

namespace cvx_gl {

    namespace  {
        // @brief estimate homography from single-image correspondences
        struct PointLineHomographyFunctor
        {
            typedef double Scalar;

            typedef Eigen::VectorXd InputType;
            typedef Eigen::VectorXd ValueType;
            typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> JacobianType;

            enum {
                InputsAtCompileTime = Eigen::Dynamic,
                ValuesAtCompileTime = Eigen::Dynamic
            };

            const vector<Vector2d> src_pts_;
            const vector<Vector2d> dst_pts_;
            const vector<Eigen::ParametrizedLine<double, 2> > src_lines_;
            const vector<Vector2d> dst_line_pts_;

            int m_inputs;
            int m_values;

            PointLineHomographyFunctor(const vector<Vector2d> & src_pts,
                                       const vector<Vector2d> & dst_pts,
                                       const vector<Eigen::ParametrizedLine<double, 2> > & src_lines,
                                       const vector<Vector2d> & dst_line_pts):
            src_pts_(src_pts),
            dst_pts_(dst_pts),
            src_lines_(src_lines),
            dst_line_pts_(dst_line_pts)

            {
                m_inputs = 9;
                m_values = (int)src_pts.size() * 2 + (int)src_lines.size();
                assert(m_values >= 9);
            }

            int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fx) const
            {
                Eigen::Matrix3d h;
                for (int i = 0; i<9; i++) {
                    h(i/3, i%3) = x[i];
                }
                h.normalize();
                Eigen::Matrix3d inv_h = h.inverse();

                // point to point correspondence
                int idx = 0;
                for (int i = 0; i<src_pts_.size(); i++) {
                    Eigen::Vector3d p(src_pts_[i].x(), src_pts_[i].y(), 1.0);
                    Eigen::Vector3d q = h * p;
                    assert(q.z() != 0);
                    q /= q.z();

                    fx[idx++] = dst_pts_[i].x() - q.x();
                    fx[idx++] = dst_pts_[i].y() - q.y();
                }

                // point to line distance, from destination to source
                for (int i = 0; i<src_lines_.size(); i++) {
                    Eigen::Vector3d p(dst_line_pts_[i].x(), dst_line_pts_[i].y(), 1.0);
                    Eigen::Vector3d q = inv_h * p;
                    assert(q.z() != 0);
                    q /= q.z();

                    double dist = src_lines_[i].distance(Eigen::Vector2d(q.x(), q.y()));
                    fx[idx++] = dist;
                }
                return 0;
            }

            int inputs() const { return m_inputs; }  // inputs is the dimension of x.
            int values() const { return m_values; }   // "values" is the number of fx and

            void getResult(const Eigen::VectorXd &x, Eigen::Matrix3d & result) const
            {
                for (int i = 0; i<9; i++) {
                    result(i/3, i%3) = x[i];
                }
                result.normalize();
            }
        };
    }

    bool estimateHomography(const vector<Vector2d> & src_pts,
                            const vector<Vector2d> & dst_pts,
                            const vector<Eigen::ParametrizedLine<double, 2> > & src_lines,
                            const vector<Vector2d> & dst_line_pts,
                            const Eigen::Matrix3d& init_homo,
                            Eigen::Matrix3d& final_homo)
    {
        assert(src_pts.size() == dst_pts.size());
        assert(src_lines.size() == dst_line_pts.size());

        int num = (int)src_pts.size() * 2 + (int)src_lines.size();
        if (num <= 9) {
            return false;
        }

        PointLineHomographyFunctor opt_functor(src_pts, dst_pts, src_lines, dst_line_pts);
        Eigen::NumericalDiff<PointLineHomographyFunctor> numerical_dif_functor(opt_functor);
        Eigen::LevenbergMarquardt<Eigen::NumericalDiff<PointLineHomographyFunctor>, double> lm(numerical_dif_functor);
        lm.parameters.ftol = 1e-6;
        lm.parameters.xtol = 1e-6;
        lm.parameters.maxfev = 100;

        Eigen::VectorXd x(9);
        for (int i = 0; i<9; i++) {
            x[i] = init_homo(i/3, i%3);
        }

        Eigen::LevenbergMarquardtSpace::Status status = lm.minimize(x);
        std::cout << "status: " << status << std::endl;

        opt_functor.getResult(x, final_homo);

        return true;
    }

    namespace {

        struct MultiFramePointLineHomographyFunctor {
            typedef double Scalar;
            typedef Eigen::VectorXd InputType;
            typedef Eigen::VectorXd ValueType;
            typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> JacobianType;

            enum {
                InputsAtCompileTime = Eigen::Dynamic,
                ValuesAtCompileTime = Eigen::Dynamic
            };

            const vector<vector<Vector2d> > src_pts_;
            const vector<vector<Vector2d> > dst_pts_;
            const vector<vector<Eigen::ParametrizedLine<double, 2> > > src_lines_;
            const vector<vector<Vector2d> > dst_line_pts_;
            const vector<Eigen::Matrix3d> model_to_image_homos_;

            int m_inputs;
            int m_values;

            MultiFramePointLineHomographyFunctor(const vector<vector<Vector2d> > & src_pts,
                                                 const vector<vector<Vector2d> > & dst_pts,
                                                 const vector<vector<Eigen::ParametrizedLine<double, 2> > > & src_lines,
                                                 const vector<vector<Vector2d> > & dst_line_pts,
                                                 const vector<Eigen::Matrix3d>& model_to_image_homos):
            src_pts_(src_pts),
            dst_pts_(dst_pts),
            src_lines_(src_lines),
            dst_line_pts_(dst_line_pts),
            model_to_image_homos_(model_to_image_homos)
            {
                m_inputs = 9;
                m_values = 0;//(int)src_pts.size() * 2 + (int)src_lines.size();
                assert(src_pts_.size() == dst_pts_.size());
                assert(src_pts_.size() == src_lines_.size());
                assert(src_lines_.size() == dst_line_pts_.size());
                assert(src_pts_.size() == model_to_image_homos_.size());

                // point-to-point provides 2 constraint
                // point-on-line provides 1 constraint
                for (int i = 0; i<src_pts_.size(); i++) {
                    m_values += (int)src_pts_[i].size() * 2 + (int)src_lines_[i].size();
                }
                assert(m_values >= 9);
            }

            int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fx) const
            {
                Eigen::Matrix3d h;
                for (int i = 0; i<9; i++) {
                    h(i/3, i%3) = x[i];
                }
                h.normalize();

                // point to point correspondence
                int idx = 0;
                for (int i = 0; i<src_pts_.size(); i++) {
                    // source image --> model --> destination image
                    Eigen::Matrix3d cur_h = h * model_to_image_homos_[i].inverse();
                    for (int j = 0; j<src_pts_[i].size(); j++) {
                        Eigen::Vector3d p(src_pts_[i][j].x(), src_pts_[i][j].y(), 1.0);
                        Eigen::Vector3d q = cur_h * p;
                        assert(q.z() != 0);
                        q /= q.z();

                        fx[idx++] = dst_pts_[i][j].x() - q.x();
                        fx[idx++] = dst_pts_[i][j].y() - q.y();
                    }
                }

                // point to line distance, from destination to source
                for (int i = 0; i<src_lines_.size(); i++) {
                    // source image --> model --> destination image
                    // note from destination to source
                    Eigen::Matrix3d cur_h_inv = (h * model_to_image_homos_[i].inverse()).inverse();
                    for (int j = 0; j<src_lines_[i].size(); j++) {
                        Eigen::Vector3d p(dst_line_pts_[i][j].x(), dst_line_pts_[i][j].y(), 1.0);
                        Eigen::Vector3d q = cur_h_inv * p;
                        assert(q.z() != 0);
                        q /= q.z();

                        double dist = src_lines_[i][j].distance(Eigen::Vector2d(q.x(), q.y()));
                        fx[idx++] = dist;
                    }
                }
                return 0;
            }

            int inputs() const { return m_inputs; }  // inputs is the dimension of x.
            int values() const { return m_values; }   // "values" is the number of fx and

            void getResult(const Eigen::VectorXd &x, Eigen::Matrix3d & result) const
            {
                for (int i = 0; i<9; i++) {
                    result(i/3, i%3) = x[i];
                }
                result.normalize();
            }
        };
    }

    bool estimateHomography(const vector<vector<Vector2d> > & src_pts,
                            const vector<vector<Vector2d> > & dst_pts,
                            const vector<vector<Eigen::ParametrizedLine<double, 2> > > & src_lines,
                            const vector<vector<Vector2d> > & dst_line_pts,
                            const vector<Eigen::Matrix3d>& model_to_image_homos,
                            const Eigen::Matrix3d& init_homo,
                            Eigen::Matrix3d& refined_homo)
    {
        assert(src_pts.size() == dst_pts.size());
        assert(src_lines.size() == dst_line_pts.size());

        int num = 0;
        for (int i = 0; i<src_pts.size(); i++) {
            num += (int)src_pts[i].size() * 2 + (int)src_lines[i].size();
        }
        if (num <= 9) {
            return false;
        }

        MultiFramePointLineHomographyFunctor opt_functor(src_pts, dst_pts, src_lines, dst_line_pts, model_to_image_homos);
        Eigen::NumericalDiff<MultiFramePointLineHomographyFunctor> numerical_dif_functor(opt_functor);
        Eigen::LevenbergMarquardt<Eigen::NumericalDiff<MultiFramePointLineHomographyFunctor>, double> lm(numerical_dif_functor);
        lm.parameters.ftol = 1e-6;
        lm.parameters.xtol = 1e-6;
        lm.parameters.maxfev = 100;

        Eigen::VectorXd x(9);
        for (int i = 0; i<9; i++) {
            x[i] = init_homo(i/3, i%3);
        }

        Eigen::LevenbergMarquardtSpace::Status status = lm.minimize(x);
        std::cout << "status: " << status << std::endl;

        opt_functor.getResult(x, refined_homo);
        return true;
    }

} // namespace