// Copyright Christopher Kormanyos 2013.
// Copyright Paul A. Bristow 2013.
// Copyright John Maddock 2013.

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

#ifdef _MSC_VER
#  pragma warning (disable : 4512) // assignment operator could not be generated.
#  pragma warning (disable : 4996) // assignment operator could not be generated.
#endif

#include <iostream>
#include <limits>
#include <vector>
#include <algorithm>
#include <iomanip>
#include <iterator>

// Weisstein, Eric W. "Bessel Function Zeros." From MathWorld--A Wolfram Web Resource.
// http://mathworld.wolfram.com/BesselFunctionZeros.html
// Test values can be calculated using [@wolframalpha.com WolframAplha]
// See also http://dlmf.nist.gov/10.21

//[bessel_zeros_example_1

/*`This example demonstrates calculating zeros of the Bessel and Neumann functions.
It also shows how Boost.Math and Boost.Multiprecision can be combined to provide
a many decimal digit precision. For 50 decimal digit precision we need to include
*/

  #include <boost/multiprecision/cpp_dec_float.hpp>

/*`and a `typedef` for `float_type` may be convenient
(allowing a quick switch to re-compute at built-in `double` or other precision)
*/
  typedef boost::multiprecision::cpp_dec_float_50 float_type;

//`To use the functions for finding zeros of the functions we need

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

//`This file includes the forward declaration signatures for the zero-finding functions:

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

/*`but more details are in the full documentation, for example at
[@http://www.boost.org/doc/libs/1_53_0/libs/math/doc/sf_and_dist/html/math_toolkit/special/bessel/bessel_over.html Boost.Math Bessel functions].
*/

/*`This example shows obtaining both a single zero of the Bessel function,
and then placing multiple zeros into a container like `std::vector` by providing an iterator.
*/
//] [/bessel_zeros_example_1]

/*The signature of the single value function is:

  template <class T>
  inline typename detail::bessel_traits<T, T, policies::policy<> >::result_type
    cyl_bessel_j_zero(
           T v,      // Floating-point value for Jv.
           int m);   // start index.

The result type is controlled by the floating-point type of parameter `v`
(but subject to the usual __precision_policy and __promotion_policy).

The signature of multiple zeros function is:

  template <class T, class OutputIterator>
  inline OutputIterator cyl_bessel_j_zero(
                                T v,                      // Floating-point value for Jv.
                                int start_index,          // 1-based start index.
                                unsigned number_of_zeros, // How many zeros to generate
                                OutputIterator out_it);   // Destination for zeros.

There is also a version which allows control of the __policy_section for error handling and precision.

  template <class T, class OutputIterator, class Policy>
  inline OutputIterator cyl_bessel_j_zero(
                                T v,                      // Floating-point value for Jv.
                                int start_index,          // 1-based start index.
                                unsigned number_of_zeros, // How many zeros to generate
                                OutputIterator out_it,    // Destination for zeros.
                                const Policy& pol);       // Policy to use.
*/

int main()
{
  try
  {
//[bessel_zeros_example_2

/*`[tip It is always wise to place code using Boost.Math inside try'n'catch blocks;
this will ensure that helpful error messages are shown when exceptional conditions arise.]

First, evaluate a single Bessel zero.

The precision is controlled by the float-point type of template parameter `T` of `v`
so this example has `double` precision, at least 15 but up to 17 decimal digits (for the common 64-bit double).
*/
//    double root = boost::math::cyl_bessel_j_zero(0.0, 1);
//    // Displaying with default precision of 6 decimal digits:
//    std::cout << "boost::math::cyl_bessel_j_zero(0.0, 1) " << root << std::endl; // 2.40483
//    // And with all the guaranteed (15) digits:
//    std::cout.precision(std::numeric_limits<double>::digits10);
//    std::cout << "boost::math::cyl_bessel_j_zero(0.0, 1) " << root << std::endl; // 2.40482555769577
/*`But note that because the parameter `v` controls the precision of the result,
`v` [*must be a floating-point type].
So if you provide an integer type, say 0, rather than 0.0, then it will fail to compile thus:
``
    root = boost::math::cyl_bessel_j_zero(0, 1);
``
with this error message
``
  error C2338: Order must be a floating-point type.
``

Optionally, we can use a policy to ignore errors, C-style, returning some value,
perhaps infinity or NaN, or the best that can be done. (See __user_error_handling).

To create a (possibly unwise!) policy `ignore_all_policy` that ignores all errors:
*/

  typedef boost::math::policies::policy<
    boost::math::policies::domain_error<boost::math::policies::ignore_error>,
    boost::math::policies::overflow_error<boost::math::policies::ignore_error>,
    boost::math::policies::underflow_error<boost::math::policies::ignore_error>,
    boost::math::policies::denorm_error<boost::math::policies::ignore_error>,
    boost::math::policies::pole_error<boost::math::policies::ignore_error>,
    boost::math::policies::evaluation_error<boost::math::policies::ignore_error>
              > ignore_all_policy;
 //`Examples of use of this `ignore_all_policy` are

    double inf = std::numeric_limits<double>::infinity();
    double nan = std::numeric_limits<double>::quiet_NaN();

    double dodgy_root = boost::math::cyl_bessel_j_zero(-1.0, 1, ignore_all_policy());
    std::cout << "boost::math::cyl_bessel_j_zero(-1.0, 1) " << dodgy_root << std::endl; // 1.#QNAN
    double inf_root = boost::math::cyl_bessel_j_zero(inf, 1, ignore_all_policy());
    std::cout << "boost::math::cyl_bessel_j_zero(inf, 1) " << inf_root << std::endl; // 1.#QNAN
    double nan_root = boost::math::cyl_bessel_j_zero(nan, 1, ignore_all_policy());
    std::cout << "boost::math::cyl_bessel_j_zero(nan, 1) " << nan_root << std::endl; // 1.#QNAN

/*`Another version of `cyl_bessel_j_zero`  allows calculation of multiple zeros with one call,
placing the results in a container, often `std::vector`.
For example, generate and display the first five `double` roots of J[sub v] for integral order 2,
as column ['J[sub 2](x)] in table 1 of
[@ http://mathworld.wolfram.com/BesselFunctionZeros.html Wolfram Bessel Function Zeros].
*/
    unsigned int n_roots = 5U;
    std::vector<double> roots;
    boost::math::cyl_bessel_j_zero(2.0, 1, n_roots, std::back_inserter(roots));
    std::copy(roots.begin(),
              roots.end(),
              std::ostream_iterator<double>(std::cout, "\n"));

/*`Or we can use Boost.Multiprecision to generate 50 decimal digit roots of ['J[sub v]]
for non-integral order `v= 71/19 == 3.736842`, expressed as an exact-integer fraction
to generate the most accurate value possible for all floating-point types.

We set the precision of the output stream, and show trailing zeros to display a fixed 50 decimal digits.
*/
    std::cout.precision(std::numeric_limits<float_type>::digits10); // 50 decimal digits.
    std::cout << std::showpoint << std::endl; // Show trailing zeros.

    float_type x = float_type(71) / 19;
    float_type r = boost::math::cyl_bessel_j_zero(x, 1); // 1st root.
    std::cout << "x = " << x << ", r = " << r << std::endl;

    r = boost::math::cyl_bessel_j_zero(x, 20U); // 20th root.
    std::cout << "x = " << x << ", r = " << r << std::endl;

    std::vector<float_type> zeros;
    boost::math::cyl_bessel_j_zero(x, 1, 3, std::back_inserter(zeros));

    std::cout << "cyl_bessel_j_zeros" << std::endl;
    // Print the roots to the output stream.
    std::copy(zeros.begin(), zeros.end(),
              std::ostream_iterator<float_type>(std::cout, "\n"));
//] [/bessel_zeros_example_2]
  }
  catch (std::exception const& ex)
  {
    std::cout << "Thrown exception " << ex.what() << std::endl;
  }

 } // int main()

 /*

 Output:

   Description: Autorun "J:\Cpp\big_number\Debug\bessel_zeros_example_1.exe"
  boost::math::cyl_bessel_j_zero(-1.0, 1) 3.83171
  boost::math::cyl_bessel_j_zero(inf, 1) 1.#QNAN
  boost::math::cyl_bessel_j_zero(nan, 1) 1.#QNAN
  5.13562
  8.41724
  11.6198
  14.796
  17.9598

  x = 3.7368421052631578947368421052631578947368421052632, r = 7.2731751938316489503185694262290765588963196701623
  x = 3.7368421052631578947368421052631578947368421052632, r = 67.815145619696290925556791375555951165111460585458
  cyl_bessel_j_zeros
  7.2731751938316489503185694262290765588963196701623
  10.724858308883141732536172745851416647110749599085
  14.018504599452388106120459558042660282427471931581

*/


//
//  Functions.h
//  Functor classes for activation and activation derivative functions.
//

#include <Eigen/Dense>
#include <cmath>

#ifndef Cynapse_Functions_h
#define Cynapse_Functions_h

double sigmoid(double theta) {
    return 1.0 / (1.0 + exp(-theta));
}

double sigmoid_derivative(double z) {
    return sigmoid(z) * (1-sigmoid(z));
}

double heaviside(double theta) {
    return (theta >= 0.5 ? 1.0 : 0.0);
}

Eigen::MatrixXd quadratic_cost_derivative(Eigen::MatrixXd output, Eigen::MatrixXd input) {
    return output - input;
}

class FunctionFactory
{
public:
    static std::pair<std::function<double(double)>, std::function<double(double)> >
        create(const std::string& funcName) {

        std::function<double(double)> activation_func;
        std::function<double(double)> activation_deriv;

        if (funcName == "sigmoid") {
            activation_func = std::function<double(double)>(sigmoid);
            activation_deriv = std::function<double(double)>(sigmoid_derivative);
        }
        else if (funcName == "step")
        {
            activation_func = std::function<double(double)>(heaviside);
        }
        else
        {
            throw std::runtime_error("Function " + funcName + " is not supported.");
        }

        return std::pair<std::function<double(double)>, std::function<double(double)> >(activation_func, activation_deriv);
    }
};

#endif


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

#include "gramschmidt.hpp"

using namespace std;
using namespace Eigen;

void test1(const MatrixXd& A)
{
    cout << "======================================" << endl;
    cout << "== Usual way to get null space of A ==" << endl;
    cout << "== A is m x n matrix (m >= n)       ==" << endl;
    cout << "======================================" << endl;

    HouseholderQR<MatrixXd> qr(A);
    MatrixXd R = qr.matrixQR().triangularView<Upper>();
    MatrixXd Q = qr.householderQ();

    cout << "=== A ===" << endl;
    cout << A << endl;
    cout << "=== Q ===" << endl;
    cout << Q << endl;
    cout << "=== R ===" << endl;
    cout << R << endl;

    cout << "=== Q*R ===" << endl;
    cout << Q*R << endl;

    MatrixXd Q1 = Q.block(0,0,A.rows(),A.cols());
    MatrixXd Q2 = Q.block(0,A.cols(),A.rows(),Q.cols()-A.cols());

    cout << "== Q1 ==" << endl;
    cout << Q1 << endl;
    cout << "== Q2 ==" << endl;
    cout << Q2 << endl;

    cout << "============================================" << endl;
    cout << "== Q2 is the basis of the Null Space of A ==" << endl;
    cout << "============================================" << endl;
    cout << "== A^T*Q2 ==" << endl;
    cout << A.transpose()*Q2 << endl;
}

void test2(const MatrixXd& A)
{
    cout << "======================================" << endl;
    cout << "== Usual way to get null space of B ==" << endl;
    cout << "== B is m x n matrix (m <= n)       ==" << endl;
    cout << "======================================" << endl;

    MatrixXd B;
    B = A.transpose();

    HouseholderQR<MatrixXd> qr(B);
    MatrixXd R = qr.matrixQR().triangularView<Upper>();
    MatrixXd Q = qr.householderQ();

    cout << "=== B ===" << endl;
    cout << B << endl;
    cout << "=== Q ===" << endl;
    cout << Q << endl;
    cout << "=== R ===" << endl;
    cout << R << endl;

    cout << "=== Q*R ===" << endl;
    cout << Q*R << endl;

    MatrixXd R1 = R.block(0,0,R.rows(),R.rows());
    MatrixXd R2 = R.block(0,R.rows(),R.rows(),R.cols()-R.rows());

    cout << "== R1 ==" << endl;
    cout << R1 << endl;
    cout << "== R2 ==" << endl;
    cout << R2 << endl;

    cout << "== N is following matrix ==" << endl;
    cout << "=====================" << endl;
    cout << "== | -R1^(-1)*R2 | ==" << endl;
    cout << "== |      I      | ==" << endl;
    cout << "=====================" << endl;
    MatrixXd N = MatrixXd::Zero(B.cols(),B.cols()-B.rows());
    N.block(0,0,B.rows(),B.cols()-B.rows()) = -R1.inverse()*R2;
    N.block(B.rows(),0,B.cols()-B.rows(),B.cols()-B.rows()) = MatrixXd::Identity(B.cols()-B.rows(),B.cols()-B.rows());
    cout << N << endl;
    cout << "==============================" << endl;
    cout << "== N is the Null space of B ==" << endl;
    cout << "==============================" << endl;
    cout << "== B*N ==" << endl;
    cout << B*N << endl;
}

