/*

createDiffusionTensorImage.cxx
Bishesh Khanal
Asclepios, INRIA Sophia Antipolis

A test program to create diffusion tensor images of desired size.

*/

#include <iostream>
#include <string>
#include <sstream>
#include <boost/program_options.hpp>

#include <itkImage.h>
#include <itkImageFileReader.h>
#include <itkImageFileWriter.h>
#include <itkDiffusionTensor3D.h>

#undef __FUNCT__
#define __FUNCT__ "main"
int main(int argc, char **argv)
{
    std::string inRefFileName, outFileName;         //input reference file for origin and spacings; name of the output file to be created.
    std::string elements;       //elements of the tensor separted by comma, without spaces in lower triangular order.
    double D[6];            //six diffusion tensor elements, in LT order.

    //-------------- Set up the command line options-------------------------
    boost::program_options::options_description optionsDescription("Possible options");
    optionsDescription.add_options()
            ("help,h", "displays help message")
            ("reference,r", boost::program_options::value< std::string >(&inRefFileName), "input reference file")
            ("output,o", boost::program_options::value< std::string >(&outFileName), "output filename")
            ("elements,e",boost::program_options::value< std::string >(&elements), "elements separted by comma without any spaces in lower traingular order")
            ;

    boost::program_options::variables_map options;
    boost::program_options::store(boost::program_options::parse_command_line(argc,argv,optionsDescription),options);
    boost::program_options::notify(options);

    //If help is asked!
    if(options.count("help")) {
        std::cout<<optionsDescription<<std::endl;
        return EXIT_SUCCESS;
    }

    //Confirm all the options required are given.
    if(!options.count("reference") || !options.count("output") || !options.count("elements")) {
        std::cerr<<"invalid options! run with --help or -h to see the proper options."<<std::endl;
        return EXIT_FAILURE;
    }

    //Parse the elements into number from the string:
    {
        std::stringstream ss(elements);
        std::cout<<"what is in elements: "<<ss.str()<<std::endl;
        unsigned int i = 0;
        while(ss>>D[i++]) { //get the string until comma to first element of D and then increase i.
            if(ss.peek() == ',') //Still don't know why ss>> gives output upto comma, did not see
                ss.ignore();        //it in the documentation!
        }
    }

    //Check if the elements are properly parsed into D array.
    //for (int i=0;i<6;++i)
    //    std::cout<<std::endl<<D[i];

    //---------------------  Read the reference image type ----------------------//
    typedef itk::Image<double, 3>                       ScalarImageType;
    typedef itk::ImageFileReader<ScalarImageType>       ScalarImageReaderType;

    ScalarImageReaderType::Pointer reader = ScalarImageReaderType::New();
    reader->SetFileName(inRefFileName);
    reader->Update();
    ScalarImageType::Pointer refImg = reader->GetOutput();

    //-------------------- Tensor Image -------------------------------------//
    typedef itk::Image<itk::DiffusionTensor3D<double>, 3>       TensorImageType;
    TensorImageType::Pointer tensorImage = TensorImageType::New();

    //------------ Get important details from the reference image ------------//
    tensorImage->SetRegions(refImg->GetLargestPossibleRegion());
    tensorImage->SetOrigin(refImg->GetOrigin());
    tensorImage->SetSpacing(refImg->GetSpacing());
    tensorImage->SetDirection(refImg->GetDirection());

    // ----------- Allocate memory ---------------------------//
    tensorImage->Allocate();

    // Fill the voxels with the input tensor value
    tensorImage->FillBuffer(D);

    // Test how the values are stored:
    std::cout<<"origin: "<<tensorImage->GetOrigin()<<std::endl;
    std::cout<<"direction: "<<tensorImage->GetDirection()<<std::endl;
    TensorImageType::IndexType posTensor;
    for (unsigned int i = 0; i<3; ++i) posTensor.SetElement(i,10);
    std::cout<<"Tensor value: "<<tensorImage->GetPixel(posTensor)<<std::endl;
    std::cout<<"Tensor values again:\n"<<tensorImage->GetPixel(posTensor)(0,0)<<"\t"<<tensorImage->GetPixel(posTensor)(0,1)<<"\t"<<tensorImage->GetPixel(posTensor)(0,2)<<std::endl;
    std::cout<<tensorImage->GetPixel(posTensor)(1,0)<<"\t"<<tensorImage->GetPixel(posTensor)(1,1)<<"\t"<<tensorImage->GetPixel(posTensor)(1,2)<<std::endl;
    std::cout<<tensorImage->GetPixel(posTensor)(2,0)<<"\t"<<tensorImage->GetPixel(posTensor)(2,1)<<"\t"<<tensorImage->GetPixel(posTensor)(2,2)<<std::endl;

    //------------ Write output image -------------------------//
    typedef itk::ImageFileWriter<TensorImageType>       TensorImageWriterType;
    TensorImageWriterType::Pointer writer = TensorImageWriterType::New();
    writer->SetFileName(outFileName);
    writer->SetInput(tensorImage);
    writer->Update();

    return EXIT_SUCCESS;
}


////////////////////////////////////////////////////////////////////////////////////
//  Geometric utilities for multicamera calibration
//
//  Copyright (C) 2016  David Alejo, Fernando Caballero, Luis Merino
//  Universidad Pablo de Olavide
//  Seville, Spain.
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
////////////////////////////////////////////////////////////////////////////////////

#ifndef GEOMETRIC_UTILS__
#define GEOMETRIC_UTILS__

#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/concept_check.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

#ifndef  CALI_VEC_SIZE
#define CALI_VEC_SIZE 7
#endif

typedef boost::variate_generator<boost::mt19937, boost::normal_distribution<> > NormalDist;
static NormalDist norm_dist(boost::mt19937(time(0)), boost::normal_distribution<>(0, 1.0));

// --------------------------- General utilities ----------------------------------
double f_rand(double fMin = 0.0, double fMax = 1.0)
{
  static bool init = false;
  if (!init) {
    init = true;
    srand(time(NULL));
  }
  double f = (double)rand() / RAND_MAX;
  return fMin + f * (fMax - fMin);
}

std::vector<double> eigen2stdvec(const Eigen::Vector3d &v) {
  std::vector<double> ret;
  ret.resize(3);
  for (int i = 0; i < 3; i++) {
    ret[i] = v[i];
  }
  return ret;
}

// ----------------- Geometric utilities ------------------------------

//! Gets a transformation from a parameter
Eigen::Affine3d getTransform(const double *p)
{
  // Get the transformation matrix T from p vector
  Eigen::Affine3d T = Eigen::Affine3d::Identity();
  Eigen::Quaterniond R(p[0], p[1], p[2], p[3]);
  T.rotate(R);
  T.pretranslate(Eigen::Vector3d(p[4], p[5], p[6]));

  return T;
}

void decomposeTransform(const Eigen::Affine3d &T, Eigen::Quaterniond &q, Eigen::Vector3d &v) {
  Eigen::Quaterniond d(T.rotation( ));
  q = d;
  v[0] = T(0,3);
  v[1] = T(1,3);
  v[2] = T(2,3);
}

bool quat2rotvec(const Eigen::Quaterniond &q, Eigen::Vector3d &v) {
  double norm = sqrt(1 - q.w());
  if (norm < 1e-4) {
    // Too small rotation
    v[0] = v[1] = v[2] = 0.0;
    return false;
  }
  norm = 1 / norm;
  v[0] = q.x() * norm;
  v[1] = q.y() * norm;
  v[2] = q.z() * norm;

  return true;
}

Eigen::Quaterniond euler2quat(double roll, double pitch, double yaw) {
  Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d(0.0, 0.0, 1.0));
  Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d(1.0, 0.0, 0.0));
  Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d(0.0, 1.0, 0.0));
  Eigen::Quaterniond q =  yawAngle * pitchAngle * rollAngle; // 3-2-1

  return q;
}

// Returns the 3-2-1 body angles in [roll, pitch, yaw]
Eigen::Vector3d quat2euler(const Eigen::Quaterniond &q) {
  Eigen::Vector3d ret;
  double w = q.w(), x = q.x(), y = q.y(), z = q.z();

  ret[0] = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y));
  ret[1] = asin(0.9999999* (2*(w*y - z*x)));
//   cout << "x = " << asin(1.0) << endl;
  ret[2] = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z));

  return ret;
}

#endif


#include <boost/compute/algorithm/partition_point.hpp>


//==================================================================================================
/*
  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <algorithm>
#include <numeric>
#include <simd_test.hpp>

namespace bs = boost::simd;

template <typename T, std::size_t N, typename Env>
void test_cast(Env& $)
{
  std::array<T, N> a;
  std::iota(a.begin(), a.end(), T(1));
  bs::pack<T, N> p0(a.begin(), a.end()), ref;

  std::transform(p0.begin(),p0.end(),ref.begin(), [](T e) { return e; } );
  STF_EXPECT(( std::equal(ref.begin(),ref.end(), a.begin()) ));
}

template <typename T, std::size_t N, typename Env>
void test_is_equal(Env& $)
{
  std::array<T, N> a;
  std::iota(a.begin(), a.end(), T(1));
  bs::pack<T, N> p(a.begin(), a.end());

  STF_EXPECT(( std::all_of(p.begin() , p.end(), [](T e) { return e == e; } ) ));
}

template <typename T, std::size_t N, typename Env>
void test_not_equal(Env& $)
{
  std::array<T, N> a;
  std::iota(a.begin(), a.end(), T(1));
  bs::pack<T, N> p(a.begin(), a.end());

  STF_EXPECT(( std::none_of(p.begin() , p.end(), [](T e) { return e != e; } ) ));
}

template <typename T, std::size_t N, typename Env>
void test_not(Env& $)
{
  std::array<T, N> a;
  std::iota(a.begin(), a.end(), T(1));
  bs::pack<T, N> p0(a.begin(), a.end()), ref(a.begin(), a.end());

  bool supports_logical_not = true;
  for (auto it0 = p0.begin(); it0 != p0.end(); ++it0)
    supports_logical_not = supports_logical_not && !(!(*it0));

  STF_EXPECT( supports_logical_not );
}

template <typename T, std::size_t N, typename Env, typename Op>
void test_binop(Env& $, Op op)
{
  std::array<T, N> a;
  std::iota(a.begin(), a.end(), T(1));
  bs::pack<T, N> p0(a.begin(), a.end()), ref;

  std::transform(p0.begin(),p0.end(),p0.begin(),ref.begin(), op );
  std::transform(a.begin() ,a.end() ,a.begin() ,a.begin()  , op );
  STF_EXPECT(( std::equal(ref.begin(),ref.end(), a.begin()) ));
}

template <typename T, std::size_t N, typename Env, typename Op>
void test_selfop(Env& $, Op op)
{
  std::array<T, N> a;
  std::iota(a.begin(), a.end(), T(1));
  bs::pack<T, N> p0(a.begin(), a.end());

  for(std::size_t i=0;i<N;++i)
  {
    op(p0[i],p0[i]);
    op(a[i] ,a[i]);
  }

  STF_EXPECT(( std::equal(p0.begin(),p0.end(), a.begin()) ));
}

STF_CASE_TPL( "Check proxy access" , STF_NUMERIC_TYPES)
{
  test_cast<T,  2>($);
  test_cast<T,  4>($);
  test_cast<T,  8>($);
  test_cast<T, 16>($);
  test_cast<T, 32>($);
}

STF_CASE_TPL( "Check proxy comparison" , STF_NUMERIC_TYPES)
{
  test_is_equal<T,  2>($);
  test_is_equal<T,  4>($);
  test_is_equal<T,  8>($);
  test_is_equal<T, 16>($);
  test_is_equal<T, 32>($);
}

STF_CASE_TPL( "Check proxy non equal comparison" , STF_NUMERIC_TYPES)
{
  test_not_equal<T,  2>($);
  test_not_equal<T,  4>($);
  test_not_equal<T,  8>($);
  test_not_equal<T, 16>($);
  test_not_equal<T, 32>($);
}

STF_CASE_TPL( "Check proxy bitwise negation" , STF_NUMERIC_TYPES)
{
  test_not<T,  2>($);
  test_not<T,  4>($);
  test_not<T,  8>($);
  test_not<T, 16>($);
  test_not<T, 32>($);
}

STF_CASE_TPL( "Check proxy + operator" , STF_NUMERIC_TYPES)
{
  std::plus<T> op;
  test_binop<T,  2>($,op);
  test_binop<T,  4>($,op);
  test_binop<T,  8>($,op);
  test_binop<T, 16>($,op);
  test_binop<T, 32>($,op);
}

STF_CASE_TPL( "Check proxy - operator" , STF_NUMERIC_TYPES)
{
  std::minus<T> op;
  test_binop<T,  2>($,op);
  test_binop<T,  4>($,op);
  test_binop<T,  8>($,op);
  test_binop<T, 16>($,op);
  test_binop<T, 32>($,op);
}

STF_CASE_TPL( "Check proxy * operator" , STF_NUMERIC_TYPES)
{
  std::multiplies<T> op;
  test_binop<T,  2>($,op);
  test_binop<T,  4>($,op);
  test_binop<T,  8>($,op);
  test_binop<T, 16>($,op);
  test_binop<T, 32>($,op);
}

STF_CASE_TPL( "Check proxy / operator" , STF_NUMERIC_TYPES)
{
  std::divides<T> op;
  test_binop<T,  2>($,op);
  test_binop<T,  4>($,op);
  test_binop<T,  8>($,op);
  test_binop<T, 16>($,op);
  test_binop<T, 32>($,op);
}

struct op_plus
{
  template<typename T, typename U> void operator()(T&& out, U const& in) const
  {
    std::forward<T>(out) += in;
  }
};

struct op_minus
{
  template<typename T, typename U> void operator()(T&& out, U const& in) const
  {
    std::forward<T>(out) -= in;
  }
};

struct op_times
{
  template<typename T, typename U> void operator()(T&& out, U const& in) const
  {
    std::forward<T>(out) *= in;
  }
};

struct op_divides
{
  template<typename T, typename U> void operator()(T&& out, U const& in) const
  {
    std::forward<T>(out) /= in;
  }
};

STF_CASE_TPL( "Check proxy += operator" , STF_NUMERIC_TYPES)
{
  op_plus op;

  test_selfop<T,  2>($,op);
  test_selfop<T,  4>($,op);
  test_selfop<T,  8>($,op);
  test_selfop<T, 16>($,op);
  test_selfop<T, 32>($,op);
}

STF_CASE_TPL( "Check proxy -= operator" , STF_NUMERIC_TYPES)
{
  op_minus op;

  test_selfop<T,  2>($,op);
  test_selfop<T,  4>($,op);
  test_selfop<T,  8>($,op);
  test_selfop<T, 16>($,op);
  test_selfop<T, 32>($,op);
}

STF_CASE_TPL( "Check proxy *= operator" , STF_NUMERIC_TYPES)
{
  op_times op;

  test_selfop<T,  2>($,op);
  test_selfop<T,  4>($,op);
  test_selfop<T,  8>($,op);
  test_selfop<T, 16>($,op);
  test_selfop<T, 32>($,op);
}

STF_CASE_TPL( "Check proxy /= operator" , STF_NUMERIC_TYPES)
{
  op_divides op;

  test_selfop<T,  2>($,op);
  test_selfop<T,  4>($,op);
  test_selfop<T,  8>($,op);
  test_selfop<T, 16>($,op);
  test_selfop<T, 32>($,op);
}


#pragma once

// system includes ------------------------------------------------------------
#include <Eigen/Sparse>
#include <boost/iterator/filter_iterator.hpp>
#include <boost/mpl/identity.hpp>
#include <functional>
#include <type_traits>
#include <iterator>
#include <tuple>

// own includes ------------------------------------------------------------
#include "spectral/basis/spectral_basis.hpp"
#include "spectral/basis/spectral_elem_accessor.hpp"
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"

namespace boltzmann {

namespace spectral {

// ----------------------------------------------------------------------
template <typename BASIS>
unsigned int
get_max_l(const BASIS& basis)
{
  typedef typename std::tuple_element<0, typename BASIS::elem_t::container_t>::type elem_t;
  // radial basis
  typename BASIS::elem_t::Acc::template get<elem_t> get_xir;
  unsigned int maxL = 0;
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    unsigned int l = get_xir(*it).get_id().l;
    if (l > maxL) maxL = l;
  }

  return maxL;
}

// ----------------------------------------------------------------------
/**
 * @Brief Return the max polynomial degree for the Polar-Laguerre basis
 *
 * @param basis
 *
 * @return
 */