void test3(const MatrixXd& A)
{
    cout << "==========================================" << endl;
    cout << "== Column pivot QR decomposition method ==" << endl;
    cout << "==========================================" << endl;
    MatrixXd B = A.transpose();

    ColPivHouseholderQR<MatrixXd> qr(B);
    MatrixXd Q = qr.matrixQ();
    MatrixXd R = qr.matrixR().triangularView<Upper>();
    MatrixXd P = qr.colsPermutation();

    cout << "=== B ===" << endl;
    cout << B << endl;
    cout << "=== Q ===" << endl;
    cout << Q << endl;
    cout << "=== R ===" << endl;
    cout << R << endl;
    cout << "=== P ===" << endl;
    cout << P << endl;

    cout << "=== Q*R*P^T ===" << endl;
    cout << Q*R*P.transpose() << endl;

    R *= P.transpose();

    MatrixXd R1 = R.block(0,0,R.rows(),R.rows());
    MatrixXd R2 = R.block(0,R.rows(),R.rows(),R.cols()-R.rows());

    cout << "== R1 ==" << endl;
    cout << R1 << endl;
    cout << "== R2 ==" << endl;
    cout << R2 << endl;

    cout << "== N is following matrix ==" << endl;
    cout << "=====================" << endl;
    cout << "== | -R1^(-1)*R2 | ==" << endl;
    cout << "== |      I      | ==" << endl;
    cout << "=====================" << endl;
    MatrixXd N = MatrixXd::Zero(B.cols(),B.cols()-B.rows());
    N.block(0,0,B.rows(),B.cols()-B.rows()) = -R1.inverse()*R2;
    N.block(B.rows(),0,B.cols()-B.rows(),B.cols()-B.rows()) = MatrixXd::Identity(B.cols()-B.rows(),B.cols()-B.rows());
    cout << N << endl;
    cout << "==============================" << endl;
    cout << "== N is the Null space of B ==" << endl;
    cout << "==============================" << endl;
    cout << "== B*N ==" << endl;
    cout << B*N << endl;

}

void test4(const MatrixXd& A)
{
    cout << "========================================" << endl;
    cout << "== Full pivot QR decomposition method ==" << endl;
    cout << "========================================" << endl;
    MatrixXd B = A.transpose();

    FullPivHouseholderQR<MatrixXd> qr(B);
    MatrixXd Q = qr.matrixQ();
    MatrixXd R = qr.matrixQR().triangularView<Upper>();
    MatrixXd P = qr.colsPermutation();

    cout << "=== B ===" << endl;
    cout << B << endl;
    cout << "=== Q ===" << endl;
    cout << Q << endl;
    cout << "=== R ===" << endl;
    cout << R << endl;
    cout << "=== P ===" << endl;
    cout << P << endl;

    cout << "=== Q*R*P^T ===" << endl;
    cout << Q*R*P.transpose() << endl;

    R *= P.transpose();

    MatrixXd R1 = R.block(0,0,R.rows(),R.rows());
    MatrixXd R2 = R.block(0,R.rows(),R.rows(),R.cols()-R.rows());

    cout << "== R1 ==" << endl;
    cout << R1 << endl;
    cout << "== R2 ==" << endl;
    cout << R2 << endl;

    cout << "== N is following matrix ==" << endl;
    cout << "=====================" << endl;
    cout << "== | -R1^(-1)*R2 | ==" << endl;
    cout << "== |      I      | ==" << endl;
    cout << "=====================" << endl;
    MatrixXd N = MatrixXd::Zero(B.cols(),B.cols()-B.rows());
    N.block(0,0,B.rows(),B.cols()-B.rows()) = -R1.inverse()*R2;
    N.block(B.rows(),0,B.cols()-B.rows(),B.cols()-B.rows()) = MatrixXd::Identity(B.cols()-B.rows(),B.cols()-B.rows());
    cout << N << endl;
    cout << "==============================" << endl;
    cout << "== N is the Null space of B ==" << endl;
    cout << "==============================" << endl;
    cout << "== B*N ==" << endl;
    cout << B*N << endl;
}

int main(void)
{
    // Set Matrix
    MatrixXd A(6,4);
    A <<
        1, -2, 4, -8,
        2, -1, 1, -1,
        3, 2, 0, 3,
        4, 1, 1, 1,
        7, 2, 4, 8,
        10, -4, 9, -2;

    test1(A);
    test2(A);
    test3(A);
    test4(A);

    return 0;
}


#include <sm/kinematics/rotations.hpp>
#include <sm/assert_macros.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/round.hpp>

namespace sm { namespace kinematics {

  // Euler angle rotations.
  Eigen::Matrix3d Rx(double radians){
    Eigen::Matrix3d C;
    double c = cos(radians);
    double s = sin(radians);
    C(0,0) = 1;   C(0,1) = 0.0; C(0,2) =  0;
    C(1,0) = 0.0; C(1,1) = c;   C(1,2) = -s;
    C(2,0) = 0.0; C(2,1) = s;   C(2,2) =  c;

    return C;
  }

  Eigen::Matrix3d Ry(double radians){
    Eigen::Matrix3d C;
    double c = cos(radians);
    double s = sin(radians);
    C(0,0) =  c;   C(0,1) = 0.0; C(0,2) =  s;
    C(1,0) =  0.0; C(1,1) = 1;   C(1,2) = 0.0;
    C(2,0) = -s;   C(2,1) = 0.0; C(2,2) =  c;

    return C;
  }
  Eigen::Matrix3d Rz(double radians){
    Eigen::Matrix3d C;
    double c = cos(radians);
    double s = sin(radians);
    C(0,0) = c;   C(0,1) = -s;  C(0,2) = 0.0;
    C(1,0) = s;   C(1,1) =  c;  C(1,2) = 0.0;
    C(2,0) = 0.0; C(2,1) = 0.0; C(2,2) =  1;

    return C;
  }

  Eigen::Matrix3d rph2R(double x, double y, double z){
    Eigen::Matrix3d C;
    double cx = cos(x);
    double sx = sin(x);
    double cy = cos(y);
    double sy = sin(y);
    double cz = cos(z);
    double sz = sin(z);
    //[cos(z)*cos(y), -sin(z)*cos(x)+cos(z)*sin(y)*sin(x),  sin(z)*sin(x)+cos(z)*sin(y)*cos(x)]
    //[sin(z)*cos(y),  cos(z)*cos(x)+sin(z)*sin(y)*sin(x), -cos(z)*sin(x)+sin(z)*sin(y)*cos(x)]
    //[      -sin(y),                       cos(y)*sin(x),                       cos(y)*cos(x)]
    C(0,0) = cz*cy; C(0,1) = -sz*cx+cz*sy*sx; C(0,2) =   sz*sx+cz*sy*cx;
    C(1,0) = sz*cy; C(1,1) =  cz*cx+sz*sy*sx; C(1,2) =  -cz*sx+sz*sy*cx;
    C(2,0) = -sy;   C(2,1) =           cy*sx; C(2,2) =            cy*cx;

    return C;
  }
  Eigen::Matrix3d rph2R(Eigen::Vector3d const & x){
    return rph2R(x[0],x[1],x[2]);
  }

  Eigen::Vector3d R2rph(Eigen::Matrix3d const & C){
    double phi = asin(C(2,0));
    double theta = atan2(C(2,1),C(2,2));
    double psi = atan2(C(1,0), C(0,0));

    Eigen::Vector3d ret;
    ret[0] = theta;
    ret[1] = -phi;
    ret[2] = psi;

    return ret;
  }

  //// Small angle approximation.
  template <typename Scalar_>
  Eigen::Matrix<Scalar_, 3, 3> crossMx(Scalar_ x, Scalar_ y, Scalar_ z){
    Eigen::Matrix<Scalar_, 3, 3> C;
    C(0,0) =  0.0; C(0,1) = -z;   C(0,2) =   y;
    C(1,0) =  z;   C(1,1) =  0.0; C(1,2) =  -x;
    C(2,0) = -y;   C(2,1) =  x;   C(2,2) =   0.0;
    return C;
  }
  template Eigen::Matrix<double, 3, 3> crossMx(double x, double y, double z);
  template Eigen::Matrix<float, 3, 3> crossMx(float x, float y, float z);

  template Eigen::Matrix<double, 3, 3> crossMx(Eigen::MatrixBase<Eigen::Matrix<double, 3, 1> > const &);
  template Eigen::Matrix<float, 3, 3> crossMx(Eigen::MatrixBase<Eigen::Matrix<float, 3, 1> > const &);

  template Eigen::Matrix<double, 3, 3> crossMx(Eigen::MatrixBase<Eigen::Matrix<double, Eigen::Dynamic, 1> > const &);
  template Eigen::Matrix<float, 3, 3> crossMx(Eigen::MatrixBase<Eigen::Matrix<float, Eigen::Dynamic, 1> > const &);

  // Axis Angle rotation.
  Eigen::Matrix3d axisAngle2R(double a, double ax, double ay, double az){

    SM_ASSERT_LT_DBG(std::runtime_error,fabs(sqrt(ax*ax + ay*ay + az*az) - 1.0), 1e-4, "The axis is not a unit vector. ||a|| = " << (sqrt(ax*ax + ay*ay + az*az)));

    if(a < 1e-12)
      return Eigen::Matrix3d::Identity();
    // e = [ax ay az]
    // e*(e') + (eye(3) - e*(e'))*cos(a) - crossMx(e) * sin(a) =
    //[         ax^2+ca*(1-ax^2), ax*ay-ca*ax*ay+sa*az, ax*az-ca*ax*az-sa*ay]
    //[ ax*ay-ca*ax*ay-sa*az,         ay^2+ca*(1-ay^2), ay*az-ca*ay*az+sa*ax]
    //[ ax*az-ca*ax*az+sa*ay, ay*az-ca*ay*az-sa*ax,         az^2+ca*(1-az^2)]
    double sa = sin(a);
    double ca = cos(a);
    double ax2 = ax*ax;
    double ay2 = ay*ay;
    double az2 = az*az;
    double const one = double(1);

    Eigen::Matrix3d C;
    C(0,0) =  ax2+ca*(one-ax2);       C(0,1) = ax*ay-ca*ax*ay+sa*az; C(0,2) = ax*az-ca*ax*az-sa*ay;
    C(1,0) =  ax*ay-ca*ax*ay-sa*az; C(1,1) = ay2+ca*(one-ay2);       C(1,2) = ay*az-ca*ay*az+sa*ax;
    C(2,0) =  ax*az-ca*ax*az+sa*ay; C(2,1) = ay*az-ca*ay*az-sa*ax; C(2,2) = az2+ca*(one-az2);

    return C;
  }
  Eigen::Matrix3d axisAngle2R(double x, double y, double z) {
    double a = sqrt(x*x + y*y + z*z);
    if(a < 1e-12)
      return Eigen::Matrix3d::Identity();

    double d = 1/a;
    return axisAngle2R(a,x*d, y*d, z*d);
  }

  Eigen::Matrix3d axisAngle2R(Eigen::Vector3d const & x){
    return axisAngle2R(x[0], x[1], x[2]);
  }

  Eigen::Vector3d R2AxisAngle(Eigen::Matrix3d const & C){
    // Sometimes, because of roundoff error, the value of tr ends up outside
    // the valid range of arccos. Truncate to the valid range.
    double tr = std::max(-1.0, std::min( (C(0,0) + C(1,1) + C(2,2) - 1.0) * 0.5, 1.0));
    double a = acos( tr ) ;

    Eigen::Vector3d axis;

    if(fabs(a) < 1e-10) {
      return Eigen::Vector3d::Zero();
    }

    axis[0] = (C(2,1) - C(1,2));
    axis[1] = (C(0,2) - C(2,0));
    axis[2] = (C(1,0) - C(0,1));
    double n2 = axis.norm();
    if(fabs(n2) < 1e-10)
      return Eigen::Vector3d::Zero();

    double scale = -a/n2;
    axis = scale * axis;

    return axis;

  }

  // Utility functions
  double angleMod(double radians){
    return (double)(radians - (SM_2PI * boost::math::round(radians / SM_2PI)));
  }
  double deg2rad(double degrees){
    return (double)(degrees * SM_DEG2RAD);
  }
  double rad2deg(double radians){
    return (double)(radians * SM_RAD2DEG);
  }

  Eigen::Matrix3d Cx(double radians)
  {
    return Rx(-radians);
  }
  Eigen::Matrix3d Cy(double radians)
  {
    return Ry(-radians);
  }
  Eigen::Matrix3d Cz(double radians)
  {
    return Rz(-radians);
  }

   Eigen::Matrix3d rph2C(double x, double y, double z)
   {
     return rph2R(-x,-y,-z);
   }

   Eigen::Matrix3d rph2C(Eigen::Vector3d const & x)
   {
     return rph2C(x[0],x[1],x[2]);
   }
   Eigen::Matrix3d rph2C(Eigen::VectorXd const & x)
   {
     SM_ASSERT_EQ_DBG(std::runtime_error,x.size(),3,"The input vector must have 3 components");
     return rph2C(x[0],x[1],x[2]);
   }

   Eigen::Matrix3d rph2C(Eigen::MatrixXd const & A, unsigned column)
   {
     SM_ASSERT_EQ_DBG(std::runtime_error,A.rows(),3,"The input matrix must have 3 rows");
     SM_ASSERT_LT_DBG(std::runtime_error,column,A.cols(),"The requested column is out of bounds");
     return rph2C(A(0,column),A(1,column),A(2,column));
   }

   Eigen::Vector3d C2rph(Eigen::MatrixXd const & C)
   {
     SM_ASSERT_EQ_DBG(std::runtime_error,C.rows(),3,"The input matrix must be 3x3");
     SM_ASSERT_EQ_DBG(std::runtime_error,C.cols(),3,"The input matrix must be 3x3");

     Eigen::Vector3d rph;

     rph[1] = asin(C(2,0));
     rph[2] = atan2(-C(1,0),C(0,0));
     rph[0] = atan2(-C(2,1),C(2,2));

     return rph;
   }

   Eigen::Vector3d C2rph(Eigen::Matrix3d const & C)
   {
     Eigen::Vector3d rph;

     rph[1] = asin(C(2,0));
     rph[2] = atan2(-C(1,0),C(0,0));
     rph[0] = atan2(-C(2,1),C(2,2));

     return rph;
   }

  }} // sm::kinematics


#include <iostream>
#include <cmath>

using namespace std;

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

using namespace Eigen;

// This program demonstrates how to use the Eigen geometry module

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

  // The Eigen/Geometry module provides a variety of rotation and translation representations
  // 3D rotation matrix directly using Matrix3d or Matrix3f
  Matrix3d rotation_matrix = Matrix3d::Identity();
  // The rotation vector uses AngleAxis, the underlying layer is not directly Matrix,
  // but the operation can be treated as a matrix (because the operator is overloaded)
  AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1)); // Rotate 45 degrees along the Z axis
  cout.precision(3);
  cout << "rotation matrix =\n" << rotation_vector.matrix() << endl; // convert to matrix with matrix()
  // can also be assigned directly
  rotation_matrix = rotation_vector.toRotationMatrix();
  // coordinate transformation with AngleAxis
  Vector3d v(1, 0, 0);
  Vector3d v_rotated = rotation_vector * v;
  cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
  // Or use a rotation matrix
  v_rotated = rotation_matrix * v;
  cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;

  // Euler angle: You can convert the rotation matrix directly into Euler angles
  Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX order, ie roll pitch yaw order
  cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

  // Euclidean transformation matrix using Eigen::Isometry
  Isometry3d T = Isometry3d::Identity(); // Although called 3d, it is essentially a 4∗4 matrix
  T.rotate(rotation_vector);             // Rotate according to rotation_vector
  T.pretranslate(Vector3d(1, 3, 4));     // Set the translation vector to (1,3,4)
  cout << "Transform matrix = \n" << T.matrix() << endl;

  // Use the transformation matrix for coordinate transformation
  Vector3d v_transformed = T * v; // Equivalent to R∗v+t
  cout << "v tranformed = " << v_transformed.transpose() << endl;

  // For affine and projective transformations, use Eigen::Affine3d and Eigen::Projective3d.

  // Quaternion
  // You can assign AngleAxis directly to quaternions, and vice versa
  Quaterniond q = Quaterniond(rotation_vector);
  cout << "quaternion from rotation vector = " << q.coeffs().transpose()
       << endl; // Note that the order of coeffs is (x, y, z, w), w is the real part, the first three are the imaginary part
  // can also assign a rotation matrix to it
  q = Quaterniond(rotation_matrix);
  cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
  // Rotate a vector with a quaternion and use overloaded multiplication
  v_rotated = q * v; // Note that the math is qvq^{−1}
  cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
  // expressed by regular vector multiplication, it should be calculated as follows
  cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

  return 0;
}


#include <iostream>
#include <cmath>
using namespace std;

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

/****************************
* This program demonstrates how to use Eigen geometry module
****************************/

int main ( int argc, char** argv )
{
    // Eigen/Geometry The module provides various representations of rotation and translation
    // 3D rotation matrix directly use Matrix3d or Matrix3f
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // Rotation vector uses AngleAxis, its underlying layer is not directly Matrix,
    // but the operation can be treated as a matrix (because the operator is overloaded)
    //Rotate 45 degrees along the Z axis
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );
    cout .precision(3);
    //Use matrix() to convert to a matrix
    cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;
    // You can also assign directly
    rotation_matrix = rotation_vector.toRotationMatrix();
    // Coordinate transformation can be performed with AngleAxis
    Eigen::Vector3d v ( 1,0,0 );
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    // Or use the rotation matrix
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    // Euler angle: The rotation matrix can be directly converted into Euler angle
    // ZYX order, that is, roll pitch yaw order
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 );
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    // Euclidean transformation matrix uses Eigen::Isometry
    // Although it is called 3d, it is essentially a 4*4 matrix
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();
    // Rotate according to rotation_vector
    T.rotate ( rotation_vector );
    // Set the translation vector to (1,3,4)
    T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );
    cout << "Transform matrix = \n" << T.matrix() <<endl;

    // Use transformation matrix for coordinate transformation
    // equivalent to R*v+t
    Eigen::Vector3d v_transformed = T*v;
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

    // For affine and projective transformations, use Eigen::Affine3d and Eigen::Projective3d.
    // Quaternion
    // You can directly assign AngleAxis to a quaternion, and vice versa
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    // Please note that the order of coeffs is (x,y,z,w), w is the real part, and
    // the first three are the imaginary part
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // You can also assign the rotation matrix to it
    q = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // Use a quaternion to rotate a vector and use overloaded multiplication
    // Note that mathematically it is qvq^{-1}
    v_rotated = q*v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    return 0;
}


// #include "mex.h"
// #include "matrix.h"
#include "mxarray.h"
#include <mex.h>

#include <Eigen/Eigen>

#include <range/v3/all.hpp>
// #include <range/v3/core.hpp>

#include <iostream>
#include <vector>
// #include "matrix.h"
#include "tbb/parallel_for.h"
#include <omp.h>
// #include <tbb/tbb.h>
#include <typeinfo>
// #include <eigen3/Eigen/CXX11/Tensor>
// #include <unsupported/eigen3/Eigen/CXX11/Tensor>
// #include <Eigen/src/Core/util/DisableStupidWarnings.h>
// #include <Eigen/Tensor>
#include "storage_aliases.h"
#include <unsupported/Eigen/CXX11/Tensor>

// #define ngp int(*mxGetPr(prhs[0]))
#define input prhs[0]
#define output plhs[0]

typedef Eigen::Map<Eigen::MatrixXd> MexMat;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  if (nrhs != 2) {
    mexErrMsgTxt("Needs 2 arguments -- X (NxD), Y (DxM)");
    return;
  }

  int N = mxGetM(prhs[0]);
  int D = mxGetN(prhs[0]);

  int P = mxGetM(prhs[1]);
  int M = mxGetN(prhs[1]);

  if ((N != P) || (P != M)) {
    mexErrMsgTxt("X #cols needs to match Y #rows!");
    return;
  }

  MexMat U(mxGetPr(prhs[0]), N, D);
  MexMat MM(mxGetPr(prhs[1]), P, M);

  // compute the Cholesky decomposition of A
  Eigen::MatrixXd L =
      MM.llt().matrixL(); // retrieve factor L  in the decomposition
  // << A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b) << endl;

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(
      U, Eigen::DecompositionOptions::ComputeThinU |
             Eigen::DecompositionOptions::ComputeThinV);
  svd.setThreshold(1e-14);

  // Create output matrix (using Matlab's alloc)
  int const number_singular_values = svd.singularValues().size();
  plhs[0] = mxCreateDoubleMatrix(N, number_singular_values, mxREAL);
  plhs[1] = mxCreateDoubleMatrix(number_singular_values, D, mxREAL);
  plhs[2] = mxCreateDoubleMatrix(number_singular_values, number_singular_values,
                                 mxREAL);
  MexMat R(mxGetPr(plhs[0]), N, number_singular_values);
  MexMat V(mxGetPr(plhs[1]), number_singular_values, D);
  MexMat S(mxGetPr(plhs[2]), number_singular_values, number_singular_values);
  // R = L.lu().solve(svd.matrixU());
  R = svd.matrixU();
  S = svd.singularValues().asDiagonal();
  V = svd.matrixV().transpose();
  // Vector3f rhs(1, 0, 0);
  // cout << "Now consider this rhs vector:" << endl << rhs << endl;
  // cout << "A least-squares solution of m*x = rhs is:" << endl <<
  // svd.solve(rhs) << endl;
}


#include <iostream>
#include <armadillo>
#include <random>
#include "common.h"

using arma::mat;
using arma::svd;
using arma::vec;
using std::vector;

mat calculate_pseudoinverse(mat x)
{
    return x.t() * inv(x * x.t());
}

mat calculate_sum_signal(mat weigths, mat signals)
{
    return weigths * signals;
}

mat calculate_svd_inverse(mat x, int rank)
{
    // If rank = -1, use full rank (= smaller dimension of matrix x)
    if (rank == -1)
    {
        rank = std::min(x.n_cols, x.n_rows);
    }

    mat u, v;
    vec s;
    svd(u, s, v, x);
    mat s_inv = 1 / s;

    int lb = s_inv.n_elem - rank;
    int ub = s_inv.n_elem - 1;

    s_inv = s_inv.rows(lb, ub);
    v = v.cols(lb, ub);
    u = u.cols(lb, ub);
    mat sigma_inv = diagmat(s_inv);

    return v * sigma_inv * u.t();
}

mat low_rank_approximation(mat x, int rank)
{
    mat u, v;
    vec s;
    svd(u, s, v, x);

    s = s.rows(0, rank - 1);
    v = v.cols(0, rank - 1);
    u = u.cols(0, rank - 1);

    return u * diagmat(s) * v.t();
}

arma::mat trimmed_mean(arma::vec x, int n_points)
{
    int lb = n_points;
    int ub = x.n_elem - n_points - 1;
    arma::vec x_sorted = arma::sort(x, "ascend");
    return arma::mean(x_sorted.rows(lb, ub), 0);
}

arma::mat trimmed_mean(arma::vec x, float proportion)
{
    int n_points = round(proportion * x.n_elem);
    return trimmed_mean(x, n_points);
}

arma::mat rmse(arma::mat estimate_values, arma::mat true_values)
{
    arma::mat difference = estimate_values - true_values;
    return sqrt(sum(pow(difference, 2), 1) / difference.n_elem);
}

arma::mat mae(arma::mat estimate_values, arma::mat true_values)
{
    arma::mat difference = estimate_values - true_values;
    return arma::sum(arma::abs(difference), 1) / difference.n_elem;
}

arma::mat trimmed_mae(arma::vec estimate_values, arma::vec true_values, double rejection_threshold)
{
    arma::vec difference = estimate_values - true_values;
    arma::vec abs_difference = arma::abs(difference);
    int n_points = round(rejection_threshold * abs_difference.n_elem);
    arma::vec abs_difference_sorted = arma::sort(abs_difference, "descent");
    return arma::mean(abs_difference_sorted.rows(n_points, abs_difference_sorted.n_elem - 1), 0);
}

std::vector<int> sample_without_replacement(int lb, int ub, int n)
{
    std::vector<int> vec;
    for (size_t i = lb; i <= ub; i++)
    {
        vec.push_back(i);
    }
    std::random_device device;
    std::mt19937 generator(device());
    std::shuffle(vec.begin(), vec.end(), generator);
    std::vector<int> out(vec.begin(), vec.begin() + n);
    return out;
}


#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

/// This program demonstrates the basic usage of Sophus

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

  // Rotation matrix with 90 degrees along Z axis
  Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
  // or quaternion
  Quaterniond q(R);
  Sophus::SO3d SO3_R(R);              // Sophus::SO3d can be constructed from rotation matrix
  Sophus::SO3d SO3_q(q);              // or quaternion
  // they are equivalent of course
  cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
  cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl;
  cout << "they are equal" << endl;

  // Use logarithmic map to get the Lie algebra
  Vector3d so3 = SO3_R.log();
  cout << "so3 = " << so3.transpose() << endl;
  // hat is from vector to skew−symmetric matrix
  cout << "so3 hat=\n" << Sophus::SO3d::hat(so3) << endl;
  // inversely from matrix to vector
  cout << "so3 hat vee= " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;

  // update by perturbation model
  Vector3d update_so3(1e-4, 0, 0); // this is a small update
  Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
  cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;

  cout << "*******************************" << endl;
  // Similar for SE(3)
  Vector3d t(1, 0, 0);           // translation 1 along X
  Sophus::SE3d SE3_Rt(R, t);     // construction SE3 from R,t
  Sophus::SE3d SE3_qt(q, t);     // or q,t
  cout << "SE3 from R,t= \n" << SE3_Rt.matrix() << endl;
  cout << "SE3 from q,t= \n" << SE3_qt.matrix() << endl;
  // Lie Algebra is 6d vector, we give a typedef
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  Vector6d se3 = SE3_Rt.log();
  cout << "se3 = " << se3.transpose() << endl;
  // The output shows Sophus puts the translation at first in se(3), then rotation.
  // Save as SO(3) wehave hat and vee
  cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;
  cout << "se3 hat vee = " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;

  // Finally the update
  Vector6d update_se3;
  update_se3.setZero();
  update_se3(0, 0) = 1e-4;
  Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
  cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;

  return 0;
}