template <typename BASIS>
typename std::enable_if<std::is_same<typename BASIS::elem_t::container_t,
                                     typename ::boltzmann::SpectralBasisFactoryKS::elem_t::container_t
                                     >::value,
                        unsigned int>::type
get_max_k(const BASIS& basis)
{
  typedef typename std::tuple_element<1, typename BASIS::elem_t::container_t>::type elem_t;
  typename BASIS::elem_t::Acc::template get<elem_t> get_phi;
  unsigned int maxK = 0;
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    unsigned int k = get_phi(*it).get_id().k;
    if (k > maxK) maxK = k;
  }
  return maxK;
}

// ----------------------------------------------------------------------
/**
 * @Brief Return the max polynomial degree for the Polar-Laguerre basis
 *
 * @param basis
 *
 * @return
 */
template <typename BASIS>
typename std::enable_if<std::is_same<typename BASIS::elem_t::container_t,
                                     typename ::boltzmann::SpectralBasisFactoryKS::elem_t::container_t
                                     >::value,
                        unsigned int>::type
get_K(const BASIS& basis)
{
  typedef typename std::tuple_element<1, typename BASIS::elem_t::container_t>::type elem_t;
  typename BASIS::elem_t::Acc::template get<elem_t> get_phi;
  unsigned int maxK = 0;
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    unsigned int k = get_phi(*it).get_id().k;
    if (k > maxK) maxK = k;
  }
  return maxK+1;
}

// ----------------------------------------------------------------------
/**
 * @Brief Return the max polynomial degree for the Hermite basis
 *
 * @param basis
 *
 * @return
 */
template <typename BASIS>
typename std::enable_if<std::is_same<typename BASIS::elem_t::container_t,
                                     typename ::boltzmann::SpectralBasisFactoryHN::elem_t::container_t
                                     >::value,
                        unsigned int>::type
get_K(const BASIS& basis)
{
  typedef typename std::tuple_element<0, typename BASIS::elem_t::container_t>::type hx_t;
  typedef typename std::tuple_element<1, typename BASIS::elem_t::container_t>::type hy_t;
  typename BASIS::elem_t::Acc::template get<hx_t> get_hx;
  unsigned int maxKx = 0;
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    unsigned int k = get_hx(*it).get_id().k;
    if (k > maxKx) maxKx = k;
  }

  typename BASIS::elem_t::Acc::template get<hy_t> get_hy;
  unsigned int maxKy = 0;
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    unsigned int k = get_hx(*it).get_id().k;
    if (k > maxKy) maxKy = k;
  }

  return std::max(maxKx, maxKy)+1;
}

#ifdef USE_CXX14
/**
 * @brief return boost::filter_iterator with basis functions that have angular frequency \f$ l \f$.
 *
 * @param begin
 * @param end
 * @param l    angular frequency
 *
 * @return
 */
template <typename ITERATOR>
auto
filter_freq(const ITERATOR& begin,
            const typename boost::mpl::identity<ITERATOR>::type& end,
            unsigned int l)
{
  typedef typename std::iterator_traits<ITERATOR>::value_type elem_t;

  typedef typename std::tuple_element<0, typename elem_t::container_t>::type aelem_t;

  auto f = [l](const elem_t& elem) {
    typename elem_t::Acc::template get<aelem_t> get_xir;
    return get_xir(elem).get_id().l == l;
  };

  return std::make_tuple(boost::make_filter_iterator(f, begin, end),
                         boost::make_filter_iterator(f, end, end));
}

/**
 * @brief return boost::filter_iterator with basis functions that total polynomial degree \f$ k \f$.
 *
 * @param begin
 * @param end
 * @param k    polynomial degree
 *
 * @return
 */
template <typename ITERATOR>
auto
filter_deg(const ITERATOR& begin,
           const typename boost::mpl::identity<ITERATOR>::type& end,
           unsigned int k)
{
  typedef typename std::iterator_traits<ITERATOR>::value_type elem_t;

  typedef typename std::tuple_element<1, typename elem_t::container_t>::type aelem_t;

  auto f = [k](const elem_t& elem) {
    typename elem_t::Acc::template get<aelem_t> get_lag;
    return get_lag(elem).get_id().k == k;
  };

  return std::make_tuple(boost::make_filter_iterator(f, begin, end),
                         boost::make_filter_iterator(f, end, end));
}
#endif  // USE_CXX14

}  // end namespace spectral_basis
}  // end namespace boltzmann


#include <vector>
#include <string>
#include <iostream>
#include <sstream>
#include <memory>

#include <boost/lexical_cast.hpp>

struct Token
{
    enum Type { integer, plus, minus, lparen, rparen } type;

    std::string text;

    friend std::ostream& operator<<(std::ostream& oss, const Token& token)
    {
        return oss << "'" << token.text << "'";
    }
};

std::vector<Token> lex(const std::string& input)
{
    std::vector<Token> result;

    for(size_t i = 0; i < input.size(); i++)
    {
        switch(input[i])
        {
            case '+':
                result.push_back( { Token::plus, "+" } );
                break;
            case '-':
                result.push_back( { Token::minus, "-" } );
                break;
            case '(':
                result.push_back( { Token::lparen, "(" } );
                break;
            case ')':
                result.push_back( { Token::rparen, ")" } );
                break;
            default:
                std::ostringstream buffer;
                buffer << input[i];
                for(size_t j = i + 1; j < input.size(); j++)
                {
                    if(std::isdigit(input[j]))
                    {
                        buffer << input[j];
                        ++i;
                    }
                    else
                    {
                        break;
                    }
                }

                result.push_back( { Token::integer, buffer.str() } );
        }
    }

    return result;
}

struct Element
{
    using Ptr = std::shared_ptr<Element>;

    virtual int eval() const = 0;
};

struct Integer : Element
{
    int value;

    Integer(int value) : value (value) {};

    int eval() const override
    {
        return value;
    }
};

struct BinaryOp : Element
{
    enum Type { addition, substraction } type;

    Element::Ptr lhs;
    Element::Ptr rhs;

    int eval() const override
    {
        if(type == addition)
        {
            return lhs->eval() + rhs->eval();
        }
        else
        {
            return lhs->eval() - rhs->eval();
        }
    }
};

Element::Ptr parse(const std::vector<Token>& tokens)
{
    auto result = std::make_shared<BinaryOp>();

    bool have_lhs { false };

    for(size_t i = 0; i < tokens.size(); i++)
    {
        auto token = tokens[i];

        switch(token.type)
        {
            case Token::integer:
            {
                int value = boost::lexical_cast<int>(token.text);
                if(!have_lhs)
                {
                    result->lhs = std::make_shared<Integer>(value);
                    have_lhs = true;
                }
                else
                {
                    result->rhs = std::make_shared<Integer>(value);
                }

            }
                break;
            case Token::plus:
                result->type = BinaryOp::addition;
                break;
            case Token::minus:
                result->type = BinaryOp::substraction;
                break;
            case Token::lparen:
            {
                size_t j = i;
                for(; j < tokens.size(); ++j)
                {
                    if(tokens[j].type == Token::rparen)
                    {
                        break;
                    }
                }

                std::vector<Token> subexpression(&tokens[i + 1], &tokens[j]);
                auto element = parse(subexpression);

                if(!have_lhs)
                {
                    result->lhs = element;
                    have_lhs = true;
                }
                else
                {
                    result->rhs = element;
                }

                i = j;
            }
                break;
            case Token::rparen:
                break;
        }
    }

    return result;
}

int main()
{
    std::string input { "(13-4)-(12+1)" };

    auto tokens = lex(input);

    for(auto& token : tokens)
    {
        std::cout << token << " ";
    }
    std::cout << "\n";

    try
    {
        auto parsed = parse(tokens);
        std::cout << input << " = " << parsed->eval() << std::endl;
    }
    catch(const std::exception& ex)
    {
        std::cout << ex.what() << std::endl;
    }

    return 0;
}


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

#include <boost/hana/assert.hpp>
#include <boost/hana/integral_constant.hpp>

#include "matrix/comparable.hpp"
using namespace boost::hana;
using namespace cppcon;