#include "BayesC_distributions.h"
#include <Eigen/Core>
using namespace Eigen;

//sampling functions
double sample_mu(int N, double Esigma2,const VectorXd& Y,const MatrixXd& X,const VectorXd& beta)
{
	double mean=((Y-X*beta).sum())/N;
	double sd=sqrt(Esigma2/N);
	double mu=rnorm(mean,sd);
	return(mu);
}

//sample variance of beta
double sample_psi2_chisq(const VectorXd& beta,int NZ,double v0B,double s0B){
	double df=v0B+NZ;
	double scale=(beta.squaredNorm()*NZ+v0B*s0B)/(v0B+NZ);
	double psi2=rinvchisq(df, scale);
	return(psi2);
}

//sample error variance of Y
double sample_sigma_chisq(int N,const VectorXd& epsilon,double v0E,double s0E){
	double sigma2=rinvchisq(v0E+N, (epsilon.squaredNorm()+v0E*s0E)/(v0E+N));
	return(sigma2);
}

//sample mixture weight
double sample_w(int M,int NZ){
	double w=rbeta(1+NZ,1+(M-NZ));
	return(w);
}


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

template <typename FuncType, typename JacType>
void dampnewton(const FuncType &F, const JacType &DF,
                Eigen::VectorXd &x, double rtol = 1e-4,double atol = 1e-6)
{
    const int n = x.size();
    const double lmin = 1E-3; // Minimal damping factor
    double lambda = 1.0; // Initial and actual damping factor
    Eigen::VectorXd s(n),st(n); // Newton corrections
    Eigen::VectorXd xn(n);      // Tentative new iterate
    double sn,stn;    // Norms of Newton corrections

    do {
        auto jacfac = DF(x).lu(); // LU-factorize Jacobian

        s = jacfac.solve(F(x));   // Newton correction
        sn = s.norm();            // Norm of Newton correction
        lambda *= 2.0;
        do {
            lambda /= 2;
            if (lambda < lmin) throw "No convergence: lambda -> 0";
            xn = x-lambda*s;           // {\bf Tentative next iterate}
            st = jacfac.solve(F(xn));  // Simplified Newton correction
            stn = st.norm();
        }
        while (stn > (1-lambda/2)*sn); // {\bf Natural monotonicity test}
        x = xn; // Now: xn accepted as new iterate
        lambda = std::min(2.0*lambda,1.0); // Try to mitigate damping
    }
    // Termination based on simplified Newton correction
    while ((stn > rtol*x.norm()) && (stn > atol));
}

Nummerical Methods for CSE/PS14/solutions_ps14/dampnewton.hpp

Nummerical Methods for CSE/PS14/templates_ps14/dampnewton.hpp


#pragma once

#include <numeric>
#include <type_traits>
#include <utility>
#include <vector>

#include <opencv2/core/core.hpp>
#include <Eigen/Dense>

namespace pcv
{

template<class NumericType>
double getReprojectionError(
    const Eigen::Matrix3d &model,
    const std::pair<cv::Point_<NumericType>, cv::Point_<NumericType>>
        &correspondence)
{
    static_assert(std::is_arithmetic<NumericType>::value,
                  "Must have a numerical point type.");

    // Make the first correspondence a homogenous-coordinate column vector.
    Eigen::Vector3d homogenousFirst;
    homogenousFirst <<
        correspondence.first.x,
        correspondence.first.y,
        1.0;

    Eigen::Vector3d estimate = model * homogenousFirst;
    estimate /= estimate(2);

    return std::sqrt(  std::pow(estimate(0)-correspondence.second.x, 2)
                     + std::pow(estimate(1)-correspondence.second.y, 2));
}

template<class NumericType>
Eigen::Matrix3d
findHomographyWithLeastSquares(
    std::vector<std::pair<
        cv::Point_<NumericType>, cv::Point_<NumericType> >>
        correspondences)
{
    static_assert(std::is_arithmetic<NumericType>::value,
                  "Must have a numerical point type.");

    // Allocate appropriate amount of memory for Matrix.
    Eigen::MatrixXd joinedPoints(2*correspondences.size(), 9);

    for (std::size_t correspondenceIndex = 0; correspondenceIndex < correspondences.size(); ++correspondenceIndex)
    {
        const auto &pointPair = correspondences[correspondenceIndex];

        joinedPoints.row(2*correspondenceIndex) <<
            -pointPair.first.x,
            -pointPair.first.y,
            -1,
            0,
            0,
            0,
            pointPair.first.x*pointPair.second.x,
            pointPair.first.y*pointPair.second.x,
            pointPair.second.x;

        joinedPoints.row(2*correspondenceIndex+1) <<
            0,
            0,
            0,
            -pointPair.first.x,
            -pointPair.first.y,
            -1,
            pointPair.first.x*pointPair.second.y,
            pointPair.first.y*pointPair.second.y,
            pointPair.second.y;
    }

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(
        joinedPoints.transpose()*joinedPoints, Eigen::ComputeFullV);

    auto output =
        Eigen::Matrix<double, 3, 3, Eigen::RowMajor>(svd.matrixV().col(8).data());

    return output / output(2, 2);
}

template<class NumericType>
Eigen::Matrix3d
findHomographyWithDirectLinearTransform(
    std::vector<std::pair<
        cv::Point_<NumericType>, cv::Point_<NumericType> >>
        correspondences)
{
    static_assert(std::is_arithmetic<NumericType>::value,
                  "Must have a numerical point type.");

    Eigen::Matrix<double, 8, 9> joinedPoints;

    std::array<Eigen::Matrix<double, 2, 9>, 4> points;

    std::transform(
        std::cbegin(correspondences),
        std::cend(correspondences),
        std::begin(points),
        [](std::pair<cv::Point2f, cv::Point2f> pointPair)
        {
            Eigen::Matrix<double, 2, 9> tmp;
            tmp <<
                -pointPair.first.x,
                -pointPair.first.y, -1, 0,
                0,
                0,
                pointPair.first.x*pointPair.second.x,
                pointPair.first.y*pointPair.second.x,
                pointPair.second.x,
                0,
                0,
                0,
                -pointPair.first.x,
                -pointPair.first.y,
                -1,
                pointPair.first.x*pointPair.second.y,
                pointPair.first.y*pointPair.second.y,
                pointPair.second.y;

            return tmp;
        });

    joinedPoints << points[0], points[1], points[2], points[3];

    Eigen::JacobiSVD<decltype(joinedPoints)> svd(joinedPoints, Eigen::ComputeFullV);

    auto output =
        Eigen::Matrix<double, 3, 3, Eigen::RowMajor>(svd.matrixV().col(8).data());

    return output / output(2, 2);
}

} // end namespace pcv


#include <iostream>

//#define USING_MLOGD
#include "utility/tool.h"
#include <Eigen/Core>
//#include <Eigen/Geometry>  // Eigen 几何模块

/**
 * @brief       使用Gramy-Schmidt方法实现向量正交化,并单位化
 * @param  [in]  a       向量a
 * @param  [in]  b       向量b
 * @param  [in]  c       向量c
 * @param  [out]  An      标准化后的向量A
 * @param  [out]  Bn      标准化后的向量B
 * @param  [out]  Cn      标准化后的向量C
 * @retval
 * @note  a,b,c必须为线性无关组
 */
static void schmidtOrthogonalV3D(
                        Eigen::Vector3d a,
                        Eigen::Vector3d b,
                        Eigen::Vector3d c,
                        Eigen::Vector3d* An,
                        Eigen::Vector3d* Bn,
                        Eigen::Vector3d* Cn ){

    Eigen::Vector3d A,B,C;
    //A直接赋值为a
    A = a;
    //MLOGD("vector A: %.3lf %.3f %.3f", A.x(), A.y(), A.z() );
    //求B
    double xab = A.dot(b) / A.dot( A );
    B = b - xab * A;
    //MLOGD("vector B: %.3lf %.3f %.3f", B.x(), B.y(), B.z() );
    //求C
    double xac = A.dot( c ) / A.dot( A );
    double xbc = B.dot( c ) / B.dot( B );
    C = c - ( xac * A ) - ( xbc * B );
    //MLOGD("vector C: %.3lf %.3f %.3f", C.x(), C.y(), C.z() );
    //单位化并输出
    *An = A.normalized();
    *Bn = B.normalized();
    *Cn = C.normalized();
    //MLOGD("normalized vector A: %.3lf %.3f %.3f", An->x(), An->y(), An->z() );
    //MLOGD("normalized vector B: %.3lf %.3f %.3f", Bn->x(), Bn->y(), Bn->z() );
    //MLOGD("normalized vector C: %.3lf %.3f %.3f", Cn->x(), Cn->y(), Cn->z() );
}

//--------------------测试函数----------------------------
static void schmidtOrthogonalTest( void ) {
    //线性无关组1
    Eigen::Vector3d a(1,1.2,0);
    Eigen::Vector3d b(1,2,0);
    Eigen::Vector3d c(0,1,1);

    //线性无关组2
    Eigen::Vector3d v1(9,1.2,2.4);
    Eigen::Vector3d v2(1,2,6.7);
    Eigen::Vector3d v3(5,1.5,1);

    Eigen::Vector3d A,B,C;

    schmidtOrthogonalV3D( a, b, c, &A, &B, &C );
    //schmidtOrthogonalV3D( v1, v2, v3, &A, &B, &C );
    TPLOGI("normalized vector A: %.3lf %.3f %.3f", A.x(), A.y(), A.z() );
    TPLOGI("normalized vector B: %.3lf %.3f %.3f", B.x(), B.y(), B.z() );
    TPLOGI("normalized vector C: %.3lf %.3f %.3f", C.x(), C.y(), C.z() );
}


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

using namespace Eigen;
using namespace std;

int main()
{
  MatrixXd m = MatrixXd::Random(3,3);
  m = (m + MatrixXd::Constant(3,3,1.2)) * 50;
  cout << "m =" << endl << m << endl;
  VectorXd v(3);
  v << 1, 2, 3;
  cout << "m * v =" << endl << m * v << endl;
}


#include "tester.hpp"
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <iostream>
#include <cmath>

namespace mp = boost::multiprecision;

using degree_t = unsigned;
using num_t = mp::number<mp::cpp_int_backend<4194304, 4194304, mp::unsigned_magnitude, mp::unchecked, void> >;
using power_t = mp::number<mp::cpp_dec_float<1024>>;

template<typename T>
auto fast_power(T base, degree_t degree, T identity) {
    T result = identity;
    while (degree) {
        if (degree % 2) result *= base;
        base *= base;
        degree /= 2;
    }
    return result;
}

auto fib1(degree_t id) {
    if (id == 0) return 0;
    if (id < 2) return 1;
    return fib1(id - 1) + fib1(id - 2);
}

auto fib2(degree_t id) {
    static const power_t phi = (1 + mp::sqrt(power_t(5.0))) / 2;
    return mp::floor(
            fast_power(phi, id, power_t(1.0)) / mp::sqrt(power_t(5.0)) + 0.5
    ).convert_to<num_t>();
}

struct fib_matrix {
    num_t a11, a12, a21, a22;

    fib_matrix operator*(const fib_matrix &r) const {
        return fib_matrix{
                a11 * r.a11 + a12 * r.a21,
                a11 * r.a12 + a12 * r.a22,
                a21 * r.a11 + a22 * r.a21,
                a21 * r.a12 + a22 * r.a22
        };
    }

    fib_matrix &operator*=(const fib_matrix &r) {
        auto tmp = fib_matrix(*this);
        *this = tmp * r;
        return *this;
    }
};

auto fib3(degree_t id) {
    fib_matrix base{1, 1, 1, 0};
    fib_matrix identity{1, 0, 0, 1};
    auto matrix_power = fast_power(base, id, identity);
    return matrix_power.a12;
}

int main(int argc, const char *argv[]) {
    const auto usage = "test_power <test data path>";
    std::string test_path;
    try {
        if (argc < 2) {
            std::cerr << "Not all required arguments were passed. Program usage:\n" << usage;
            exit(1);
        }
        test_path = argv[1];
    } catch (const std::exception &e) {
        std::cerr << "Invalid argument passed: " << e.what();
        exit(1);
    }

    [[maybe_unused]] auto fibo_recursion = [](tester::test_args_t &args) {
        degree_t fib_id = std::stol(args[0]);
        return fib1(fib_id);
    };

    [[maybe_unused]] auto fibo_golden = [](tester::test_args_t &args) {
        degree_t fib_id = std::stol(args[0]);
        return fib2(fib_id);
    };

    [[maybe_unused]] auto fibo_matrix = [](tester::test_args_t &args) {
        degree_t fib_id = std::stol(args[0]);
        return fib3(fib_id);
    };

    std::cout << "\n Fibo: recursion\n";
    tester::run_tests<decltype(fibo_recursion), num_t>(fibo_recursion, test_path);

    std::cout << "\n Fibo: golden ratio\n";
    tester::run_tests<decltype(fibo_golden), num_t>(fibo_golden, test_path);

    std::cout << "\n Fibo: matrix product\n";
    tester::run_tests<decltype(fibo_matrix), num_t>(fibo_matrix, test_path);
}


#include <armadillo>
#include <iostream>
using namespace arma;
int main()
{
    // arma::mat A(5, 5, arma::fill::randu);
    // arma::mat B(5, 5, arma::fill::randn);
    // std::cout << A * B << std::endl;
    mat X = randu<mat>(5,5);
    mat Y = X.t()*X;
    mat R1 = chol(Y);
    std::cout << R1 << std::endl;
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

void test_log_diff_exp(double a, double b) {
  using std::log;
  using std::exp;
  using stan::math::log_diff_exp;
  EXPECT_FLOAT_EQ(log(exp(a) - exp(b)),
                  log_diff_exp(a,b));
}

TEST(MathFunctions, log_diff_exp) {
  using stan::math::log_diff_exp;
  test_log_diff_exp(3.0,2.0);
  test_log_diff_exp(4.0,1.0);
  test_log_diff_exp(3.0,2.0);
  test_log_diff_exp(0,-2.1);
  test_log_diff_exp(-20.0,-23);
  test_log_diff_exp(-21.2,-32.1);
  EXPECT_NO_THROW(log_diff_exp(-20.0,12));
  EXPECT_NO_THROW(log_diff_exp(-20.0,-12.1));
  EXPECT_NO_THROW(log_diff_exp(120.0,120.10));
  EXPECT_NO_THROW(log_diff_exp(-20.0,10.2));
  EXPECT_NO_THROW(log_diff_exp(10,11));
  EXPECT_NO_THROW(log_diff_exp(10,10));
  EXPECT_NO_THROW(log_diff_exp(-10.21,-10.21));

  // exp(10000.0) overflows
  EXPECT_FLOAT_EQ(10000.0,log_diff_exp(10000.0,0.0));
  EXPECT_FLOAT_EQ(0.0,log_diff_exp(0.0,-10000.0));
}

TEST(MathFunctions, log_diff_exp_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::log_diff_exp(3.0, nan));

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::log_diff_exp(nan, 2.0));

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::log_diff_exp(nan, nan));
}


/*
 * main.cpp
 *
 *  Created on: 2021/07/22
 *      Author: matsu
 */
#include <Eigen/Dense>
#include <cstdio>
#include <iostream>
#include <string>
#include <vector>

using namespace Eigen;
using namespace std;

void test_intro1();
void test_intro2_1();
void test_intro2_2();

int main( int argc, char** argv )
{
    vector< string > args( argv, argv + argc );

    if ( args.at( 1 ) == string( "intro1" ) )
    {
        test_intro1();
    }
    else if ( args.at( 1 ) == string( "intro2_1" ) )
    {
        test_intro2_1();
    }
    else if ( args.at( 1 ) == string( "intro2_2" ) )
    {
        test_intro2_2();
    }
    else
    {
        fprintf( stderr, "Matching test is not exist.\n" );
        exit( -1 );
    }

    return 0;
}

void test_intro1()
{
    MatrixXd m( 2, 2 );
    m( 0, 0 ) = 3;
    m( 1, 0 ) = 2.5;
    m( 0, 1 ) = -1;
    m( 1, 1 ) = m( 1, 0 ) + m( 0, 1 );
    std::cout << m << std::endl;
}

void test_intro2_1()
{
    MatrixXd m = MatrixXd::Random( 3, 3 );
    m = ( m + MatrixXd::Constant( 3, 3, 1.2 ) ) * 50;
    cout << "m =" << endl
         << m << endl;
    VectorXd v( 3 );
    v << 1, 2, 3;
    cout << "m * v =" << endl
         << m * v << endl;
}

void test_intro2_2()
{
    Matrix3d m = Matrix3d::Random();
    m = ( m + Matrix3d::Constant( 1.2 ) ) * 50;
    cout << "m =" << endl
         << m << endl;
    Vector3d v( 1, 2, 3 );

    cout << "m * v =" << endl
         << m * v << endl;
}


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

using namespace std;
using namespace Eigen;

void rankoneinvit(const VectorXd & d, const double & tol, double & lmin) {
	MatrixXd M;
	VectorXd ev = d;
	lmin = 0.;
	double lnew = d.cwiseAbs().minCoeff();
	while (abs(lnew-lmin) > tol*lmin){
		lmin = lnew;
		M = d.asDiagonal(); M+=ev*ev.transpose();
		ev = M.lu().solve(ev);
		ev.normalize();
		lnew = ev.transpose()*M* ev;
		}
	lmin=lnew;
}

int main(){
    double tol=1e-3;
    double lmin;
    int n=10;

    VectorXd d=VectorXd::Random(n);
    rankoneinvit(d,tol,lmin);
    cout<<"lmin = "<<lmin<<endl;

    //Compare runtimes
    unsigned int repeats = 3;

    for(unsigned int p = 2; p <= 9; p++) {
        unsigned int n = pow(2,p);

        for(unsigned int r = 0; r < repeats; ++r) {

//             d = VectorXd::Random(n);
            d = VectorXd::LinSpaced(n,1,2);
            rankoneinvit(d,tol,lmin);

            }

    }
return 0;}

Nummerical Methods for CSE/PS3/problem1_a.cpp


#include <iostream>
#include <Eigen/Dense>
#include "timer.h"

void rankoneinvit(const Eigen::VectorXd & d, const double & tol, double & lmin){
	lmin=0.;
	Eigen::VectorXd ev;
	int n=d.size();
	Eigen::MatrixXd M;
	ev=d;
	double lnew = d.cwiseAbs().minCoeff();
	while(std::abs(lnew-lmin)> tol*lmin){
		lmin=lnew;
		M=d.asDiagonal();
		M+=ev*ev.transpose();
		ev= M.partialPivLu().solve(ev);
		ev.normalize();
		lnew= ev.transpose()*M*ev;
	}
	lmin=lnew;
}

using namespace std;
using namespace Eigen;
int main(){
    srand((unsigned int) time(0));
    double tol=1e-3;
    double lmin;
    int n=10;

    // Check correctedness of the fast version
    VectorXd d=VectorXd::Random(n);
    rankoneinvit(d,tol,lmin);
    cout<<"lmin = "<<lmin<<endl;
    //rankoneinvit_fast(d,tol,lmin);
    //cout<<"lmin = "<<lmin<<endl;

    //Compare runtimes
    unsigned int repeats = 3;
    /*
    for(unsigned int p = 2; p <= 9; p++) {
        tm_slow.reset();
        tm_fast.reset();
        unsigned int n = pow(2,p);

        for(unsigned int r = 0; r < repeats; ++r) {

//             d = VectorXd::Random(n);
            d = VectorXd::LinSpaced(n,1,2);
            rankoneinvit(d,tol,lmin);

            rankoneinvit_fast(d,tol,lmin);
        }

        cout << "The slow method took:    " <<  tm_slow.min().count() / 1000000. << " ms for n = " <<n<<endl;
        cout << "The fast method took:    " <<  tm_fast.min().count() / 1000000. << " ms for n = " <<n<<endl;
    }*/
}

Nummerical Methods for CSE/PS3/rankoneinvit.cpp


/**
 * @brief Partial Least Squares Regression with Eigen
 *
 * Same source as LDA, \cite friedman2001elements
 *
 * @file pls-eigen.hpp
 * @author François-David Collin <Francois-David.Collin@umontpellier.fr>
 * @brief
 * @version 0.1
 * @date 2018-11-08
 *
 * @copyright Copyright (c) 2018
 *
 */
#pragma once

#include "various.hpp"
#include <Eigen/Dense>
#include <list>
#include <algorithm>
#include <range/v3/all.hpp>

using namespace Eigen;
using namespace std;
using namespace ranges;

template<class Derived, class OtherDerived>
VectorXd pls(const MatrixBase<Derived>& x,
         const MatrixBase<OtherDerived>& y,
         size_t ncomp,
         MatrixXd& Projection,
         RowVectorXd& mean,
         RowVectorXd& std,
         bool stopping = false)
{
    auto n = x.rows();
    auto p = x.cols();
    mean = x.colwise().mean();
    std = ((x.rowwise() - mean).array().square().colwise().sum() / (x.rows() - 1)).sqrt();;
    MatrixXd X = (x.rowwise() - mean).array().rowwise() / std.array();
    MatrixXd Xo = X;
    MatrixXd XX = X.transpose() * X;
    MatrixXd Y = MatrixXd::Zero(n, ncomp + 1);
    MatrixXd P(p,ncomp);
    MatrixXd R(p,ncomp);
    VectorXd XY(n);
    VectorXd w(p);
    VectorXd r(p);
    VectorXd t(p);
    VectorXd res(ncomp);
    size_t window_size = std::max(2_z,ncomp/10_z);

    std::list<unsigned char> stopping_criterium(window_size,0);
    // Y.col(0) = y.rowwise() - y.colwise().mean();
    double ymean = y.mean();
    Y.col(0).array() = ymean;
    double SSTO = (y.array() - ymean).array().square().sum();
    int m = 0;
    // for (auto m = 0; m < ncomp; m++)
    while (m < ncomp)
    {
        XY = X.transpose() * y;
        SelfAdjointEigenSolver<MatrixXd> es( XY.transpose() * XY );
        auto abs_eigenvalues = es.eigenvalues().array().abs();
        size_t max_eigenvalue_indice = ranges::distance(abs_eigenvalues.begin(),ranges::max_element(abs_eigenvalues));
        auto q = es.eigenvectors().col(max_eigenvalue_indice);
        w = XY * q;
        w /= sqrt((w.transpose()*w)(0,0));
        r=w;
        for (auto j=0; j<=m-1;j++)
        {
            r -= (P.col(j).transpose()*w)(0,0)*R.col(j);
        }
        R.col(m) = r;
        t = Xo * r;
        P.col(m) = XX *r/(t.transpose()*t)(0,0);
        VectorXd Zm = X * XY;
        double Znorm = Zm.array().square().sum();
        double Thetam = Zm.dot(y) / Znorm;
        Y.col(m + 1) = Y.col(m) + Thetam * Zm;
        X -= Zm.rowwise().replicate(p) * ((Zm/Znorm).transpose() * X).asDiagonal();
        res(m) = (Y.col(m + 1).array() - ymean).array().square().sum() / SSTO;
        if ((m >= 2) && stopping) {
            auto lastdiff = res(m) - res(m-1);
            auto lastmean = (res(m) + res(m-1))/2.0;
            size_t remains = ncomp - m;
            stopping_criterium.pop_front();
            stopping_criterium.push_back(lastmean >= 0.99 * remains * lastdiff);
            auto wcrit = ranges::accumulate(stopping_criterium,0);
            if (wcrit == window_size) break;
        }
        m++;
    }
    if (m < ncomp) {
        m--;
        res = res(seq(0,m)).eval();
    }
    Projection = R;
//    VectorXd res = (Y.block(0,1,n,ncomp).array() - ymean).array().square().colwise().sum() / SSTO;
    return res;
}


#pragma once
#include "dirichlet_boundary.hpp"
#include "load_vector_assembly.hpp"
#include "stiffness_matrix_assembly.hpp"
#include <Eigen/Core>
#include <igl/readSTL.h>
#include <igl/slice.h>
#include <igl/slice_into.h>
#include <string>

typedef Eigen::VectorXd Vector;

//----------------solveBegin----------------
//! Solve the FEM system.
//!
//! @param[out] u will at the end contain the FEM solution.
//! @param[in] vertices list of triangle vertices for the mesh
//! @param[in] triangles list of triangles (described by indices)
//! @param[in] f the RHS f (as in the exercise)
//! @param[in] g the boundary value (as in the exercise)
//! @param[in] sigma the function sigma as in the exercise
//! @param[in] r the parameter r from the lecture notes
//! return number of degrees of freedom (without the boundary dofs)
int solveFiniteElement(Vector &               u,
                       const Eigen::MatrixXd &vertices,
                       const Eigen::MatrixXi &triangles,
                       const std::function<double(double, double)> &f,
                       const std::function<double(double, double)> &sigma,
                       const std::function<double(double, double)> &g,
                       double r) {
	SparseMatrix A;
	// (write your solution here)
	assembleStiffnessMatrix(A, vertices, triangles, sigma, r);

	Vector F;
	// (write your solution here)
	assembleLoadVector(F, vertices, triangles, f);

	u.resize(vertices.rows());
	u.setZero();
	Eigen::VectorXi interiorVertexIndices;

	// set Dirichlet Boundary conditions
	// (write your solution here)
	setDirichletBoundary(u, interiorVertexIndices, vertices, triangles, g);
	F -= A * u;

	SparseMatrix AInterior;

	igl::slice(A, interiorVertexIndices, interiorVertexIndices, AInterior);
	Eigen::SimplicialLDLT<SparseMatrix> solver;

	Vector FInterior;

	igl::slice(F, interiorVertexIndices, FInterior);

	//initialize solver for AInterior
	// (write your solution here)
	//// NPDE_START_TEMPLATE
	solver.compute(AInterior);

	if (solver.info() != Eigen::Success) {
		throw std::runtime_error("Could not decompose the matrix");
	}
	//// NPDE_END_TEMPLATE

	//solve interior system
	// (write your solution here)
	//// NPDE_START_TEMPLATE
	Vector uInterior = solver.solve(FInterior);
	igl::slice_into(uInterior, interiorVertexIndices, u);
	//// NPDE_END_TEMPLATE

	return interiorVertexIndices.size();
}
//----------------solveEnd----------------


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