int main() {
    // transpose
    {
        auto m = matrix(
            row(1, 2.2, '3'),
            row(4, '5', 6)
        );

        BOOST_HANA_CONSTEXPR_CHECK(transpose(m) ==
            matrix(
                row(1, 4),
                row(2.2, '5'),
                row('3', 6)
            )
        );
    }

    // vector
    {
        auto v = vector(1, '2', int_<3>, 4.2f);
        BOOST_HANA_CONSTEXPR_CHECK(v.size() == 4ul);
        BOOST_HANA_CONSTEXPR_CHECK(v.nrows() == 4ul);
        BOOST_HANA_CONSTEXPR_CHECK(v.ncolumns() == 1ul);
    }

    // matrix.at
    {
        auto m = matrix(
            row(1, '2', 3),
            row('4', char_<'5'>, 6),
            row(int_<7>, '8', 9.3)
        );
        BOOST_HANA_CONSTEXPR_CHECK(m.at(int_<0>, int_<0>) == 1);
        BOOST_HANA_CONSTEXPR_CHECK(m.at(int_<0>, int_<1>) == '2');
        BOOST_HANA_CONSTEXPR_CHECK(m.at(int_<0>, int_<2>) == 3);

        BOOST_HANA_CONSTEXPR_CHECK(m.at(int_<1>, int_<0>) == '4');
        BOOST_HANA_CONSTANT_CHECK(m.at(int_<1>, int_<1>) == char_<'5'>);
        BOOST_HANA_CONSTEXPR_CHECK(m.at(int_<1>, int_<2>) == 6);

        BOOST_HANA_CONSTANT_CHECK(m.at(int_<2>, int_<0>) == int_<7>);
        BOOST_HANA_CONSTEXPR_CHECK(m.at(int_<2>, int_<1>) == '8');
        BOOST_HANA_CONSTEXPR_CHECK(m.at(int_<2>, int_<2>) == 9.3);
    }

    // size, ncolumns, nrows
    {
        auto m = matrix(
            row(1, '2', 3),
            row('4', char_<'5'>, 6)
        );
        BOOST_HANA_CONSTEXPR_CHECK(m.size() == 6ul);
        BOOST_HANA_CONSTEXPR_CHECK(m.ncolumns() == 3ul);
        BOOST_HANA_CONSTEXPR_CHECK(m.nrows() == 2ul);
    }
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::int64_t, Eigen::Dynamic, 4> test_long_D_4(const Eigen::Matrix<boost::int64_t, Eigen::Dynamic, 4> & M)
{
	return M;
}
void export_long_D_4()
{
	boost::python::def("test_long_D_4",test_long_D_4);
}


//==================================================================================================
/*!
  @file

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/pow.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/eight.hpp>
#include <boost/simd/constant/third.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/true.hpp>
#include <boost/simd/function/is_negative.hpp>
#include <boost/simd/function/is_positive.hpp>

template <typename T, std::size_t N, typename Env>
void test(Env& runtime)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T, N>;

  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  T a1[N], a2[N], b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
     a1[i] = (i%2) ? T(i) : T(-i);
     a2[i] = (i%2) ? T(i+N) : T(-(i+N));
     b[i] = bs::pow(a1[i], a2[i]);
   }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t aa2(&a2[0], &a2[0]+N);
  p_t bb(&b[0], &b[0]+N);
  STF_IEEE_EQUAL(bs::pow(aa1, aa2), bb);

}

STF_CASE_TPL("Check pow on pack" , STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}

STF_CASE_TPL("pow",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::pow;
  using p_t = bs::pack<T>;
  using r_t =  decltype(pow(p_t(), p_t()));

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

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(pow(bs::Inf<p_t>(), bs::Inf<p_t>()), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(pow(bs::Nan<p_t>(), bs::Nan<p_t>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(pow(bs::Minf<p_t>(), bs::Minf<p_t>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(pow(bs::Inf<p_t>(), bs::Minf<p_t>()), bs::Zero<r_t>(), 0);
#endif
  STF_ULP_EQUAL(pow(p_t(-1),p_t(6)), p_t(1), 0);
  STF_ULP_EQUAL(pow(bs::Mone<p_t>(), bs::Mone<p_t>()), bs::Mone<r_t>(), 0);
  STF_ULP_EQUAL(pow(bs::One<p_t>(), bs::One<p_t>()), bs::One<r_t>(), 0);
  STF_ULP_EQUAL(pow(bs::Zero<p_t>(), bs::Zero<p_t>()), bs::One<r_t>(), 0);
  STF_ULP_EQUAL(pow(p_t(-1),p_t(5)), p_t(-1), 0);
  STF_ULP_EQUAL(pow(bs::Zero<p_t>(), bs::One<p_t>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(pow(p_t(8),bs::Third<p_t>()), r_t(2), 0.5);
}

STF_CASE_TPL("powreal_int",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::pow;
  using p_t = bs::pack<T>;
  using ip_t = bd::as_integer_t<p_t>;
  using r_t =  decltype(pow(p_t(), ip_t()));

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

  // specific values tests
  #ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(pow(bs::Inf<p_t>(),3), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(pow(bs::Minf<p_t>(),3), bs::Minf<r_t>(), 0);
  STF_ULP_EQUAL(pow(bs::Nan<p_t>(),3), bs::Nan<r_t>(), 0);
  #endif

  STF_ULP_EQUAL(pow(bs::Two<p_t>(),-3), (bs::Ratio<r_t, 1, 8>()),  0);
 }

STF_CASE_TPL("pow conformity",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::pow;
  using p_t = bs::pack<T>;
  using r_t =  decltype(pow(p_t(), p_t()));

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

  // specific values tests
   STF_ULP_EQUAL(pow(p_t(0), p_t(-1)), bs::Inf<r_t>(), 0);
   STF_ULP_EQUAL(pow(-p_t(0), p_t(-1)), bs::Minf<r_t>(), 0);
   STF_ULP_EQUAL(pow(-p_t(0), p_t(-2)), bs::Inf<p_t>(), 0);
   STF_ULP_EQUAL(pow(p_t(0), p_t(-2)), bs::Inf<p_t>(), 0);
   STF_ULP_EQUAL(pow(p_t(0),  bs::Minf<p_t>()),  bs::Inf<p_t>(), 0);
   STF_ULP_EQUAL(pow(-p_t(0),  bs::Minf<p_t>()),  bs::Inf<p_t>(), 0);
   STF_ULP_EQUAL(pow(-p_t(1),  bs::Minf<p_t>()),  bs::One<p_t>(), 0);
   STF_ULP_EQUAL(pow(-p_t(1),  bs::Inf <p_t>()),  bs::One<p_t>(), 0);
   STF_ULP_EQUAL(pow(p_t(1), bs::Nan<p_t>()) ,  bs::One<p_t>(), 0);
   STF_ULP_EQUAL(pow(bs::Nan<p_t>(), p_t(0)) ,  bs::One<p_t>(), 0);
   STF_ULP_EQUAL(pow(bs::Nan<p_t>(), -p_t(0)) ,  bs::One<p_t>(), 0);
   STF_ULP_EQUAL(pow(p_t(0.5), bs::Inf<p_t>()),  bs::Zero<p_t>(), 0);
   STF_ULP_EQUAL(pow(p_t(2), bs::Inf<p_t>()),  bs::Inf<p_t>(), 0);
   STF_ULP_EQUAL(pow(p_t(0.5), bs::Minf<p_t>()),  bs::Inf<p_t>(), 0);
   STF_ULP_EQUAL(pow(p_t(2), bs::Minf<p_t>()),  bs::Zero<p_t>(), 0);
   STF_ULP_EQUAL(pow(-p_t(0.5), bs::Inf<p_t>()),  bs::Zero<p_t>(), 0);

   STF_ULP_EQUAL(pow(-p_t(2), bs::Inf<p_t>()),  bs::Inf<p_t>(), 0);
   STF_ULP_EQUAL(pow(-p_t(0.5), bs::Minf<p_t>()),  bs::Inf<p_t>(), 0);
   STF_ULP_EQUAL(pow(-p_t(2), bs::Minf<p_t>()),  bs::Zero<p_t>(), 0);
   STF_ULP_EQUAL(pow(bs::Minf<p_t>(), p_t(-3) ),  bs::Mzero<p_t>(), 0);
   STF_EQUAL(bs::is_negative(pow(bs::Minf<p_t>(), p_t(-3) )), bs::True<p_t>());
   STF_ULP_EQUAL(pow(bs::Minf<p_t>(), p_t(-4) ),  bs::Zero<p_t>(), 0);
   STF_EQUAL(bs::is_positive(pow(bs::Minf<p_t>(), p_t(-4) )), bs::True<p_t>());
   STF_ULP_EQUAL(pow(bs::Inf<p_t>(), p_t(4) ),  bs::Inf<p_t>(), 0);
   STF_ULP_EQUAL(pow(bs::Inf<p_t>(), p_t(-4) ),  bs::Zero<p_t>(), 0);
}


/*=============================================================================
    Copyright (c) 2001-2007 Joel de Guzman
    Copyright (c) 2001-2009 Hartmut Kaiser

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#include <boost/config/warning_disable.hpp>
#include <boost/detail/lightweight_test.hpp>
#include <boost/spirit/include/karma_char.hpp>
#include <boost/spirit/include/karma_auxiliary.hpp>
#include <boost/spirit/include/karma_action.hpp>
#include <boost/spirit/include/support_argument.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>

#include <iostream>
#include "test.hpp"

///////////////////////////////////////////////////////////////////////////////
struct number_generator : public boost::spirit::karma::functor_base
{
    template <typename Context>
    struct apply
    {
        typedef int type;
    };

    template <typename Parameter, typename Context, typename OutputIterator>
    bool operator()(Parameter v, Context& ctx, OutputIterator& sink) const
    {
        char ch = v % 10 + '0';
        v /= 10;

        if (0 != v)
            (*this)(v, ctx, sink);

        *sink = ch;
        ++sink;
        return true;
    }
};

boost::spirit::karma::functor_generator<number_generator> number;

///////////////////////////////////////////////////////////////////////////////
int main()
{
    using spirit_test::test;
    using namespace boost::spirit;
    using namespace boost::spirit::karma;

    {
        BOOST_TEST(test("0", number));
        BOOST_TEST(test("1234", number, 1234));
    }

    return boost::report_errors();
}


// Copyright 2018 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/config/workaround.hpp>
#if BOOST_WORKAROUND(BOOST_CLANG, >= 1)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wlanguage-extension-token"
#endif
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/length.hpp>
#if BOOST_WORKAROUND(BOOST_CLANG, >= 1)
#pragma clang diagnostic pop
#endif
#include <boost/core/lightweight_test.hpp>
#include <boost/histogram/axis/ostream.hpp>
#include <boost/histogram/axis/regular.hpp>
#include <boost/histogram/detail/throw_exception.hpp>
#include <boost/histogram/histogram.hpp>
#include <boost/histogram/indexed.hpp>
#include <boost/histogram/literals.hpp>
#include <boost/histogram/make_histogram.hpp>
#include <boost/histogram/ostream.hpp>
#include <limits>
#include "is_close.hpp"

using namespace boost::histogram;
using namespace boost::histogram::literals;
namespace tr = axis::transform;

int main() {
  using namespace boost::units;
  using Q = quantity<si::length>;

  // axis::regular with quantity
  {
    axis::regular<Q> b(2, 0 * si::meter, 2 * si::meter);
    BOOST_TEST_EQ(b.bin(-1).lower() / si::meter,
                  -std::numeric_limits<double>::infinity());
    BOOST_TEST_IS_CLOSE(b.bin(0).lower() / si::meter, 0.0, 1e-9);
    BOOST_TEST_IS_CLOSE(b.bin(1).lower() / si::meter, 1.0, 1e-9);
    BOOST_TEST_IS_CLOSE(b.bin(2).lower() / si::meter, 2.0, 1e-9);
    BOOST_TEST_EQ(b.bin(2).upper() / si::meter, std::numeric_limits<double>::infinity());

    BOOST_TEST_EQ(b.index(-std::numeric_limits<double>::infinity() * si::meter), -1);
    BOOST_TEST_EQ(b.index(-1 * si::meter), -1); // produces NaN in conversion
    BOOST_TEST_EQ(b.index(0 * si::meter), 0);
    BOOST_TEST_EQ(b.index(0.99 * si::meter), 0);
    BOOST_TEST_EQ(b.index(1 * si::meter), 1);
    BOOST_TEST_EQ(b.index(1.99 * si::meter), 1);
    BOOST_TEST_EQ(b.index(2 * si::meter), 2);
    BOOST_TEST_EQ(b.index(100 * si::meter), 2);
    BOOST_TEST_EQ(b.index(std::numeric_limits<double>::infinity() * si::meter), 2);
  }

  // axis::regular with quantity and transform
  {
    axis::regular<Q, tr::log> b(2, 1 * si::meter, 10 * si::meter);
    BOOST_TEST_EQ(b.value(-1) / si::meter, 0);
    BOOST_TEST_IS_CLOSE(b.value(0) / si::meter, 1.0, 1e-9);
    BOOST_TEST_IS_CLOSE(b.value(1) / si::meter, 3.1623, 1e-3);
    BOOST_TEST_IS_CLOSE(b.value(2) / si::meter, 10.0, 1e-9);
    BOOST_TEST_EQ(b.value(3) / si::meter, std::numeric_limits<double>::infinity());
  }

  // histogram with quantity axis
  {
    auto h = make_histogram(axis::regular<Q>(2, 0 * si::meter, 1 * si::meter),
                            axis::regular<>(2, 0, 1));
    h(0.1 * si::meter, 0.1); // fills bin (0, 0)
    BOOST_TEST_EQ(h.at(0, 0), 1);
    for (auto&& x : indexed(h)) {
      BOOST_TEST_THROWS(x.density(), std::runtime_error); // cannot use density method
      BOOST_TEST_EQ(x.index(0), 2.0 * x.bin(0_c).lower() / si::meter);
      BOOST_TEST_EQ(x.index(1), 2.0 * x.bin(1_c).lower());
    }
  }

  return boost::report_errors();
}


// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
//       its contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: Johannes L. Schoenberger (jsch at inf.ethz.ch)

#define TEST_NAME "base/pose"
#include "util/testing.h"

#include <Eigen/Core>

#include "base/pose.h"
#include "base/projection.h"
#include "util/math.h"

using namespace colmap;

BOOST_AUTO_TEST_CASE(TestCrossProductMatrix) {
  BOOST_CHECK_EQUAL(CrossProductMatrix(Eigen::Vector3d(0, 0, 0)),
                    Eigen::Matrix3d::Zero());
  Eigen::Matrix3d ref_matrix;
  ref_matrix << 0, -3, 2, 3, 0, -1, -2, 1, 0;
  BOOST_CHECK_EQUAL(CrossProductMatrix(Eigen::Vector3d(1, 2, 3)), ref_matrix);
}

BOOST_AUTO_TEST_CASE(TestEulerAnglesX) {
  const double rx = 0.3;
  const double ry = 0;
  const double rz = 0;
  double rxx, ryy, rzz;

  RotationMatrixToEulerAngles(EulerAnglesToRotationMatrix(rx, ry, rz), &rxx,
                              &ryy, &rzz);

  BOOST_CHECK_CLOSE(rx, rxx, 1e-6);
  BOOST_CHECK_CLOSE(ry, ryy, 1e-6);
  BOOST_CHECK_CLOSE(rz, rzz, 1e-6);
}

BOOST_AUTO_TEST_CASE(TestEulerAnglesY) {
  const double rx = 0;
  const double ry = 0.3;
  const double rz = 0;
  double rxx, ryy, rzz;

  RotationMatrixToEulerAngles(EulerAnglesToRotationMatrix(rx, ry, rz), &rxx,
                              &ryy, &rzz);

  BOOST_CHECK_CLOSE(rx, rxx, 1e-6);
  BOOST_CHECK_CLOSE(ry, ryy, 1e-6);
  BOOST_CHECK_CLOSE(rz, rzz, 1e-6);
}

BOOST_AUTO_TEST_CASE(TestEulerAnglesZ) {
  const double rx = 0;
  const double ry = 0;
  const double rz = 0.3;
  double rxx, ryy, rzz;

  RotationMatrixToEulerAngles(EulerAnglesToRotationMatrix(rx, ry, rz), &rxx,
                              &ryy, &rzz);

  BOOST_CHECK_CLOSE(rx, rxx, 1e-6);
  BOOST_CHECK_CLOSE(ry, ryy, 1e-6);
  BOOST_CHECK_CLOSE(rz, rzz, 1e-6);
}

BOOST_AUTO_TEST_CASE(TestQuaternionToRotationMatrix) {
  const double rx = 0;
  const double ry = 0;
  const double rz = 0.3;
  const Eigen::Matrix3d rot_mat0 = EulerAnglesToRotationMatrix(rx, ry, rz);
  const Eigen::Matrix3d rot_mat1 =
      QuaternionToRotationMatrix(RotationMatrixToQuaternion(rot_mat0));
  BOOST_CHECK(rot_mat0.isApprox(rot_mat1));
}

BOOST_AUTO_TEST_CASE(TestComposeIdentityQuaternion) {
  BOOST_CHECK_EQUAL(ComposeIdentityQuaternion(), Eigen::Vector4d(1, 0, 0, 0));
}

BOOST_AUTO_TEST_CASE(TestNormalizeQuaternion) {
  BOOST_CHECK_EQUAL(NormalizeQuaternion(ComposeIdentityQuaternion()),
                    ComposeIdentityQuaternion());
  BOOST_CHECK_EQUAL(NormalizeQuaternion(Eigen::Vector4d(2, 0, 0, 0)),
                    ComposeIdentityQuaternion());
  BOOST_CHECK_EQUAL(NormalizeQuaternion(Eigen::Vector4d(0.5, 0, 0, 0)),
                    ComposeIdentityQuaternion());
  BOOST_CHECK_EQUAL(NormalizeQuaternion(Eigen::Vector4d(0, 0, 0, 0)),
                    ComposeIdentityQuaternion());
  BOOST_CHECK(
      NormalizeQuaternion(Eigen::Vector4d(1, 1, 0, 0))
          .isApprox(Eigen::Vector4d(std::sqrt(2) / 2, std::sqrt(2) / 2, 0, 0)));
  BOOST_CHECK(
      NormalizeQuaternion(Eigen::Vector4d(0.5, 0.5, 0, 0))
          .isApprox(Eigen::Vector4d(std::sqrt(2) / 2, std::sqrt(2) / 2, 0, 0)));
}

BOOST_AUTO_TEST_CASE(TestInvertQuaternion) {
  BOOST_CHECK_EQUAL(InvertQuaternion(ComposeIdentityQuaternion()),
                    Eigen::Vector4d(1, -0, -0, -0));
  BOOST_CHECK_EQUAL(InvertQuaternion(Eigen::Vector4d(2, 0, 0, 0)),
                    Eigen::Vector4d(2, -0, -0, -0));
  BOOST_CHECK_EQUAL(
      InvertQuaternion(InvertQuaternion(Eigen::Vector4d(0.1, 0.2, 0.3, 0.4))),
      Eigen::Vector4d(0.1, 0.2, 0.3, 0.4));
}

BOOST_AUTO_TEST_CASE(TestConcatenateQuaternions) {
  BOOST_CHECK_EQUAL(ConcatenateQuaternions(ComposeIdentityQuaternion(),
                                           ComposeIdentityQuaternion()),
                    ComposeIdentityQuaternion());
  BOOST_CHECK_EQUAL(ConcatenateQuaternions(Eigen::Vector4d(2, 0, 0, 0),
                                           ComposeIdentityQuaternion()),
                    ComposeIdentityQuaternion());
  BOOST_CHECK_EQUAL(ConcatenateQuaternions(ComposeIdentityQuaternion(),
                                           Eigen::Vector4d(2, 0, 0, 0)),
                    ComposeIdentityQuaternion());
  BOOST_CHECK(ConcatenateQuaternions(
                  Eigen::Vector4d(0.1, 0.2, 0.3, 0.4),
                  InvertQuaternion(Eigen::Vector4d(0.1, 0.2, 0.3, 0.4)))
                  .isApprox(ComposeIdentityQuaternion()));
  BOOST_CHECK(ConcatenateQuaternions(
                  InvertQuaternion(Eigen::Vector4d(0.1, 0.2, 0.3, 0.4)),
                  Eigen::Vector4d(0.1, 0.2, 0.3, 0.4))
                  .isApprox(ComposeIdentityQuaternion()));
}

BOOST_AUTO_TEST_CASE(TestQuaternionRotatePoint) {
  BOOST_CHECK_EQUAL(QuaternionRotatePoint(ComposeIdentityQuaternion(),
                                          Eigen::Vector3d(0, 0, 0)),
                    Eigen::Vector3d(0, 0, 0));
  BOOST_CHECK_EQUAL(QuaternionRotatePoint(Eigen::Vector4d(0.1, 0, 0, 0),
                                          Eigen::Vector3d(0, 0, 0)),
                    Eigen::Vector3d(0, 0, 0));
  BOOST_CHECK_EQUAL(QuaternionRotatePoint(ComposeIdentityQuaternion(),
                                          Eigen::Vector3d(1, 1, 0)),
                    Eigen::Vector3d(1, 1, 0));
  BOOST_CHECK_EQUAL(QuaternionRotatePoint(Eigen::Vector4d(0.1, 0, 0, 0),
                                          Eigen::Vector3d(1, 1, 0)),
                    Eigen::Vector3d(1, 1, 0));
  BOOST_CHECK(
      QuaternionRotatePoint(
          RotationMatrixToQuaternion(EulerAnglesToRotationMatrix(M_PI, 0, 0)),
          Eigen::Vector3d(1, 1, 0))
          .isApprox(Eigen::Vector3d(1, -1, 0)));
}

BOOST_AUTO_TEST_CASE(TestAverageQuaternions) {
  std::vector<Eigen::Vector4d> qvecs;
  std::vector<double> weights;

  qvecs = {{ComposeIdentityQuaternion()}};
  weights = {1.0};
  BOOST_CHECK_EQUAL(AverageQuaternions(qvecs, weights),
                    ComposeIdentityQuaternion());

  qvecs = {ComposeIdentityQuaternion()};
  weights = {2.0};
  BOOST_CHECK_EQUAL(AverageQuaternions(qvecs, weights),
                    ComposeIdentityQuaternion());

  qvecs = {ComposeIdentityQuaternion(), ComposeIdentityQuaternion()};
  weights = {1.0, 1.0};
  BOOST_CHECK_EQUAL(AverageQuaternions(qvecs, weights),
                    ComposeIdentityQuaternion());

  qvecs = {ComposeIdentityQuaternion(), ComposeIdentityQuaternion()};
  weights = {1.0, 2.0};
  BOOST_CHECK_EQUAL(AverageQuaternions(qvecs, weights),
                    ComposeIdentityQuaternion());

  qvecs = {ComposeIdentityQuaternion(), Eigen::Vector4d(2, 0, 0, 0)};
  weights = {1.0, 2.0};
  BOOST_CHECK_EQUAL(AverageQuaternions(qvecs, weights),
                    ComposeIdentityQuaternion());

  qvecs = {ComposeIdentityQuaternion(), Eigen::Vector4d(1, 1, 0, 0)};
  weights = {1.0, 1.0};
  BOOST_CHECK(AverageQuaternions(qvecs, weights)
                  .isApprox(Eigen::Vector4d(0.92388, 0.382683, 0, 0), 1e-6));

  qvecs = {ComposeIdentityQuaternion(), Eigen::Vector4d(1, 1, 0, 0)};
  weights = {1.0, 2.0};
  BOOST_CHECK(AverageQuaternions(qvecs, weights)
                  .isApprox(Eigen::Vector4d(0.850651, 0.525731, 0, 0), 1e-6));
}

BOOST_AUTO_TEST_CASE(TestRotationFromUnitVectors) {
  BOOST_CHECK_EQUAL(RotationFromUnitVectors(Eigen::Vector3d(0, 0, 1),
                                            Eigen::Vector3d(0, 0, 1)),
                    Eigen::Matrix3d::Identity());
  BOOST_CHECK_EQUAL(RotationFromUnitVectors(Eigen::Vector3d(0, 0, 2),
                                            Eigen::Vector3d(0, 0, 2)),
                    Eigen::Matrix3d::Identity());

  Eigen::Matrix3d ref_matrix1;
  ref_matrix1 << 1, 0, 0, 0, 0, 1, 0, -1, 0;
  BOOST_CHECK_EQUAL(RotationFromUnitVectors(Eigen::Vector3d(0, 0, 1),
                                            Eigen::Vector3d(0, 1, 0)),
                    ref_matrix1);
  BOOST_CHECK_EQUAL(ref_matrix1 * Eigen::Vector3d(0, 0, 1),
                    Eigen::Vector3d(0, 1, 0));
  BOOST_CHECK_EQUAL(RotationFromUnitVectors(Eigen::Vector3d(0, 0, 2),
                                            Eigen::Vector3d(0, 2, 0)),
                    ref_matrix1);
  BOOST_CHECK_EQUAL(ref_matrix1 * Eigen::Vector3d(0, 0, 2),
                    Eigen::Vector3d(0, 2, 0));

  BOOST_CHECK_EQUAL(RotationFromUnitVectors(Eigen::Vector3d(0, 0, 1),
                                            Eigen::Vector3d(0, 0, -1)),
                    Eigen::Matrix3d::Identity());
}

BOOST_AUTO_TEST_CASE(TestPoseFromProjectionMatrix) {
  const Eigen::Vector4d qvec = Eigen::Vector4d::Random().normalized();
  const Eigen::Vector3d tvec(3, 4, 5);
  const Eigen::Matrix3x4d proj_matrix = ComposeProjectionMatrix(qvec, tvec);
  const Eigen::Matrix3x4d inv_proj_matrix = InvertProjectionMatrix(proj_matrix);
  const Eigen::Vector3d pose = ProjectionCenterFromMatrix(proj_matrix);
  BOOST_CHECK_CLOSE((inv_proj_matrix.rightCols<1>() - pose).norm(), 0, 1e-6);
}

BOOST_AUTO_TEST_CASE(TestPoseFromProjectionParameters) {
  const Eigen::Vector4d qvec = Eigen::Vector4d::Random().normalized();
  const Eigen::Vector3d tvec(3, 4, 5);
  const Eigen::Matrix3x4d proj_matrix = ComposeProjectionMatrix(qvec, tvec);
  const Eigen::Matrix3x4d inv_proj_matrix = InvertProjectionMatrix(proj_matrix);
  const Eigen::Vector3d pose = ProjectionCenterFromPose(qvec, tvec);
  BOOST_CHECK((inv_proj_matrix.rightCols<1>() - pose).norm() < 1e-6);
}

BOOST_AUTO_TEST_CASE(TestComputeRelativePose) {
  Eigen::Vector4d qvec12;
  Eigen::Vector3d tvec12;

  ComputeRelativePose(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
                      ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
                      &qvec12, &tvec12);
  BOOST_CHECK_EQUAL(qvec12, ComposeIdentityQuaternion());
  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(0, 0, 0));

  ComputeRelativePose(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
                      ComposeIdentityQuaternion(), Eigen::Vector3d(1, 0, 0),
                      &qvec12, &tvec12);
  BOOST_CHECK_EQUAL(qvec12, ComposeIdentityQuaternion());
  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(1, 0, 0));

  ComputeRelativePose(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
                      Eigen::Vector4d(1, 1, 0, 0), Eigen::Vector3d(0, 0, 0),
                      &qvec12, &tvec12);
  BOOST_CHECK(qvec12.isApprox(Eigen::Vector4d(0.707107, 0.707107, 0, 0), 1e-6));
  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(0, 0, 0));

  ComputeRelativePose(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
                      Eigen::Vector4d(1, 1, 0, 0), Eigen::Vector3d(1, 0, 0),
                      &qvec12, &tvec12);
  BOOST_CHECK(qvec12.isApprox(Eigen::Vector4d(0.707107, 0.707107, 0, 0), 1e-6));
  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(1, 0, 0));

  ComputeRelativePose(Eigen::Vector4d(1, 1, 0, 0), Eigen::Vector3d(0, 0, 0),
                      Eigen::Vector4d(1, 1, 0, 0), Eigen::Vector3d(1, 0, 0),
                      &qvec12, &tvec12);
  BOOST_CHECK(qvec12.isApprox(ComposeIdentityQuaternion()));
  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(1, 0, 0));

  ComputeRelativePose(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 1),
                      Eigen::Vector4d(1, 1, 0, 0), Eigen::Vector3d(0, 0, 0),
                      &qvec12, &tvec12);
  BOOST_CHECK(qvec12.isApprox(Eigen::Vector4d(0.707107, 0.707107, 0, 0), 1e-6));
  BOOST_CHECK(tvec12.isApprox(Eigen::Vector3d(0, 1, 0)));
}

BOOST_AUTO_TEST_CASE(TestConcatenatePoses) {
  Eigen::Vector4d qvec12;
  Eigen::Vector3d tvec12;

  ConcatenatePoses(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
                   ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
                   &qvec12, &tvec12);
  BOOST_CHECK_EQUAL(qvec12, ComposeIdentityQuaternion());
  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(0, 0, 0));

  ConcatenatePoses(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
                   ComposeIdentityQuaternion(), Eigen::Vector3d(0, 1, 2),
                   &qvec12, &tvec12);
  BOOST_CHECK_EQUAL(qvec12, ComposeIdentityQuaternion());
  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(0, 1, 2));

  ConcatenatePoses(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 1, 2),
                   ComposeIdentityQuaternion(), Eigen::Vector3d(3, 4, 5),
                   &qvec12, &tvec12);
  BOOST_CHECK_EQUAL(qvec12, ComposeIdentityQuaternion());
  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(3, 5, 7));

  Eigen::Vector4d rel_qvec12;
  Eigen::Vector3d rel_tvec12;
  ComputeRelativePose(Eigen::Vector4d(1, 1, 0, 0), Eigen::Vector3d(0, 1, 2),
                      Eigen::Vector4d(1, 3, 0, 0), Eigen::Vector3d(3, 4, 5),
                      &rel_qvec12, &rel_tvec12);
  ConcatenatePoses(Eigen::Vector4d(1, 1, 0, 0), Eigen::Vector3d(0, 1, 2),
                   rel_qvec12, rel_tvec12, &qvec12, &tvec12);
  BOOST_CHECK(
      qvec12.isApprox(NormalizeQuaternion(Eigen::Vector4d(1, 3, 0, 0))));
  BOOST_CHECK(tvec12.isApprox(Eigen::Vector3d(3, 4, 5)));
}

BOOST_AUTO_TEST_CASE(TestInvertPose) {
  Eigen::Vector4d inv_qvec;
  Eigen::Vector3d inv_tvec;
  InvertPose(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0), &inv_qvec,
             &inv_tvec);
  BOOST_CHECK_EQUAL(inv_qvec, ComposeIdentityQuaternion());
  BOOST_CHECK_EQUAL(inv_tvec, Eigen::Vector3d(0, 0, 0));
  InvertPose(Eigen::Vector4d(0, 1, 2, 3), Eigen::Vector3d(0, 1, 2), &inv_qvec,
             &inv_tvec);
  Eigen::Vector4d inv_inv_qvec;
  Eigen::Vector3d inv_inv_tvec;
  InvertPose(inv_qvec, inv_tvec, &inv_inv_qvec, &inv_inv_tvec);
  BOOST_CHECK_EQUAL(inv_inv_qvec, Eigen::Vector4d(0, 1, 2, 3));
  BOOST_CHECK(inv_inv_tvec.isApprox(Eigen::Vector3d(0, 1, 2)));
}

BOOST_AUTO_TEST_CASE(TestInterpolatePose) {
  const Eigen::Vector4d qvec1 = Eigen::Vector4d::Random().normalized();
  const Eigen::Vector3d tvec1 = Eigen::Vector3d::Random();
  const Eigen::Vector4d qvec2 = Eigen::Vector4d::Random().normalized();
  const Eigen::Vector3d tvec2 = Eigen::Vector3d::Random();

  Eigen::Vector4d qveci;
  Eigen::Vector3d tveci;

  InterpolatePose(qvec1, tvec1, qvec2, tvec2, 0, &qveci, &tveci);
  BOOST_CHECK(tvec1.isApprox(tveci));

  InterpolatePose(qvec1, tvec1, qvec2, tvec2, 1, &qveci, &tveci);
  BOOST_CHECK(tvec2.isApprox(tveci));

  InterpolatePose(qvec1, tvec1, qvec2, tvec2, 0.5, &qveci, &tveci);
  BOOST_CHECK(((tvec1 + tvec2) / 2).isApprox(tveci));
}

BOOST_AUTO_TEST_CASE(TestCalculateBaseline) {
  Eigen::Vector4d qvec1(1, 0, 0, 0);
  Eigen::Vector4d qvec2(1, 0, 0, 0);

  Eigen::Vector3d tvec1(0, 0, 0);
  Eigen::Vector3d tvec2(0, 0, 1);

  const double baseline1 = CalculateBaseline(qvec1, tvec1, qvec2, tvec2).norm();
  BOOST_CHECK_CLOSE(baseline1, 1, 1e-10);

  tvec2(2) = 2;

  const double baseline2 = CalculateBaseline(qvec1, tvec1, qvec2, tvec2).norm();
  BOOST_CHECK_CLOSE(baseline2, 2, 1e-10);
}

BOOST_AUTO_TEST_CASE(TestCheckCheirality) {
  const Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  const Eigen::Vector3d t(1, 0, 0);

  std::vector<Eigen::Vector2d> points1;
  std::vector<Eigen::Vector2d> points2;
  std::vector<Eigen::Vector3d> points3D;

  points1.emplace_back(0, 0);
  points2.emplace_back(0.1, 0);
  BOOST_CHECK(CheckCheirality(R, t, points1, points2, &points3D));
  BOOST_CHECK_EQUAL(points3D.size(), 1);

  points1.emplace_back(0, 0);
  points2.emplace_back(-0.1, 0);
  BOOST_CHECK(CheckCheirality(R, t, points1, points2, &points3D));
  BOOST_CHECK_EQUAL(points3D.size(), 1);

  points2[1][0] = 0.2;
  BOOST_CHECK(CheckCheirality(R, t, points1, points2, &points3D));
  BOOST_CHECK_EQUAL(points3D.size(), 2);

  points2[0][0] = -0.2;
  points2[1][0] = -0.2;
  BOOST_CHECK(!CheckCheirality(R, t, points1, points2, &points3D));
  BOOST_CHECK_EQUAL(points3D.size(), 0);
}


#include <boost/mp11/integer_sequence.hpp>


// Boost.Geometry
// Unit Test

// Copyright (c) 2017 Barend Gehrels, Amsterdam, the Netherlands.

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

#include <sstream>

#include <geometry_test_common.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include <boost/geometry/algorithms/correct_closure.hpp>
#include <boost/geometry/io/wkt/read.hpp>
#include <boost/geometry/io/wkt/write.hpp>

#include <boost/variant/variant.hpp>

template <typename BaseGeometry, typename Geometry>
void check_geometry(Geometry const& geometry, std::string const& expected)
{
    std::ostringstream out;
    out << bg::wkt_manipulator<Geometry>(geometry, false);
    BOOST_CHECK_EQUAL(out.str(), expected);
}

template <typename Geometry>
void test_geometry(std::string const& wkt, std::string const& expected)
{
    Geometry geometry;
    bg::read_wkt(wkt, geometry);

    // Test tye type
    bg::correct_closure(geometry);
    check_geometry<Geometry>(geometry, expected);

    // Test varianted type
    boost::variant<Geometry> v(geometry);
    bg::correct_closure(v);
    check_geometry<Geometry>(v, expected);
}

template <typename P>
void test_all()
{
    typedef bg::model::ring<P, true, true> cw_closed_ring_type;
    typedef bg::model::ring<P, true, false> cw_open_ring_type;
    typedef bg::model::ring<P, false, true> ccw_closed_ring_type;
    typedef bg::model::ring<P, false, false> ccw_open_ring_type;

    // Define clockwise and counter clockwise polygon
    std::string cw_ring       = "POLYGON((0 0,0 1,1 1,1 0,0 0))";
    std::string cw_open_ring  = "POLYGON((0 0,0 1,1 1,1 0))";

    std::string ccw_ring      = "POLYGON((0 0,1 0,1 1,0 1,0 0))";
    std::string ccw_open_ring = "POLYGON((0 0,1 0,1 1,0 1))";

    // Cases which should be closed or opened
    test_geometry<cw_closed_ring_type>(cw_open_ring, cw_ring);
    test_geometry<cw_open_ring_type>(cw_ring, cw_open_ring);
    test_geometry<ccw_closed_ring_type>(ccw_open_ring, ccw_ring);
    test_geometry<ccw_open_ring_type>(ccw_ring, ccw_open_ring);

    // Cases which are incorrect but should still be closed or opened
    test_geometry<cw_closed_ring_type>(ccw_open_ring, ccw_ring);
    test_geometry<ccw_open_ring_type>(cw_ring, cw_open_ring);

    // Cases where no action is necessary (even if order is incorrect)
    test_geometry<cw_closed_ring_type>(cw_ring, cw_ring);
    test_geometry<cw_closed_ring_type>(ccw_ring, ccw_ring);
    test_geometry<cw_open_ring_type>(cw_open_ring, cw_open_ring);
    test_geometry<cw_open_ring_type>(ccw_open_ring, ccw_open_ring);
    test_geometry<ccw_closed_ring_type>(cw_ring, cw_ring);
    test_geometry<ccw_closed_ring_type>(ccw_ring, ccw_ring);
    test_geometry<ccw_open_ring_type>(cw_open_ring, cw_open_ring);
    test_geometry<ccw_open_ring_type>(ccw_open_ring, ccw_open_ring);

    // Polygon cases
    std::string cw_polygon =
            "POLYGON((0 0,0 4,4 4,4 0,0 0),(1 1,2 1,2 2,1 2,1 1))";

    std::string cw_open_polygon =
            "POLYGON((0 0,0 4,4 4,4 0),(1 1,2 1,2 2,1 2))";

    typedef bg::model::polygon<P, true, true> cw_closed_polygon_type;
    typedef bg::model::polygon<P, true, false> cw_open_polygon_type;

    test_geometry<cw_closed_polygon_type>(cw_open_polygon, cw_polygon);
    test_geometry<cw_open_polygon_type>(cw_polygon, cw_open_polygon);

    test_geometry<cw_closed_polygon_type>(cw_polygon, cw_polygon);
    test_geometry<cw_open_polygon_type>(cw_open_polygon, cw_open_polygon);
}

int test_main(int, char* [])
{
    test_all<bg::model::d2::point_xy<int> >();
    test_all<bg::model::d2::point_xy<float> >();
    test_all<bg::model::d2::point_xy<double> >();

    test_all<bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree> > >();
    test_all<bg::model::point<double, 2, bg::cs::geographic<bg::degree> > >();

    return 0;
}


#include "SPlisHSPlasH/Common.h"
#include <Eigen/Dense>
#include <iostream>
#include "Utilities/Timing.h"
#include "Utilities/PartioReaderWriter.h"
#include "Utilities/OBJLoader.h"
#include "SPlisHSPlasH/Utilities/SurfaceSampling.h"
#include "Utilities/FileSystem.h"
#include "Utilities/StringTools.h"
#include "Utilities/Version.h"
#include "SPlisHSPlasH/TriangleMesh.h"
#include "extern/cxxopts/cxxopts.hpp"

// Enable memory leak detection
#ifdef _DEBUG
#ifndef EIGEN_ALIGN
	#define new DEBUG_NEW
#endif
#endif

INIT_TIMING
INIT_LOGGING

using namespace SPH;
using namespace Eigen;
using namespace Utilities;

void loadObj(const std::string &filename, TriangleMesh &mesh, const Vector3r &scale);

std::string inputFile = "";
std::string outputFile = "";
Real particleRadius = 0.025;
Vector3r scale = Vector3r::Ones();
unsigned int samplingMode = 1;
Vector3r translation = Vector3r::Zero();
Vector3r rotationAxis = Vector3r::UnitY();
Real angle = 0;

std::istream& operator >> (std::istream& istream, Vector3r& v)
{
	return istream >> std::skipws >> v[0] >> v[1] >> v[2];
}

// main
int main( int argc, char **argv )
{
	REPORT_MEMORY_LEAKS;

	std::cout << "Git refspec: " << GIT_REFSPEC << std::endl;
	std::cout << "Git SHA1: " << GIT_SHA1 << std::endl;
	std::cout << "Git status: " << GIT_LOCAL_STATUS << std::endl;

	try
	{
		cxxopts::Options options(argv[0], "SurfaceSampling - Sample a surface geometry given by an OBJ file.");

		options.add_options()
			("h,help", "Print help")
			("i,input", "Input file (obj)", cxxopts::value<std::string>())
			("o,output", "Output file (bgeo)", cxxopts::value<std::string>())
			("r,radius", "Particle radius", cxxopts::value<Real>()->default_value("0.025"))
			("s,scale", "Scaling of input geometry (e.g. --scale \"1 2 3\")", cxxopts::value<Vector3r>())
			("m,mode", "Sampling mode 0 Poisson disk, 1 Regular, 2 2D sampling", cxxopts::value<unsigned int>()->default_value("1"))
			("t,translation", "Translation for 2D sampling (default: \"0 0 0\")", cxxopts::value<Vector3r>())
			("rotationAxis", "Rotation axis for 2D sampling (default: \"0 1 0\")", cxxopts::value<Vector3r>())
			("a,angle", "Rotation angle for 2D simulation", cxxopts::value<Real>()->default_value("0"))
			;

		auto result = options.parse(argc, argv);

		if (result.count("help"))
		{
			std::cout << options.help({ "", "Group" }) << std::endl;
			exit(0);
		}

		if (result.count("input") && result.count("output"))
		{
			inputFile = result["input"].as<std::string>();
			std::cout << "Input = " << inputFile << std::endl;
			outputFile = result["output"].as<std::string>();
			std::cout << "Output = " << outputFile << std::endl;
		}
		else
		{
			std::cout << "Input or output missing!" << std::endl;
			std::cout << options.help({ "", "Group" }) << std::endl;
			exit(1);
		}

		if (result.count("radius"))
			particleRadius = result["radius"].as<Real>();
		std::cout << "Radius: " << particleRadius << std::endl;

		if (result.count("scale"))
			scale = result["scale"].as<Vector3r>();
		std::cout << "Scale: [" << scale.transpose() << "]^T" << std::endl;

		if (result.count("mode"))
			samplingMode = result["mode"].as<unsigned int>();
		std::cout << "Sampling mode: " << samplingMode << std::endl;

		if (result.count("translation"))
			translation = result["translation"].as<Vector3r>();
		if (result.count("rotationAxis"))
			rotationAxis = result["rotationAxis"].as<Vector3r>();
		if (result.count("angle"))
			angle = result["angle"].as<Real>();

		if (samplingMode == 2)
		{
			std::cout << "Translation:    [" << translation.transpose() << "]^T" << std::endl;
			std::cout << "Rotation axis:  [" << rotationAxis.transpose() << "]^T" << std::endl;
			std::cout << "Rotation angle: " << angle << std::endl;
		}
		else if (result.count("translation") || result.count("rotationAxis") || result.count("angle"))
		{
			std::cout << "--translation, --rotationAxis and --angle only affect 2D sampling and are ignored." << std::endl;
		}
	}
	catch (const cxxopts::OptionException& e)
	{
		std::cout << "error parsing options: " << e.what() << std::endl;
		exit(1);
	}

	TriangleMesh mesh;
	loadObj(inputFile, mesh, scale);

	std::vector<Vector3r> samplePoints;

	const auto samplePoissonDisk = [&]()
	{
		std::cout << "Poisson disk surface sampling of " << inputFile << std::endl;
		START_TIMING("Poisson disk sampling");
		PoissonDiskSampling sampling;
		sampling.sampleMesh(mesh.numVertices(), mesh.getVertices().data(), mesh.numFaces(), mesh.getFaces().data(), particleRadius, 10, 1, samplePoints);
		STOP_TIMING_PRINT;
	};
	const auto sampleRegularTriangle = [&]()
	{
		std::cout << "Regular triangle surface sampling of " << inputFile << std::endl;
		START_TIMING("Regular triangle sampling");
		RegularTriangleSampling sampling;
		sampling.sampleMesh(mesh.numVertices(), mesh.getVertices().data(), mesh.numFaces(), mesh.getFaces().data(), 1.5f * particleRadius, samplePoints);
		STOP_TIMING_PRINT;
	};
	const auto sampleRegular2D = [&]()
	{
		std::cout << "2D regular sampling of " << inputFile << std::endl;
		START_TIMING("2D regular sampling");
		RegularSampling2D sampling;
		sampling.sampleMesh(AngleAxisr(angle, rotationAxis).toRotationMatrix(), translation,
			mesh.numVertices(), mesh.getVertices().data(), mesh.numFaces(),
			mesh.getFaces().data(), 1.75f * particleRadius, samplePoints);
		STOP_TIMING_AVG;
	};
	if (SurfaceSamplingMode::PoissonDisk == samplingMode)
		samplePoissonDisk();
	else if (SurfaceSamplingMode::RegularTriangle == samplingMode)
		sampleRegularTriangle();
	else if (SurfaceSamplingMode::Regular2D == samplingMode)
		sampleRegular2D();
	else
	{
		std::cout << "Unknown surface sampling method: " << samplingMode;
		std::cout << "Falling back to:";
		sampleRegularTriangle();
	}

	std::cout << "Number of sample points: " << samplePoints.size() << "\n";

	PartioReaderWriter::writeParticles(outputFile, (unsigned int) samplePoints.size(), samplePoints.data(), NULL, particleRadius);

	Timing::printAverageTimes();
	Timing::printTimeSums();

	return 0;
}

void loadObj(const std::string &filename, TriangleMesh &mesh, const Vector3r &scale)
{
	std::vector<OBJLoader::Vec3f> x;
	std::vector<OBJLoader::Vec3f> normals;
	std::vector<MeshFaceIndices> faces;
	OBJLoader::Vec3f s = { (float)scale[0], (float)scale[1], (float)scale[2] };
	OBJLoader::loadObj(filename, &x, &faces, &normals, nullptr, s);

	mesh.release();
	const unsigned int nPoints = (unsigned int)x.size();
	const unsigned int nFaces = (unsigned int)faces.size();
	mesh.initMesh(nPoints, nFaces);
	for (unsigned int i = 0; i < nPoints; i++)
	{
		mesh.addVertex(Vector3r(x[i][0], x[i][1], x[i][2]));
	}
	for (unsigned int i = 0; i < nFaces; i++)
	{
		// Reduce the indices by one
		int posIndices[3];
		for (int j = 0; j < 3; j++)
		{
			posIndices[j] = faces[i].posIndices[j] - 1;
		}

		mesh.addFace(&posIndices[0]);
	}

	LOG_INFO << "Number of triangles: " << nFaces;
	LOG_INFO << "Number of vertices: " << nPoints;
}


#include "myslam/dataset.h"
#include "myslam/frame.h"

#include <boost/format.hpp>
#include <fstream>
#include <opencv2/opencv.hpp>
using namespace std;

namespace myslam{

    Dataset::Dataset(const std::string& dataset_path)
            : dataset_path_(dataset_path) {}

    bool Dataset::Init()
    {
        ifstream fin(dataset_path_ + "/calib.txt");
        if (!fin) {
            LOG(ERROR) << "cannot find " << dataset_path_ << "/calib.txt!";
            return false;
        }

        for (int i = 0; i < 4; ++i){   //一共有P0,P1,P2,P3四个相机，这里就是要把四个相机的参数全部读取到
            //前三个字符是P0：所以这里定义了一个长度为3的字符数组，读完这三个字符后就遇到了第一个空格，fin将会跳过这个空格，读取参数
            char camera_name[3];
            for (int k = 0; k < 3; ++k) {
                fin >> camera_name[k];
            }

            //将相机后面对应的12个参数读入到projection_data[12]中
            double projection_data[12];
            for (int k = 0; k < 12; ++k) {
                fin >> projection_data[k];
            }

            //将projection_data[12]的参数分为两部分提取出来
            Mat33 K;
            K << projection_data[0], projection_data[1], projection_data[2],
                    projection_data[4], projection_data[5], projection_data[6],
                    projection_data[8], projection_data[9], projection_data[10];
            Vec3 t;
            t << projection_data[3], projection_data[7], projection_data[11];

            t = K.inverse() * t;
            ///t = K.inverse() * t,参考https://blog.csdn.net/yangziluomu/article/details/78339575
            K = K * 0.5;//因为前面你把读到的图像全部resize成了原来的一半，所以需要在内参矩阵上乘以0.5,将投影获得的像素坐标也变为原来的一半
            Camera::Ptr new_camera(new Camera(K(0, 0), K(1, 1), K(0, 2), K(1, 2),
                                              t.norm(), SE3(SO3(), t)));
            cameras_.push_back(new_camera);
            LOG(INFO) << "Camera " << i << " extrinsics: " << t.transpose();

        }

        fin.close();

        current_image_index_ = 0;//虽然类内已经对这个成员变量赋值了，但是这里还要写一个置0语句，是因为每次初始化之后都得给index置0
        return true;
    }

    Frame::Ptr Dataset::NextFrame()
    {

        boost::format fmt("%s/image_%d/%06d.png");

        //boost::format的相关内容和用法参考：
        ///https://blog.csdn.net/weixin_33802505/article/details/90594738?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromBaidu-1.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromBaidu-1.control

        cv::Mat image_left, image_right;
        // read images
        image_left =
                cv::imread((fmt % dataset_path_ % 0 % current_image_index_).str(),
                           cv::IMREAD_GRAYSCALE);
        image_right =
                cv::imread((fmt % dataset_path_ % 1 % current_image_index_).str(),
                           cv::IMREAD_GRAYSCALE);

        if (image_left.data == nullptr || image_right.data == nullptr) {
            LOG(WARNING) << "cannot find images at index " << current_image_index_;
            return nullptr;
        }

        //利用resize()函数改变图像尺寸
        //resize(InputArray src, OutputArray dst, Size dsize,double fx=0, double fy=0, int interpolation=INTER_LINEAR )
        /*
         * InputArray src ：输入，原图像，即待改变大小的图像；
           OutputArray dst： 输出，改变后的图像。这个图像和原图像具有相同的内容，只是大小和原图像不一样而已；
           dsize：输出图像的大小。
           如果这个参数不为0，那么就代表将原图像缩放到这个Size(width，height)指定的大小；如果这个参数为0，那么原图像缩放之后的大小就要通过下面的公式来计算：
           dsize = Size(round(fxsrc.cols), round(fysrc.rows))

          其中，fx和fy就是下面要说的两个参数，是图像width方向和height方向的缩放比例。
          fx：width方向的缩放比例，如果它是0，那么它就会按照(double)dsize.width/src.cols来计算；
          fy：height方向的缩放比例，如果它是0，那么它就会按照(double)dsize.height/src.rows来计算；

          interpolation：这个是指定插值的方式，图像缩放之后，肯定像素要进行重新计算的，就靠这个参数来指定重新计算像素的方式，有以下几种：
          INTER_NEAREST - 最邻近插值
          INTER_LINEAR - 双线性插值，如果最后一个参数你不指定，默认使用这种方法
          INTER_AREA - resampling using pixel area relation. It may be a preferred method for image decimation, as it gives moire’-free results. But when the image is zoomed, it is similar to the INTER_NEAREST method.
          INTER_CUBIC - 4x4像素邻域内的双立方插值
          INTER_LANCZOS4 - 8x8像素邻域内的Lanczos插值
         */

        cv::Mat image_left_resized, image_right_resized;
        cv::resize(image_left, image_left_resized, cv::Size(), 0.5, 0.5,
                   cv::INTER_NEAREST);
        cv::resize(image_right, image_right_resized, cv::Size(), 0.5, 0.5,
                   cv::INTER_NEAREST);

        auto new_frame = Frame::CreateFrame();
        new_frame->left_img_ = image_left_resized;
        new_frame->right_img_ = image_right_resized;
        current_image_index_++;
        return new_frame;

    }

}