using Eigen::VectorXf;
using Eigen::MatrixXf;
using namespace LBFGSpp;

class Rosenbrock
{
private:
    int n;
public:
    Rosenbrock(int n_) : n(n_) {}
    float operator()(const VectorXf& x, VectorXf& grad)
    {
        float fx = 0.0;
        for(int i = 0; i < n; i += 2)
        {
            float t1 = 1.0 - x[i];
            float 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;
        }
        return fx;
    }
};

int main()
{
    const int n = 10;
    LBFGSParam<float> param;
    LBFGSSolver<float> solver(param);
    Rosenbrock fun(n);

    VectorXf x = VectorXf::Zero(n);
    float fx;
    int niter = solver.minimize(fun, x, fx);

    std::cout << niter << " iterations" << std::endl;
    std::cout << "x = \n" << x.transpose() << std::endl;
    std::cout << "f(x) = " << fx << std::endl;

    return 0;
}


// https://stackoverflow.com/questions/21412169/creating-a-rotation-matrix-with-pitch-yaw-roll-using-eigen
#include <Eigen/Dense>
#include <iostream>
#include <math.h>

Eigen::Matrix3d rotation_from_euler(double roll, double pitch, double yaw)
{
    // roll and pitch and yaw in radians
    double su = sin(roll);
    double cu = cos(roll);
    double sv = sin(pitch);
    double cv = cos(pitch);
    double sw = sin(yaw);
    double cw = cos(yaw);
    Eigen::Matrix3d Rot_matrix(3, 3);
    Rot_matrix(0, 0) = cv * cw;
    Rot_matrix(0, 1) = su * sv * cw - cu * sw;
    Rot_matrix(0, 2) = su * sw + cu * sv * cw;
    Rot_matrix(1, 0) = cv * sw;
    Rot_matrix(1, 1) = cu * cw + su * sv * sw;
    Rot_matrix(1, 2) = cu * sv * sw - su * cw;
    Rot_matrix(2, 0) = -sv;
    Rot_matrix(2, 1) = su * cv;
    Rot_matrix(2, 2) = cu * cv;
    return Rot_matrix;
}

int main()
{
    Eigen::Matrix3d rot_mat = rotation_from_euler(0, 0, 0.5 * M_PI);
    std::cout << rot_mat << std::endl;
    return 0;
}


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

using Eigen::MatrixXd;
using namespace Eigen;
using namespace std;

// -> https://aleksandarhaber.com/eigen-matrix-library-c-tutorial-saving-and-loading-data-in-from-a-csv-file/
// -> https://stackoverflow.com/questions/20786220/eigen-library-initialize-matrix-with-data-from-file-or-existing-stdvector
int main()
{
  MatrixXd m = MatrixXd::Random(3,3);
  m = (m + MatrixXd::Constant(3,3,1.2)) * 50;
  cout << "m =" << endl << m << endl;
  VectorXd v(3);
  v << 1, 2, 3;
  cout << "m * v =" << endl << m * v << endl;
}


/**
 * @file logistic_regression_test.cpp
 * @author Ryan Curtin
 *
 * Test for LogisticFunction and LogisticRegression.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/logistic_regression/logistic_regression.hpp>
#include <mlpack/core/optimizers/sgd/sgd.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace mlpack;
using namespace mlpack::regression;
using namespace mlpack::optimization;
using namespace mlpack::distribution;

BOOST_AUTO_TEST_SUITE(LogisticRegressionTest);

/**
 * Test the LogisticRegressionFunction on a simple set of points.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionFunctionEvaluate)
{
  // Very simple fake dataset.
  arma::mat data("1 2 3;"
                 "1 2 3");
  arma::Row<size_t> responses("1 1 0");

  // Create a LogisticRegressionFunction.
  LogisticRegressionFunction<> lrf(data, responses,
      0.0 /* no regularization */);

  // These were hand-calculated using Octave.
  BOOST_REQUIRE_CLOSE(lrf.Evaluate(arma::vec("1 1 1")), 7.0562141665, 1e-5);
  BOOST_REQUIRE_CLOSE(lrf.Evaluate(arma::vec("0 0 0")), 2.0794415417, 1e-5);
  BOOST_REQUIRE_CLOSE(lrf.Evaluate(arma::vec("-1 -1 -1")), 8.0562141665, 1e-5);
  BOOST_REQUIRE_CLOSE(lrf.Evaluate(arma::vec("200 -40 -40")), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(lrf.Evaluate(arma::vec("200 -80 0")), 0.0, 1e-5);
  BOOST_REQUIRE_CLOSE(lrf.Evaluate(arma::vec("200 -100 20")), 0.0, 1e-5);
}

/**
 * A more complicated test for the LogisticRegressionFunction.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionFunctionRandomEvaluate)
{
  const size_t points = 1000;
  const size_t dimension = 10;
  const size_t trials = 50;

  // Create a random dataset.
  arma::mat data;
  data.randu(dimension, points);
  // Create random responses.
  arma::Row<size_t> responses(points);
  for (size_t i = 0; i < points; ++i)
    responses[i] = math::RandInt(0, 2);

  LogisticRegressionFunction<> lrf(data, responses,
      0.0 /* no regularization */);

  // Run a bunch of trials.
  for (size_t i = 0; i < trials; ++i)
  {
    // Generate a random set of parameters.
    arma::vec parameters;
    parameters.randu(dimension + 1);

    // Hand-calculate the loss function.
    double loglikelihood = 0.0;
    for (size_t j = 0; j < points; ++j)
    {
      const double sigmoid = (1.0 / (1.0 + exp(-parameters[0] -
          arma::dot(data.col(j), parameters.subvec(1, dimension)))));
      if (responses[j] == 1.0)
        loglikelihood += log(std::pow(sigmoid, responses[j]));
      else
        loglikelihood += log(std::pow(1.0 - sigmoid, 1.0 - responses[j]));
    }

    BOOST_REQUIRE_CLOSE(lrf.Evaluate(parameters), -loglikelihood, 1e-5);
  }
}

/**
 * Test regularization for the LogisticRegressionFunction Evaluate() function.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionFunctionRegularizationEvaluate)
{
  const size_t points = 5000;
  const size_t dimension = 25;
  const size_t trials = 10;

  // Create a random dataset.
  arma::mat data;
  data.randu(dimension, points);
  // Create random responses.
  arma::Row<size_t> responses(points);
  for (size_t i = 0; i < points; ++i)
    responses[i] = math::RandInt(0, 2);

  LogisticRegressionFunction<> lrfNoReg(data, responses, 0.0);
  LogisticRegressionFunction<> lrfSmallReg(data, responses, 0.5);
  LogisticRegressionFunction<> lrfBigReg(data, responses, 20.0);

  for (size_t i = 0; i < trials; ++i)
  {
    arma::vec parameters(dimension + 1);
    parameters.randu();

    // Regularization term: 0.5 * lambda * || parameters ||_2^2 (but note that
    // the first parameters term is ignored).
    const double smallRegTerm = 0.25 * std::pow(arma::norm(parameters, 2), 2.0)
        - 0.25 * std::pow(parameters[0], 2.0);
    const double bigRegTerm = 10.0 * std::pow(arma::norm(parameters, 2), 2.0)
        - 10.0 * std::pow(parameters[0], 2.0);

    BOOST_REQUIRE_CLOSE(lrfNoReg.Evaluate(parameters) + smallRegTerm,
        lrfSmallReg.Evaluate(parameters), 1e-5);
    BOOST_REQUIRE_CLOSE(lrfNoReg.Evaluate(parameters) + bigRegTerm,
        lrfBigReg.Evaluate(parameters), 1e-5);
  }
}

/**
 * Test gradient of the LogisticRegressionFunction.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionFunctionGradient)
{
  // Very simple fake dataset.
  arma::mat data("1 2 3;"
                 "1 2 3");
  arma::Row<size_t> responses("1 1 0");

  // Create a LogisticRegressionFunction.
  LogisticRegressionFunction<> lrf(data, responses,
      0.0 /* no regularization */);
  arma::vec gradient;

  // If the model is at the optimum, then the gradient should be zero.
  lrf.Gradient(arma::vec("200 -40 -40"), gradient);

  BOOST_REQUIRE_EQUAL(gradient.n_elem, 3);
  BOOST_REQUIRE_SMALL(gradient[0], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[1], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[2], 1e-15);

  // Perturb two elements in the wrong way, so they need to become smaller.
  lrf.Gradient(arma::vec("200 -20 -20"), gradient);

  // The actual values are less important; the gradient just needs to be pointed
  // the right way.
  BOOST_REQUIRE_EQUAL(gradient.n_elem, 3);
  BOOST_REQUIRE_GE(gradient[1], 0.0);
  BOOST_REQUIRE_GE(gradient[2], 0.0);

  // Perturb two elements in the wrong way, so they need to become larger.
  lrf.Gradient(arma::vec("200 -60 -60"), gradient);

  // The actual values are less important; the gradient just needs to be pointed
  // the right way.
  BOOST_REQUIRE_EQUAL(gradient.n_elem, 3);
  BOOST_REQUIRE_LE(gradient[1], 0.0);
  BOOST_REQUIRE_LE(gradient[2], 0.0);

  // Perturb the intercept element.
  lrf.Gradient(arma::vec("250 -40 -40"), gradient);

  // The actual values are less important; the gradient just needs to be pointed
  // the right way.
  BOOST_REQUIRE_EQUAL(gradient.n_elem, 3);
  BOOST_REQUIRE_GE(gradient[0], 0.0);
}

/**
 * Test individual Evaluate() functions for SGD.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionSeparableEvaluate)
{
  // Very simple fake dataset.
  arma::mat data("1 2 3;"
                 "1 2 3;");
  arma::Row<size_t> responses("1 1 0");

  // Create a LogisticRegressionFunction.
  LogisticRegressionFunction<> lrf(data, responses,
      0.0 /* no regularization */);

  // These were hand-calculated using Octave.
  BOOST_REQUIRE_CLOSE(lrf.Evaluate(arma::vec("1 1 1"), 0), 4.85873516e-2, 1e-5);
  BOOST_REQUIRE_CLOSE(lrf.Evaluate(arma::vec("1 1 1"), 1), 6.71534849e-3, 1e-5);
  BOOST_REQUIRE_CLOSE(lrf.Evaluate(arma::vec("1 1 1"), 2), 7.00091146645, 1e-5);

  BOOST_REQUIRE_CLOSE(lrf.Evaluate(arma::vec("0 0 0"), 0), 0.6931471805, 1e-5);
  BOOST_REQUIRE_CLOSE(lrf.Evaluate(arma::vec("0 0 0"), 1), 0.6931471805, 1e-5);
  BOOST_REQUIRE_CLOSE(lrf.Evaluate(arma::vec("0 0 0"), 2), 0.6931471805, 1e-5);

  BOOST_REQUIRE_CLOSE(lrf.Evaluate(arma::vec("-1 -1 -1"), 0), 3.0485873516,
      1e-5);
  BOOST_REQUIRE_CLOSE(lrf.Evaluate(arma::vec("-1 -1 -1"), 1), 5.0067153485,
      1e-5);
  BOOST_REQUIRE_CLOSE(lrf.Evaluate(arma::vec("-1 -1 -1"), 2), 9.1146645377e-4,
      1e-5);

  BOOST_REQUIRE_SMALL(lrf.Evaluate(arma::vec("200 -40 -40"), 0), 1e-5);
  BOOST_REQUIRE_SMALL(lrf.Evaluate(arma::vec("200 -40 -40"), 1), 1e-5);
  BOOST_REQUIRE_SMALL(lrf.Evaluate(arma::vec("200 -40 -40"), 2), 1e-5);

  BOOST_REQUIRE_SMALL(lrf.Evaluate(arma::vec("200 -80 0"), 0), 1e-5);
  BOOST_REQUIRE_SMALL(lrf.Evaluate(arma::vec("200 -80 0"), 1), 1e-5);
  BOOST_REQUIRE_SMALL(lrf.Evaluate(arma::vec("200 -80 0"), 2), 1e-5);

  BOOST_REQUIRE_SMALL(lrf.Evaluate(arma::vec("200 -100 20"), 0), 1e-5);
  BOOST_REQUIRE_SMALL(lrf.Evaluate(arma::vec("200 -100 20"), 1), 1e-5);
  BOOST_REQUIRE_SMALL(lrf.Evaluate(arma::vec("200 -100 20"), 2), 1e-5);
}

/**
 * Test regularization for the separable LogisticRegressionFunction Evaluate()
 * function.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionFunctionRegularizationSeparableEvaluate)
{
  const size_t points = 5000;
  const size_t dimension = 25;
  const size_t trials = 10;

  // Create a random dataset.
  arma::mat data;
  data.randu(dimension, points);
  // Create random responses.
  arma::Row<size_t> responses(points);
  for (size_t i = 0; i < points; ++i)
    responses[i] = math::RandInt(0, 2);

  LogisticRegressionFunction<> lrfNoReg(data, responses, 0.0);
  LogisticRegressionFunction<> lrfSmallReg(data, responses, 0.5);
  LogisticRegressionFunction<> lrfBigReg(data, responses, 20.0);

  // Check that the number of functions is correct.
  BOOST_REQUIRE_EQUAL(lrfNoReg.NumFunctions(), points);
  BOOST_REQUIRE_EQUAL(lrfSmallReg.NumFunctions(), points);
  BOOST_REQUIRE_EQUAL(lrfBigReg.NumFunctions(), points);

  for (size_t i = 0; i < trials; ++i)
  {
    arma::vec parameters(dimension + 1);
    parameters.randu();

    // Regularization term: 0.5 * lambda * || parameters ||_2^2 (but note that
    // the first parameters term is ignored).
    const double smallRegTerm = (0.25 * std::pow(arma::norm(parameters, 2), 2.0)
        - 0.25 * std::pow(parameters[0], 2.0)) / points;
    const double bigRegTerm = (10.0 * std::pow(arma::norm(parameters, 2), 2.0)
        - 10.0 * std::pow(parameters[0], 2.0)) / points;

    for (size_t j = 0; j < points; ++j)
    {
      BOOST_REQUIRE_CLOSE(lrfNoReg.Evaluate(parameters, j) + smallRegTerm,
          lrfSmallReg.Evaluate(parameters, j), 1e-5);
      BOOST_REQUIRE_CLOSE(lrfNoReg.Evaluate(parameters, j) + bigRegTerm,
          lrfBigReg.Evaluate(parameters, j), 1e-5);
    }
  }
}

/**
 * Test separable gradient of the LogisticRegressionFunction.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionFunctionSeparableGradient)
{
  // Very simple fake dataset.
  arma::mat data("1 2 3;"
                 "1 2 3");
  arma::Row<size_t> responses("1 1 0");

  // Create a LogisticRegressionFunction.
  LogisticRegressionFunction<> lrf(data, responses,
      0.0 /* no regularization */);
  arma::vec gradient;

  // If the model is at the optimum, then the gradient should be zero.
  lrf.Gradient(arma::vec("200 -40 -40"), 0, gradient);

  BOOST_REQUIRE_EQUAL(gradient.n_elem, 3);
  BOOST_REQUIRE_SMALL(gradient[0], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[1], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[2], 1e-15);

  lrf.Gradient(arma::vec("200 -40 -40"), 1, gradient);
  BOOST_REQUIRE_EQUAL(gradient.n_elem, 3);
  BOOST_REQUIRE_SMALL(gradient[0], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[1], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[2], 1e-15);

  lrf.Gradient(arma::vec("200 -40 -40"), 2, gradient);
  BOOST_REQUIRE_EQUAL(gradient.n_elem, 3);
  BOOST_REQUIRE_SMALL(gradient[0], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[1], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[2], 1e-15);

  // Perturb two elements in the wrong way, so they need to become smaller.  For
  // the first two data points, classification is still correct so the gradient
  // should be zero.
  lrf.Gradient(arma::vec("200 -30 -30"), 0, gradient);
  BOOST_REQUIRE_EQUAL(gradient.n_elem, 3);
  BOOST_REQUIRE_SMALL(gradient[0], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[1], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[2], 1e-15);

  lrf.Gradient(arma::vec("200 -30 -30"), 1, gradient);
  BOOST_REQUIRE_EQUAL(gradient.n_elem, 3);
  BOOST_REQUIRE_SMALL(gradient[0], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[1], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[2], 1e-15);

  lrf.Gradient(arma::vec("200 -30 -30"), 2, gradient);
  BOOST_REQUIRE_EQUAL(gradient.n_elem, 3);
  BOOST_REQUIRE_GE(gradient[1], 0.0);
  BOOST_REQUIRE_GE(gradient[2], 0.0);

  // Perturb two elements in the other wrong way, so they need to become larger.
  // For the first and last data point, classification is still correct so the
  // gradient should be zero.
  lrf.Gradient(arma::vec("200 -60 -60"), 0, gradient);
  BOOST_REQUIRE_EQUAL(gradient.n_elem, 3);
  BOOST_REQUIRE_SMALL(gradient[0], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[1], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[2], 1e-15);

  lrf.Gradient(arma::vec("200 -30 -30"), 1, gradient);
  BOOST_REQUIRE_EQUAL(gradient.n_elem, 3);
  BOOST_REQUIRE_LE(gradient[1], 0.0);
  BOOST_REQUIRE_LE(gradient[2], 0.0);

  lrf.Gradient(arma::vec("200 -60 -60"), 2, gradient);
  BOOST_REQUIRE_EQUAL(gradient.n_elem, 3);
  BOOST_REQUIRE_SMALL(gradient[0], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[1], 1e-15);
  BOOST_REQUIRE_SMALL(gradient[2], 1e-15);
}