//---------------------------------------------------------------------------//
//!
//! \file   tstSubshellDopplerBroadenedPhotonEnergyDistribution.cpp
//! \author Alex Robinson
//! \brief  Subshell Doppler broadened photon energy distribution
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>
#include <limits>
#include <memory>

#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/polymorphic_iarchive.hpp>

// FRENSIE Includes
#include "MonteCarlo_StandardSubshellDopplerBroadenedPhotonEnergyDistribution.hpp"
#include "Data_SubshellType.hpp"
#include "MonteCarlo_StandardComptonProfile.hpp"
#include "Data_ElectronPhotonRelaxationDataContainer.hpp"
#include "Utility_TabularDistribution.hpp"
#include "Utility_RandomNumberGenerator.hpp"
#include "Utility_MeCMomentumUnit.hpp"
#include "Utility_InverseMeCMomentumUnit.hpp"
#include "Utility_UnitTestHarnessWithMain.hpp"

//---------------------------------------------------------------------------//
// Testing Variables
//---------------------------------------------------------------------------//

std::shared_ptr<const MonteCarlo::DopplerBroadenedPhotonEnergyDistribution>
  distribution;

//---------------------------------------------------------------------------//
// Tests
//---------------------------------------------------------------------------//
// Check that the distribution can be sampled
FRENSIE_UNIT_TEST( SubshellDopplerBroadenedPhotonEnergyDistribution,
		   sample )
{
  double incoming_energy = 20.0, scattering_angle_cosine = 0.0;
  double outgoing_energy;
  Data::SubshellType shell_of_interaction;

  // Set up the random number stream
  std::vector<double> fake_stream( 1 );
  fake_stream[0] = 0.5; // select pz = 0.0

  Utility::RandomNumberGenerator::setFakeStream( fake_stream );

  distribution->sample( incoming_energy,
			scattering_angle_cosine,
			outgoing_energy,
			shell_of_interaction );

  Utility::RandomNumberGenerator::unsetFakeStream();

  FRENSIE_CHECK_FLOATING_EQUALITY( outgoing_energy, 0.4982681851517501, 1e-12 );
  FRENSIE_CHECK_EQUAL( shell_of_interaction, Data::K_SUBSHELL );
}