/**
 * Test Gradient() function when regularization is used.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionFunctionRegularizationGradient)
{
  const size_t points = 5000;
  const size_t dimension = 25;
  const size_t trials = 10;

  // Create a random dataset.
  arma::mat data;
  data.randu(dimension, points);
  // Create random responses.
  arma::Row<size_t> responses(points);
  for (size_t i = 0; i < points; ++i)
    responses[i] = math::RandInt(0, 2);

  LogisticRegressionFunction<> lrfNoReg(data, responses, 0.0);
  LogisticRegressionFunction<> lrfSmallReg(data, responses, 0.5);
  LogisticRegressionFunction<> lrfBigReg(data, responses, 20.0);

  for (size_t i = 0; i < trials; ++i)
  {
    arma::vec parameters(dimension + 1);
    parameters.randu();

    // Regularization term: 0.5 * lambda * || parameters ||_2^2 (but note that
    // the first parameters term is ignored).  Now we take the gradient of this
    // to obtain
    //   g[i] = lambda * parameters[i]
    // although g(0) == 0 because we are not regularizing the intercept term of
    // the model.
    arma::vec gradient;
    arma::vec smallRegGradient;
    arma::vec bigRegGradient;

    lrfNoReg.Gradient(parameters, gradient);
    lrfSmallReg.Gradient(parameters, smallRegGradient);
    lrfBigReg.Gradient(parameters, bigRegGradient);

    // Check sizes of gradients.
    BOOST_REQUIRE_EQUAL(gradient.n_elem, parameters.n_elem);
    BOOST_REQUIRE_EQUAL(smallRegGradient.n_elem, parameters.n_elem);
    BOOST_REQUIRE_EQUAL(bigRegGradient.n_elem, parameters.n_elem);

    // Make sure first term has zero regularization.
    BOOST_REQUIRE_CLOSE(gradient[0], smallRegGradient[0], 1e-5);
    BOOST_REQUIRE_CLOSE(gradient[0], bigRegGradient[0], 1e-5);

    // Check other terms.
    for (size_t j = 1; j < parameters.n_elem; ++j)
    {
      const double smallRegTerm = 0.5 * parameters[j];
      const double bigRegTerm = 20.0 * parameters[j];

      BOOST_REQUIRE_CLOSE(gradient[j] + smallRegTerm, smallRegGradient[j],
          1e-5);
      BOOST_REQUIRE_CLOSE(gradient[j] + bigRegTerm, bigRegGradient[j], 1e-5);
    }
  }
}

/**
 * Test separable Gradient() function when regularization is used.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionFunctionRegularizationSeparableGradient)
{
  const size_t points = 2000;
  const size_t dimension = 25;
  const size_t trials = 3;

  // Create a random dataset.
  arma::mat data;
  data.randu(dimension, points);
  // Create random responses.
  arma::Row<size_t> responses(points);
  for (size_t i = 0; i < points; ++i)
    responses[i] = math::RandInt(0, 2);

  LogisticRegressionFunction<> lrfNoReg(data, responses, 0.0);
  LogisticRegressionFunction<> lrfSmallReg(data, responses, 0.5);
  LogisticRegressionFunction<> lrfBigReg(data, responses, 20.0);

  for (size_t i = 0; i < trials; ++i)
  {
    arma::vec parameters(dimension + 1);
    parameters.randu();

    // Regularization term: 0.5 * lambda * || parameters ||_2^2 (but note that
    // the first parameters term is ignored).  Now we take the gradient of this
    // to obtain
    //   g[i] = lambda * parameters[i]
    // although g(0) == 0 because we are not regularizing the intercept term of
    // the model.
    arma::vec gradient;
    arma::vec smallRegGradient;
    arma::vec bigRegGradient;

    // Test separable gradient for each point.  Regularization will be the same.
    for (size_t k = 0; k < points; ++k)
    {
      lrfNoReg.Gradient(parameters, k, gradient);
      lrfSmallReg.Gradient(parameters, k, smallRegGradient);
      lrfBigReg.Gradient(parameters, k, bigRegGradient);

      // Check sizes of gradients.
      BOOST_REQUIRE_EQUAL(gradient.n_elem, parameters.n_elem);
      BOOST_REQUIRE_EQUAL(smallRegGradient.n_elem, parameters.n_elem);
      BOOST_REQUIRE_EQUAL(bigRegGradient.n_elem, parameters.n_elem);

      // Make sure first term has zero regularization.
      BOOST_REQUIRE_CLOSE(gradient[0], smallRegGradient[0], 1e-5);
      BOOST_REQUIRE_CLOSE(gradient[0], bigRegGradient[0], 1e-5);

      // Check other terms.
      for (size_t j = 1; j < parameters.n_elem; ++j)
      {
        const double smallRegTerm = 0.5 * parameters[j] / points;
        const double bigRegTerm = 20.0 * parameters[j] / points;

        BOOST_REQUIRE_CLOSE(gradient[j] + smallRegTerm, smallRegGradient[j],
            1e-5);
        BOOST_REQUIRE_CLOSE(gradient[j] + bigRegTerm, bigRegGradient[j], 1e-5);
      }
    }
  }
}

// Test training of logistic regression on a simple dataset.
BOOST_AUTO_TEST_CASE(LogisticRegressionLBFGSSimpleTest)
{
  // Very simple fake dataset.
  arma::mat data("1 2 3;"
                 "1 2 3");
  arma::Row<size_t> responses("1 1 0");

  // Create a logistic regression object using L-BFGS (that is the default).
  LogisticRegression<> lr(data, responses);

  // Test sigmoid function.
  arma::vec sigmoids = 1 / (1 + arma::exp(-lr.Parameters()[0]
      - data.t() * lr.Parameters().subvec(1, lr.Parameters().n_elem - 1)));

  // Large 0.1% error tolerance is because the optimizer may terminate before
  // the predictions converge to 1.
  BOOST_REQUIRE_CLOSE(sigmoids[0], 1.0, 0.1);
  BOOST_REQUIRE_CLOSE(sigmoids[1], 1.0, 5.0);
  BOOST_REQUIRE_SMALL(sigmoids[2], 0.1);
}

// Test training of logistic regression on a simple dataset using SGD.
BOOST_AUTO_TEST_CASE(LogisticRegressionSGDSimpleTest)
{
  // Very simple fake dataset.
  arma::mat data("1 2 3;"
                 "1 2 3");
  arma::Row<size_t> responses("1 1 0");

  // Create a logistic regression object using a custom SGD object with a much
  // smaller tolerance.
  LogisticRegressionFunction<> lrf(data, responses, 0.001);
  SGD<LogisticRegressionFunction<>> sgd(lrf, 0.005, 500000, 1e-10);
  LogisticRegression<> lr(sgd);

  // Test sigmoid function.
  arma::vec sigmoids = 1 / (1 + arma::exp(-lr.Parameters()[0]
      - data.t() * lr.Parameters().subvec(1, lr.Parameters().n_elem - 1)));

  // Large 0.1% error tolerance is because the optimizer may terminate before
  // the predictions converge to 1.  SGD tolerance is larger because its default
  // convergence tolerance is larger.
  BOOST_REQUIRE_CLOSE(sigmoids[0], 1.0, 3.0);
  BOOST_REQUIRE_CLOSE(sigmoids[1], 1.0, 12.0);
  BOOST_REQUIRE_SMALL(sigmoids[2], 0.1);
}

// Test training of logistic regression on a simple dataset with regularization.
BOOST_AUTO_TEST_CASE(LogisticRegressionLBFGSRegularizationSimpleTest)
{
  // Very simple fake dataset.
  arma::mat data("1 2 3;"
                 "1 2 3");
  arma::Row<size_t> responses("1 1 0");

  // Create a logistic regression object using L-BFGS (that is the default).
  LogisticRegression<> lr(data, responses, 0.001);

  // Test sigmoid function.
  arma::vec sigmoids = 1 / (1 + arma::exp(-lr.Parameters()[0]
      - data.t() * lr.Parameters().subvec(1, lr.Parameters().n_elem - 1)));

  // Large error tolerance is because the optimizer may terminate before
  // the predictions converge to 1.
  BOOST_REQUIRE_CLOSE(sigmoids[0], 1.0, 5.0);
  BOOST_REQUIRE_CLOSE(sigmoids[1], 1.0, 10.0);
  BOOST_REQUIRE_SMALL(sigmoids[2], 0.1);
}

// Test training of logistic regression on a simple dataset using SGD with
// regularization.
BOOST_AUTO_TEST_CASE(LogisticRegressionSGDRegularizationSimpleTest)
{
  // Very simple fake dataset.
  arma::mat data("1 2 3;"
                 "1 2 3");
  arma::Row<size_t> responses("1 1 0");

  // Create a logistic regression object using custom SGD with a much smaller
  // tolerance.
  LogisticRegressionFunction<> lrf(data, responses, 0.001);
  SGD<LogisticRegressionFunction<>> sgd(lrf, 0.005, 500000, 1e-10);
  LogisticRegression<> lr(sgd);

  // Test sigmoid function.
  arma::vec sigmoids = 1 / (1 + arma::exp(-lr.Parameters()[0]
      - data.t() * lr.Parameters().subvec(1, lr.Parameters().n_elem - 1)));

  // Large error tolerance is because the optimizer may terminate before
  // the predictions converge to 1.  SGD tolerance is wider because its default
  // convergence tolerance is larger.
  BOOST_REQUIRE_CLOSE(sigmoids[0], 1.0, 7.0);
  BOOST_REQUIRE_CLOSE(sigmoids[1], 1.0, 14.0);
  BOOST_REQUIRE_SMALL(sigmoids[2], 0.1);
}

// Test training of logistic regression on two Gaussians and ensure it's
// properly separable.
BOOST_AUTO_TEST_CASE(LogisticRegressionLBFGSGaussianTest)
{
  // Generate a two-Gaussian dataset.
  GaussianDistribution g1(arma::vec("1.0 1.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("9.0 9.0 9.0"), arma::eye<arma::mat>(3, 3));

  arma::mat data(3, 1000);
  arma::Row<size_t> responses(1000);
  for (size_t i = 0; i < 500; ++i)
  {
    data.col(i) = g1.Random();
    responses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    data.col(i) = g2.Random();
    responses[i] = 1;
  }

  // Now train a logistic regression object on it.
  LogisticRegression<> lr(data.n_rows, 0.5);
  lr.Train<L_BFGS>(data, responses);

  // Ensure that the error is close to zero.
  const double acc = lr.ComputeAccuracy(data, responses);
  BOOST_REQUIRE_CLOSE(acc, 100.0, 0.3); // 0.3% error tolerance.

  // Create a test set.
  for (size_t i = 0; i < 500; ++i)
  {
    data.col(i) = g1.Random();
    responses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    data.col(i) = g2.Random();
    responses[i] = 1;
  }

  // Ensure that the error is close to zero.
  const double testAcc = lr.ComputeAccuracy(data, responses);

  BOOST_REQUIRE_CLOSE(testAcc, 100.0, 0.6); // 0.6% error tolerance.
}

// Test training of logistic regression on two Gaussians and ensure it's
// properly separable using SGD.
BOOST_AUTO_TEST_CASE(LogisticRegressionSGDGaussianTest)
{
  // Generate a two-Gaussian dataset.
  GaussianDistribution g1(arma::vec("1.0 1.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("9.0 9.0 9.0"), arma::eye<arma::mat>(3, 3));

  arma::mat data(3, 1000);
  arma::Row<size_t> responses(1000);
  for (size_t i = 0; i < 500; ++i)
  {
    data.col(i) = g1.Random();
    responses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    data.col(i) = g2.Random();
    responses[i] = 1;
  }

  // Now train a logistic regression object on it.
  LogisticRegression<> lr(data.n_rows, 0.5);
  lr.Train<SGD>(data, responses);

  // Ensure that the error is close to zero.
  const double acc = lr.ComputeAccuracy(data, responses);

  BOOST_REQUIRE_CLOSE(acc, 100.0, 0.3); // 0.3% error tolerance.

  // Create a test set.
  for (size_t i = 0; i < 500; ++i)
  {
    data.col(i) = g1.Random();
    responses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    data.col(i) = g2.Random();
    responses[i] = 1;
  }

  // Ensure that the error is close to zero.
  const double testAcc = lr.ComputeAccuracy(data, responses);

  BOOST_REQUIRE_CLOSE(testAcc, 100.0, 0.6); // 0.6% error tolerance.
}

/**
 * Test constructor that takes an already-instantiated optimizer.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionInstantiatedOptimizer)
{
  // Very simple fake dataset.
  arma::mat data("1 2 3;"
                 "1 2 3");
  arma::Row<size_t> responses("1 1 0");

  // Create an optimizer and function.
  LogisticRegressionFunction<> lrf(data, responses, 0.0005);
  L_BFGS<LogisticRegressionFunction<>> lbfgsOpt(lrf);
  lbfgsOpt.MinGradientNorm() = 1e-50;
  LogisticRegression<> lr(lbfgsOpt);

  // Test sigmoid function.
  arma::vec sigmoids = 1 / (1 + arma::exp(-lr.Parameters()[0]
      - data.t() * lr.Parameters().subvec(1, lr.Parameters().n_elem - 1)));

  // Error tolerance is small because we tightened the optimizer tolerance.
  BOOST_REQUIRE_CLOSE(sigmoids[0], 1.0, 0.1);
  BOOST_REQUIRE_CLOSE(sigmoids[1], 1.0, 0.6);
  BOOST_REQUIRE_SMALL(sigmoids[2], 0.1);

  // Now do the same with SGD.
  SGD<LogisticRegressionFunction<>> sgdOpt(lrf);
  sgdOpt.StepSize() = 0.15;
  sgdOpt.Tolerance() = 1e-75;
  LogisticRegression<> lr2(sgdOpt);

  // Test sigmoid function.
  sigmoids = 1 / (1 + arma::exp(-lr2.Parameters()[0]
      - data.t() * lr2.Parameters().subvec(1, lr2.Parameters().n_elem - 1)));

  // Error tolerance is small because we tightened the optimizer tolerance.
  BOOST_REQUIRE_CLOSE(sigmoids[0], 1.0, 0.1);
  BOOST_REQUIRE_CLOSE(sigmoids[1], 1.0, 0.6);
  BOOST_REQUIRE_SMALL(sigmoids[2], 0.1);
}

/**
 * Test the Train() function and make sure it works the same as if we'd called
 * the constructor by hand, with the L-BFGS optimizer.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionLBFGSTrainTest)
{
  // Make a random dataset with random labels.
  arma::mat dataset(5, 800);
  dataset.randu();
  arma::Row<size_t> labels(800);
  for (size_t i = 0; i < 800; ++i)
    labels[i] = math::RandInt(0, 2);

  LogisticRegression<> lr(dataset, labels, 0.3);
  LogisticRegression<> lr2(dataset.n_rows, 0.3);
  lr2.Train(dataset, labels);

  BOOST_REQUIRE_EQUAL(lr.Parameters().n_elem, lr2.Parameters().n_elem);
  for (size_t i = 0; i < lr.Parameters().n_elem; ++i)
    BOOST_REQUIRE_CLOSE(lr.Parameters()[i], lr2.Parameters()[i], 0.005);
}

/**
 * Test the Train() function and make sure it works the same as if we'd called
 * the constructor by hand, with the SGD optimizer.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionSGDTrainTest)
{
  // Make a random dataset with random labels.
  arma::mat dataset(5, 800);
  dataset.randu();
  arma::Row<size_t> labels(800);
  for (size_t i = 0; i < 800; ++i)
    labels[i] = math::RandInt(0, 2);

  LogisticRegressionFunction<> lrf(dataset, labels, 0.3);
  SGD<LogisticRegressionFunction<>> sgd(lrf);
  sgd.Shuffle() = false;
  LogisticRegression<> lr(sgd);
  LogisticRegression<> lr2(dataset.n_rows, 0.3);

  LogisticRegressionFunction<> lrf2(dataset, labels, 0.3);
  SGD<LogisticRegressionFunction<>> sgd2(lrf2);
  sgd2.Shuffle() = false;
  lr2.Train(sgd2);

  BOOST_REQUIRE_EQUAL(lr.Parameters().n_elem, lr2.Parameters().n_elem);
  for (size_t i = 0; i < lr.Parameters().n_elem; ++i)
    BOOST_REQUIRE_CLOSE(lr.Parameters()[i], lr2.Parameters()[i], 1e-5);
}

/**
 * Test sparse and dense logistic regression and make sure they both work the
 * same using the L-BFGS optimizer.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionSparseLBFGSTest)
{
  // Create a random dataset.
  arma::sp_mat dataset;
  dataset.sprandu(10, 800, 0.3);
  arma::mat denseDataset(dataset);
  arma::Row<size_t> labels(800);
  for (size_t i = 0; i < 800; ++i)
    labels[i] = math::RandInt(0, 2);

  LogisticRegression<> lr(denseDataset, labels, 0.3);
  LogisticRegression<arma::sp_mat> lrSparse(dataset, labels, 0.3);

  BOOST_REQUIRE_EQUAL(lr.Parameters().n_elem, lrSparse.Parameters().n_elem);
  for (size_t i = 0; i < lr.Parameters().n_elem; ++i)
    BOOST_REQUIRE_CLOSE(lr.Parameters()[i], lrSparse.Parameters()[i], 1e-4);
}

/**
 * Test sparse and dense logistic regression and make sure they both work the
 * same using the SGD optimizer.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionSparseSGDTest)
{
  // Create a random dataset.
  arma::sp_mat dataset;
  dataset.sprandu(10, 800, 0.3);
  arma::mat denseDataset(dataset);
  arma::Row<size_t> labels(800);
  for (size_t i = 0; i < 800; ++i)
    labels[i] = math::RandInt(0, 2);

  LogisticRegression<> lr(10, 0.3);
  LogisticRegressionFunction<> lrf(denseDataset, labels, 0.3);
  SGD<LogisticRegressionFunction<>> sgd(lrf);
  sgd.Shuffle() = false;
  lr.Train(sgd);

  LogisticRegression<arma::sp_mat> lrSparse(10, 0.3);
  LogisticRegressionFunction<arma::sp_mat> lrfSparse(dataset, labels, 0.3);
  SGD<LogisticRegressionFunction<arma::sp_mat>> sgdSparse(lrfSparse);
  sgdSparse.Shuffle() = false;
  lrSparse.Train(sgdSparse);

  BOOST_REQUIRE_EQUAL(lr.Parameters().n_elem, lrSparse.Parameters().n_elem);
  for (size_t i = 0; i < lr.Parameters().n_elem; ++i)
    BOOST_REQUIRE_CLOSE(lr.Parameters()[i], lrSparse.Parameters()[i], 1e-5);
}

BOOST_AUTO_TEST_SUITE_END();


#include "sphere.hpp"

#include "intersection_info.hpp"

#include <math/point3d.hpp>
#include <math/ray3d.hpp>
#include <math/utility.hpp>
#include <math/vector3d.hpp>

#include <boost/optional.hpp>
#include <boost/none.hpp>

#include <cmath>

namespace shapes
{

// Check for ray-sphere intersection using geometric test.
//
// Note:
//  If ray origin is on the surface of the sphere and ray direction points
//  to the outside, no intersection is found.
bool intersects(shapes::sphere shape, math::ray3d ray)
{
  const auto L = math::make_vector3d(ray.origin, shape.center);
  const auto tPX = math::dot(L, ray.direction);

  const auto orig_center_sqrd_dist = math::squared_length(L);
  const auto squared_radius = shape.radius * shape.radius;

  // Ray origin is inside sphere.
  if (orig_center_sqrd_dist < squared_radius) { return true; }

  // Sphere center is behind ray origin.
  if (tPX <= 0.0f) { return false; }

  auto dsq = orig_center_sqrd_dist - tPX * tPX;
  return dsq <= squared_radius;
}

// Returns the closest intersection point between ray and sphere.
//
// Note:
//  If ray origin is on the surface of the sphere and ray direction points
//  to the outside, no intersection is found.
boost::optional<shapes::intersection_info> closest_intersection(shapes::sphere shape, math::ray3d ray)
{
  const auto L = math::make_vector3d(ray.origin, shape.center);
  const auto tPX = math::dot(L, ray.direction);

  const auto orig_center_sqrd_dist = math::squared_length(L);
  const auto squared_radius = shape.radius * shape.radius;
  const auto origin_inside_sphere = orig_center_sqrd_dist < squared_radius;

  // Sphere center is behind ray origin.
  if (tPX <= 0.0f && origin_inside_sphere == false) { return boost::none; }

  const auto dsq = orig_center_sqrd_dist - tPX * tPX;
  if (dsq > squared_radius) { return boost::none; }

  float t;
  if (tPX > 0.0f && origin_inside_sphere == false)
  {
    t = tPX - std::sqrt(squared_radius - dsq);
  }
  else
  {
    t = tPX + std::sqrt(squared_radius - dsq);
  }

  shapes::intersection_info info;
  info.point = math::translated(ray.origin, math::scaled(ray.direction, t));
  info.normal = math::normalized(math::make_vector3d(shape.center, info.point));
  return info;
}

}