//---------------------------------------------------------------------------//
// Custom Setup
//---------------------------------------------------------------------------//
FRENSIE_CUSTOM_UNIT_TEST_SETUP_BEGIN();

std::string test_native_file_name;

FRENSIE_CUSTOM_UNIT_TEST_COMMAND_LINE_OPTIONS()
{
  ADD_STANDARD_OPTION_AND_ASSIGN_VALUE( "test_native_file",
                                        test_native_file_name, "",
                                        "Test Native file name" );
}

FRENSIE_CUSTOM_UNIT_TEST_INIT()
{
  {
    Data::ElectronPhotonRelaxationDataContainer data_container( test_native_file_name );

    // Extract the Compton profile and occupation number for the first subshell
    const std::vector<double>& compton_profile_grid_s1 =
      data_container.getComptonProfileMomentumGrid( 1 );

    const std::vector<double>& compton_profile_s1 =
      data_container.getComptonProfile( 1 );

    const std::vector<double>& occupation_number_grid_s1 =
      data_container.getOccupationNumberMomentumGrid( 1 );

    const std::vector<double>& occupation_number_s1 =
      data_container.getOccupationNumber( 1 );

    // Create the Compton profile and occupation number distributions
    std::shared_ptr<Utility::UnitAwareTabularUnivariateDistribution<Utility::Units::MeCMomentum,Utility::Units::InverseMeCMomentum> > raw_compton_profile(
       new Utility::UnitAwareTabularDistribution<Utility::LinLin,Utility::Units::MeCMomentum,Utility::Units::InverseMeCMomentum>(
                                                       compton_profile_grid_s1,
						       compton_profile_s1 ) );

    std::shared_ptr<MonteCarlo::ComptonProfile> compton_profile_s1_dist(
           new MonteCarlo::StandardComptonProfile<Utility::Units::MeCMomentum>(
                                                       raw_compton_profile ) );

    // Create the Doppler broadened energy distribution
    distribution.reset( new MonteCarlo::StandardSubshellDopplerBroadenedPhotonEnergyDistribution<MonteCarlo::FullComptonProfilePolicy>(
		    Data::convertENDFDesignatorToSubshellEnum( 1 ),
		    data_container.getSubshellOccupancy( 1 ),
		    data_container.getSubshellBindingEnergy( 1 ),
		    compton_profile_s1_dist ) );
  }

  // Initialize the random number generator
  Utility::RandomNumberGenerator::createStreams();
}

FRENSIE_CUSTOM_UNIT_TEST_SETUP_END();

//---------------------------------------------------------------------------//
// end tstSubshellDopplerBroadenedPhotonEnergyDistribution.cpp
//---------------------------------------------------------------------------//


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include "GlobalOptimization.h"

#include <vector>
#include <tuple>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Core/Utility/Console.h>
#include <Core/Utility/Timer.h>
#include <Core/Registration/PoseGraph.h>
#include <Core/Registration/GlobalOptimizationMethod.h>
#include <Core/Registration/GlobalOptimizationConvergenceCriteria.h>

namespace three{

namespace {

/// Definition of linear operators used for computing Jacobian matrix.
/// If the relative transform of the two geometry is reasonably small,
/// they can be approximated as below linearized form
/// SE(3) \approx = |     1 -gamma   beta     a |
///                 | gamma      1 -alpha     b |
///                 | -beta  alpha      1     c |
///                 |     0      0      0     1 |
/// It is from sin(x) \approx x and cos(x) \approx 1 when x is almost zero.
/// See [Choi et al 2015] for more detail. Reference list in GlobalOptimization.h
const std::vector<Eigen::Matrix4d> jacobian_operator = {
    (Eigen::Matrix4d() << /* for alpha */
    0, 0, 0, 0, 0, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0).finished(),
    (Eigen::Matrix4d() << /* for beta */
    0, 0, 1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0).finished(),
    (Eigen::Matrix4d() << /* for gamma */
    0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished(),
    (Eigen::Matrix4d() << /* for a */
    0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished(),
    (Eigen::Matrix4d() << /* for b */
    0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0).finished(),
    (Eigen::Matrix4d() << /* for c */
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0).finished() };

/// This function is intended for linearized form of SE(3).
/// It is an approximate form. See [Choi et al 2015] for derivation.
/// Alternatively, explicit representation that uses quaternion can be used
/// here to replace this function. Refer to linearizeOplus() in
/// https://github.com/RainerKuemmerle/g2o/blob/master/g2o/types/slam3d/edge_se3.cpp
inline Eigen::Vector6d GetLinearized6DVector(const Eigen::Matrix4d input)
{
    Eigen::Vector6d output;
    output(0) = (-input(1, 2) + input(2, 1)) / 2.0;
    output(1) = (-input(2, 0) + input(0, 2)) / 2.0;
    output(2) = (-input(0, 1) + input(1, 0)) / 2.0;
    output.block<3, 1>(3, 0) = input.block<3, 1>(0, 3);
    return std::move(output);
}

inline Eigen::Vector6d GetMisalignmentVector(const Eigen::Matrix4d &X_inv,
        const Eigen::Matrix4d &Ts, const Eigen::Matrix4d &Tt_inv)
{
    Eigen::Matrix4d temp;
    temp.noalias() = X_inv * Tt_inv * Ts;
    return GetLinearized6DVector(temp);
}

inline std::tuple<Eigen::Matrix4d, Eigen::Matrix4d, Eigen::Matrix4d>
        GetRelativePoses(const PoseGraph &pose_graph, int edge_id)
{
    const PoseGraphEdge &te = pose_graph.edges_[edge_id];
    const PoseGraphNode &ts = pose_graph.nodes_[te.source_node_id_];
    const PoseGraphNode &tt = pose_graph.nodes_[te.target_node_id_];
    Eigen::Matrix4d X_inv = te.transformation_.inverse();
    Eigen::Matrix4d Ts = ts.pose_;
    Eigen::Matrix4d Tt_inv = tt.pose_.inverse();
    return std::make_tuple(std::move(X_inv), std::move(Ts), std::move(Tt_inv));
}

std::tuple<Eigen::Matrix6d, Eigen::Matrix6d> GetJacobian(
        const Eigen::Matrix4d &X_inv, const Eigen::Matrix4d &Ts,
        const Eigen::Matrix4d &Tt_inv)
{
    Eigen::Matrix6d Js = Eigen::Matrix6d::Zero();
    for (int i = 0; i < 6; i++) {
        Eigen::Matrix4d temp = X_inv * Tt_inv *
                jacobian_operator[i] * Ts;
        Js.block<6, 1>(0, i) = GetLinearized6DVector(temp);
    }
    Eigen::Matrix6d Jt = Eigen::Matrix6d::Zero();
    for (int i = 0; i < 6; i++) {
        Eigen::Matrix4d temp = X_inv * Tt_inv *
                -jacobian_operator[i] * Ts;
        Jt.block<6, 1>(0, i) = GetLinearized6DVector(temp);
    }
    return std::make_tuple(std::move(Js), std::move(Jt));
}

/// Function to update line_process value defined in [Choi et al 2015]
/// See Eq (2). temp2 value in this function is derived from dE/dl = 0
int UpdateConfidence(
        PoseGraph &pose_graph, const Eigen::VectorXd &zeta,
        const double line_process_weight,
        const GlobalOptimizationOption &option)
{
    int n_edges = (int)pose_graph.edges_.size();
    int valid_edges_num = 0;
    for (int iter_edge = 0; iter_edge < n_edges; iter_edge++) {
        PoseGraphEdge &t = pose_graph.edges_[iter_edge];
        Eigen::Vector6d e = zeta.block<6, 1>(iter_edge * 6, 0);
        double residual_square = e.transpose() * t.information_ * e;
        double temp = line_process_weight /
                (line_process_weight + residual_square);
        double temp2 = temp * temp;
        t.confidence_ = temp2;
        if (temp2 > option.edge_prune_threshold_)
            valid_edges_num++;
    }
    return valid_edges_num;
}

/// Function to compute residual defined in [Choi et al 2015] See Eq (9).
double ComputeResidual(const PoseGraph &pose_graph, const Eigen::VectorXd &zeta,
        const double line_process_weight,
        const GlobalOptimizationOption &option)
{
    int n_edges = (int)pose_graph.edges_.size();
    double residual = 0.0;
    for (int iter_edge = 0; iter_edge < n_edges; iter_edge++) {
        const PoseGraphEdge &te = pose_graph.edges_[iter_edge];
        double line_process_iter = te.confidence_;
        Eigen::Vector6d e = zeta.block<6, 1>(iter_edge * 6, 0);
        residual += line_process_iter * e.transpose() * te.information_ * e +
                line_process_weight * pow(sqrt(line_process_iter) - 1, 2.0);
    }
    return residual;
}

/// Function to compute residual defined in [Choi et al 2015] See Eq (6).
Eigen::VectorXd ComputeZeta(const PoseGraph &pose_graph)
{
    int n_edges = (int)pose_graph.edges_.size();
    Eigen::VectorXd output(n_edges * 6);
    for (int iter_edge = 0; iter_edge < n_edges; iter_edge++) {
        Eigen::Matrix4d X_inv, Ts, Tt_inv;
        std::tie(X_inv, Ts, Tt_inv) = GetRelativePoses(pose_graph, iter_edge);
        Eigen::Vector6d e = GetMisalignmentVector(X_inv, Ts, Tt_inv);
        output.block<6, 1>(iter_edge * 6, 0) = e;
    }
    return std::move(output);
}

/// The information matrix used here is consistent with [Choi et al 2015].
/// It is [p_x | I]^T[p_x | I]. \zeta is [\alpha \beta \gamma a b c]
/// Another definition of information matrix used for [Kümmerle et al 2011] is
/// [I | p_x] ^ T[I | p_x]  so \zeta is [a b c \alpha \beta \gamma].
///
/// To see how H can be derived see [Kümmerle et al 2011].
/// Eq (9) for definition of H and b for k-th constraint.
/// To see how the covariance matrix forms H, check g2o technical note:
/// https ://github.com/RainerKuemmerle/g2o/blob/master/doc/g2o.pdf
/// Eq (20) and Eq (21). (There is a typo in the equation though. B should be J)
///
/// This function focuses the case that every edge has two nodes (not hyper graph)
/// so we have two Jacobian matrices from one constraint.
std::tuple<Eigen::MatrixXd, Eigen::VectorXd> ComputeLinearSystem(
        const PoseGraph &pose_graph, const Eigen::VectorXd &zeta)
{
    int n_nodes = (int)pose_graph.nodes_.size();
    int n_edges = (int)pose_graph.edges_.size();
    Eigen::MatrixXd H(n_nodes * 6, n_nodes * 6);
    Eigen::VectorXd b(n_nodes * 6);
    H.setZero();
    b.setZero();

    for (int iter_edge = 0; iter_edge < n_edges; iter_edge++) {
        const PoseGraphEdge &t = pose_graph.edges_[iter_edge];
        Eigen::Vector6d e = zeta.block<6, 1>(iter_edge * 6, 0);

        Eigen::Matrix4d X_inv, Ts, Tt_inv;
        std::tie(X_inv, Ts, Tt_inv) = GetRelativePoses(pose_graph, iter_edge);

        Eigen::Matrix6d Js, Jt;
        std::tie(Js, Jt) = GetJacobian(X_inv, Ts, Tt_inv);
        Eigen::Matrix6d JsT_Info =
                Js.transpose() * t.information_;
        Eigen::Matrix6d JtT_Info =
                Jt.transpose() * t.information_;
        Eigen::Vector6d eT_Info = e.transpose() * t.information_;
        double line_process_iter = t.confidence_;

        int id_i = t.source_node_id_ * 6;
        int id_j = t.target_node_id_ * 6;
        H.block<6, 6>(id_i, id_i).noalias() +=
                line_process_iter * JsT_Info * Js;
        H.block<6, 6>(id_i, id_j).noalias() +=
                line_process_iter * JsT_Info * Jt;
        H.block<6, 6>(id_j, id_i).noalias() +=
                line_process_iter * JtT_Info * Js;
        H.block<6, 6>(id_j, id_j).noalias() +=
                line_process_iter * JtT_Info * Jt;
        b.block<6, 1>(id_i, 0).noalias() -=
                line_process_iter * eT_Info.transpose() * Js;
        b.block<6, 1>(id_j, 0).noalias() -=
                line_process_iter * eT_Info.transpose() * Jt;
    }
    return std::make_tuple(std::move(H), std::move(b));
}

Eigen::VectorXd UpdatePoseVector(const PoseGraph &pose_graph)
{
    int n_nodes = (int)pose_graph.nodes_.size();
    Eigen::VectorXd output(n_nodes * 6);
    for (int iter_node = 0; iter_node < n_nodes; iter_node++) {
        Eigen::Vector6d output_iter = TransformMatrix4dToVector6d(
                pose_graph.nodes_[iter_node].pose_);
        output.block<6, 1>(iter_node * 6, 0) = output_iter;
    }
    return std::move(output);
}

std::shared_ptr<PoseGraph> UpdatePoseGraph(const PoseGraph &pose_graph,
        const Eigen::VectorXd delta)
{
    std::shared_ptr<PoseGraph> pose_graph_updated =
        std::make_shared<PoseGraph>();
    *pose_graph_updated = pose_graph;
    int n_nodes = (int)pose_graph.nodes_.size();
    for (int iter_node = 0; iter_node < n_nodes; iter_node++) {
        Eigen::Vector6d delta_iter = delta.block<6, 1>(iter_node * 6, 0);
        pose_graph_updated->nodes_[iter_node].pose_ =
                TransformVector6dToMatrix4d(delta_iter) *
                pose_graph_updated->nodes_[iter_node].pose_;
    }
    return pose_graph_updated;
}

bool CheckRightTerm(const Eigen::VectorXd &right_term,
        const GlobalOptimizationConvergenceCriteria &criteria)
{
    if (right_term.maxCoeff() < criteria.min_right_term_) {
        PrintDebug("Maximum coefficient of right term < %e\n",
                criteria.min_right_term_);
        return true;
    }
    return false;
}

bool CheckRelativeIncrement(
        const Eigen::VectorXd &delta, const Eigen::VectorXd &x,
        const GlobalOptimizationConvergenceCriteria &criteria)
{
    if (delta.norm() < criteria.min_relative_increment_ *
            (x.norm() + criteria.min_relative_increment_)) {
        PrintDebug("Delta.norm() < %e * (x.norm() + %e)\n",
                criteria.min_relative_increment_,
                criteria.min_relative_increment_);
        return true;
    }
    return false;
}

bool CheckRelativeResidualIncrement(
        double current_residual, double new_residual,
        const GlobalOptimizationConvergenceCriteria &criteria)
{
    if (current_residual - new_residual <
        criteria.min_relative_residual_increment_ * current_residual) {
        PrintDebug("Current_residual - new_residual < %e * current_residual\n",
                criteria.min_relative_residual_increment_);
        return true;
    }
    return false;
}

bool CheckResidual(double residual,
        const GlobalOptimizationConvergenceCriteria &criteria)
{
    if (residual < criteria.min_residual_) {
        PrintDebug("Current_residual < %e\n", criteria.min_residual_);
        return true;
    }
    return false;
}

bool CheckMaxIteration(int iteration,
        const GlobalOptimizationConvergenceCriteria &criteria)
{
    if (iteration >= criteria.max_iteration_) {
        PrintDebug("Reached maximum number of iterations (%d)\n",
                criteria.max_iteration_);
        return true;
    }
    return false;
}

bool CheckMaxIterationLM(int iteration,
        const GlobalOptimizationConvergenceCriteria &criteria)
{
    if (iteration >= criteria.max_iteration_lm_) {
        PrintDebug("Reached maximum number of iterations (%d)\n",
                criteria.max_iteration_lm_);
        return true;
    }
    return false;
}

double ComputeLineProcessWeight(const PoseGraph &pose_graph,
        const GlobalOptimizationOption &option)
{
    int n_edges = (int)pose_graph.edges_.size();
    double average_number_of_correspondences = 0.0;
    for (int iter_edge = 0; iter_edge < n_edges; iter_edge++) {
        double number_of_correspondences =
                pose_graph.edges_[iter_edge].information_(5,5);
        average_number_of_correspondences += number_of_correspondences;
    }
    if (n_edges > 0) {
        // see Section 5 in [Choi et al 2015]
        average_number_of_correspondences /= (double)n_edges;
        double line_process_weight = option.preference_loop_closure_ *
                pow(option.max_correspondence_distance_, 2) *
                average_number_of_correspondences;
        return line_process_weight;
    }
    else {
        return 0.0;
    }
}

void CompensateReferencePoseGraphNode(PoseGraph &pose_graph_new,
        const PoseGraph &pose_graph_orig, int reference_node)
{
    PrintDebug("CompensateReferencePoseGraphNode : reference : %d\n",
            reference_node);
    int n_nodes = (int)pose_graph_new.nodes_.size();
    if (reference_node < 0 || reference_node >= n_nodes) {
        return;
    } else {
        Eigen::Matrix4d compensation =
                pose_graph_orig.nodes_[reference_node].pose_ *
                pose_graph_new.nodes_[reference_node].pose_.inverse();
        for (int i = 0; i < n_nodes; i++)
        {
            pose_graph_new.nodes_[i].pose_ = compensation *
                    pose_graph_new.nodes_[i].pose_;
        }
    }
}

}    // unnamed namespace

std::shared_ptr<PoseGraph> CreatePoseGraphWithoutInvalidEdges(
        const PoseGraph &pose_graph,
        const GlobalOptimizationOption &option)
{
    std::shared_ptr<PoseGraph> pose_graph_pruned =
            std::make_shared<PoseGraph>();

    int n_nodes = (int)pose_graph.nodes_.size();
    for (int iter_node = 0; iter_node < n_nodes; iter_node++) {
        const PoseGraphNode &t = pose_graph.nodes_[iter_node];
        pose_graph_pruned->nodes_.push_back(t);
    }
    int n_edges = (int)pose_graph.edges_.size();
    for (int iter_edge = 0; iter_edge < n_edges; iter_edge++) {
        const PoseGraphEdge &t = pose_graph.edges_[iter_edge];
        if (t.uncertain_) {
            if (t.confidence_ > option.edge_prune_threshold_) {
                pose_graph_pruned->edges_.push_back(t);
            }
        } else {
            pose_graph_pruned->edges_.push_back(t);
        }
    }
    return pose_graph_pruned;
}

void GlobalOptimizationGaussNewton::
        OptimizePoseGraph(PoseGraph &pose_graph,
        const GlobalOptimizationConvergenceCriteria &criteria,
        const GlobalOptimizationOption &option) const
{
    int n_nodes = (int)pose_graph.nodes_.size();
    int n_edges = (int)pose_graph.edges_.size();
    double line_process_weight = ComputeLineProcessWeight(pose_graph, option);

    PrintDebug("[GlobalOptimizationGaussNewton] Optimizing PoseGraph having %d nodes and %d edges. \n",
            n_nodes, n_edges);
    PrintDebug("Line process weight : %f\n", line_process_weight);

    Eigen::VectorXd zeta = ComputeZeta(pose_graph);
    double current_residual, new_residual;
    new_residual = ComputeResidual(pose_graph, zeta,
            line_process_weight, option);
    current_residual = new_residual;

    int valid_edges_num;
    valid_edges_num = UpdateConfidence(pose_graph, zeta,
            line_process_weight, option);

    Eigen::MatrixXd H;
    Eigen::VectorXd b;
    Eigen::VectorXd x = UpdatePoseVector(pose_graph);

    std::tie(H, b) = ComputeLinearSystem(pose_graph, zeta);

    PrintDebug("[Initial     ] residual : %e\n", current_residual);

    bool stop = false;
    if (stop || CheckRightTerm(b, criteria))
        return;

    Timer timer_overall;
    timer_overall.Start();
    int iter;
    for (iter = 0; !stop; iter++) {
        Timer timer_iter;
        timer_iter.Start();

        Eigen::VectorXd delta = H.ldlt().solve(b);

        stop = stop || CheckRelativeIncrement(delta, x, criteria);
        if (stop) {
            break;
        } else {
            std::shared_ptr<PoseGraph> pose_graph_new =
                UpdatePoseGraph(pose_graph, delta);

            Eigen::VectorXd zeta_new;
            zeta_new = ComputeZeta(*pose_graph_new);
            new_residual = ComputeResidual(pose_graph, zeta_new,
                    line_process_weight, option);
            stop = stop || CheckRelativeResidualIncrement(
                    current_residual, new_residual, criteria);
            if (stop)
                break;
            current_residual = new_residual;

            zeta = zeta_new;
            pose_graph = *pose_graph_new;
            x = UpdatePoseVector(pose_graph);
            valid_edges_num = UpdateConfidence(pose_graph, zeta,
                    line_process_weight, option);
            std::tie(H, b) = ComputeLinearSystem(pose_graph, zeta);

            stop = stop || CheckRightTerm(b, criteria);
            if (stop)
                break;
        }
        timer_iter.Stop();
        PrintDebug("[Iteration %02d] residual : %e, valid edges : %d, time : %.3f sec.\n",
                iter, current_residual, valid_edges_num,
                timer_iter.GetDuration() / 1000.0);
        stop = stop || CheckResidual(current_residual, criteria)
                || CheckMaxIteration(iter, criteria);
    }    // end for
    timer_overall.Stop();
    PrintDebug("[GlobalOptimizationGaussNewton] total time : %.3f sec.\n",
            timer_overall.GetDuration() / 1000.0);
}

void GlobalOptimizationLevenbergMarquardt::
        OptimizePoseGraph(PoseGraph &pose_graph,
        const GlobalOptimizationConvergenceCriteria &criteria,
        const GlobalOptimizationOption &option) const
{
    int n_nodes = (int)pose_graph.nodes_.size();
    int n_edges = (int)pose_graph.edges_.size();
    double line_process_weight = ComputeLineProcessWeight(pose_graph, option);

    PrintDebug("[GlobalOptimizationLM] Optimizing PoseGraph having %d nodes and %d edges. \n",
            n_nodes, n_edges);
    PrintDebug("Line process weight : %f\n", line_process_weight);

    Eigen::VectorXd zeta = ComputeZeta(pose_graph);
    double current_residual, new_residual;
    new_residual = ComputeResidual(pose_graph, zeta,
            line_process_weight, option);
    current_residual = new_residual;

    int valid_edges_num = UpdateConfidence(pose_graph, zeta,
            line_process_weight, option);

    Eigen::MatrixXd H_I = Eigen::MatrixXd::Identity(n_nodes * 6, n_nodes * 6);
    Eigen::MatrixXd H;
    Eigen::VectorXd b;
    Eigen::VectorXd x = UpdatePoseVector(pose_graph);

    std::tie(H, b) = ComputeLinearSystem(pose_graph, zeta);

    Eigen::VectorXd H_diag = H.diagonal();
    double tau = 1e-5;
    double current_lambda = tau * H_diag.maxCoeff();
    double ni = 2.0;
    double rho = 0.0;

    PrintDebug("[Initial     ] residual : %e, lambda : %e\n",
            current_residual, current_lambda);

    bool stop = false;
    stop = stop || CheckRightTerm(b, criteria);
    if (stop)
        return;

    Timer timer_overall;
    timer_overall.Start();
    for (int iter = 0; !stop; iter++) {
        Timer timer_iter;
        timer_iter.Start();
        int lm_count = 0;
        do {
            Eigen::MatrixXd H_LM = H + current_lambda * H_I;
            Eigen::VectorXd delta(H_LM.cols());

            //Using a sparse solver
            Eigen::SparseMatrix<double> H_LM_sparse = H_LM.sparseView();
            Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> chol;
            chol.compute(H_LM_sparse);

            if (chol.info() == Eigen::Success) {
                delta = chol.solve(b);
                if (chol.info() != Eigen::Success) {
                    PrintInfo("[GlobalOptimizationLM] sparse solver couldn't solve !! switching to dense solver");
                    delta = H_LM.ldlt().solve(b);
                    }
            } else {
                PrintInfo("[GlobalOptimizationLM] Cholesky Decomposition Failed !! switching to dense solver");
                delta = H_LM.ldlt().solve(b);
            }

            stop = stop || CheckRelativeIncrement(delta, x, criteria);
            if (!stop) {
                std::shared_ptr<PoseGraph> pose_graph_new =
                        UpdatePoseGraph(pose_graph, delta);

                Eigen::VectorXd zeta_new;
                zeta_new = ComputeZeta(*pose_graph_new);
                new_residual = ComputeResidual(pose_graph, zeta_new,
                        line_process_weight, option);
                rho = (current_residual - new_residual) /
                        (delta.dot(current_lambda * delta + b) + 1e-3);
                if (rho > 0) {
                    stop = stop || CheckRelativeResidualIncrement(
                            current_residual, new_residual, criteria);
                    if (stop)
                        break;
                    double alpha = 1. - pow((2 * rho - 1), 3);
                    alpha = (std::min)(alpha, criteria.upper_scale_factor_);
                    double scaleFactor = (std::max)
                            (criteria.lower_scale_factor_, alpha);
                    current_lambda *= scaleFactor;
                    ni = 2;
                    current_residual = new_residual;

                    zeta = zeta_new;
                    pose_graph = *pose_graph_new;
                    x = UpdatePoseVector(pose_graph);
                    valid_edges_num = UpdateConfidence(pose_graph, zeta,
                            line_process_weight, option);
                    std::tie(H, b) = ComputeLinearSystem(pose_graph, zeta);

                    stop = stop || CheckRightTerm(b, criteria);
                    if (stop)
                        break;
                } else {
                    current_lambda *= ni;
                    ni *= 2;
                }
            }
            lm_count++;
            stop = stop || CheckMaxIterationLM(lm_count, criteria);
        } while (!((rho > 0) || stop));
        timer_iter.Stop();
        if (!stop) {
            PrintDebug("[Iteration %02d] residual : %e, valid edges : %d, time : %.3f sec.\n",
                    iter, current_residual, valid_edges_num,
                    timer_iter.GetDuration() / 1000.0);
        }
        stop = stop || CheckResidual(current_residual, criteria)
                || CheckMaxIteration(iter, criteria);
    }    // end for
    timer_overall.Stop();
    PrintDebug("[GlobalOptimizationLM] total time : %.3f sec.\n",
            timer_overall.GetDuration() / 1000.0);
}

void GlobalOptimization(
        PoseGraph &pose_graph,
        const GlobalOptimizationMethod &method
        /* = GlobalOptimizationLevenbergMarquardt() */,
        const GlobalOptimizationConvergenceCriteria &criteria
        /* = GlobalOptimizationConvergenceCriteria() */,
        const GlobalOptimizationOption &option
        /* = GlobalOptimizationOption() */)
{
    std::shared_ptr<PoseGraph> pose_graph_pre =
            std::make_shared<PoseGraph>();
    *pose_graph_pre = pose_graph;
    method.OptimizePoseGraph(*pose_graph_pre, criteria, option);
    auto pose_graph_pruned = CreatePoseGraphWithoutInvalidEdges(
            *pose_graph_pre, option);
    method.OptimizePoseGraph(*pose_graph_pruned, criteria, option);
    CompensateReferencePoseGraphNode(*pose_graph_pruned,
            pose_graph, option.reference_node_);
    pose_graph = *pose_graph_pruned;
}

}    // namespace three