/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*          This file is part of the program and software framework          */
/*  CMAP-LAP --- Configurable Massively Parallel Solver for Lattice Problems */
/*                                                                           */
/*  Copyright Written by Nariaki Tateiwa <n-tateiwa@kyudai.jp>,              */
/*                       Yuji Shinano <shinano@zib.de>,                      */
/*            Copyright (C) 2021 by Zuse Institute Berlin,                   */
/*            licensed under LGPL version 3 or later.                        */
/*            Commercial licenses are available through <licenses@zib.de>    */
/*                                                                           */
/* This code is free software; you can redistribute it and/or                */
/* modify it under the terms of the GNU Lesser General Public License        */
/* as published by the Free Software Foundation; either version 3            */
/* 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 Lesser General Public License for more details.                       */
/*                                                                           */
/* You should have received a copy of the GNU Lesser General Public License  */
/* along with this program.  If not, see <http://www.gnu.org/licenses/>.     */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

/**@file    cmapLapParaLattice.cpp
 * @brief   Base class for Lattice.
 * @author  Nariaki Tateiwa, Yuji Shinano
 *
 *
 *
 */

/*---+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----8----+----9----+----0----+----1----+----2*/

#include "cmapLapParaLattice.h"
#include <NTL/tools.h>
#include <assert.h>
#include <float.h>
#include <utility>
#include <string>
#include <random>
#include <fstream>
#include "NTL/LLL.h"
#include "NTL/ZZ.h"
#include "NTL/matrix.h"
#include <eigen3/Eigen/Core>
#include "cmapLapParaDef.h"

namespace ParaCMapLAP
{

///
/// @brief constructor
/// @param[in] latticeBasis basis
///
template<typename BasisFloat, typename GSFloat>
void
Lattice<BasisFloat, GSFloat>::constructFromBasis(
      LatticeBasis<BasisFloat> &latticeBasis
      )
{
   n = latticeBasis.cols();
   m = latticeBasis.rows();
   basis = latticeBasis;
   mu.resize(m, m);
   B.resize(m);
   r__.resize(m);
   setGSO();
   GH = projectedGH();
   logVolume = logProjectedVolume();
   GSOType = 0;
   MLLLdelta = 0.99;
}

///
/// @brief constructor
/// @param[in] basisArray element of basis of RowMajor
/// @param[in] row   number of rows of basis
/// @param[in] col   number of columns of basis
///
template<typename BasisFloat, typename GSFloat>
void
Lattice<BasisFloat, GSFloat>::constructFromArray(
      int *basisArray,
      int row,
      int col
      )
{
   LatticeBasis<BasisFloat> inBasis;
   inBasis.resize(row, col);
   int index = 0;
   for ( int i = 0; i < row; ++i )
   {
      for ( int j = 0; j < col; ++j )
      {
         inBasis(i, j) = basisArray[index++];
      }
   }
   constructFromBasis(inBasis);
}

///
/// @brief read basis from file
/// @param[in] basisfile basis file path
///
template<typename BasisFloat, typename GSFloat>
void
Lattice<BasisFloat, GSFloat>::readFile(
      std::string basisfile
      )
{
   std::ifstream fin(basisfile);
   if( !fin.is_open() )
   {
      std::cout << "can not open input file: " << basisfile << std::endl;
      exit(0);
   }
   NTL::Mat<int> inputBasis;
   fin >> inputBasis;

   n = inputBasis.NumCols();
   m = inputBasis.NumRows();

   LatticeBasis<BasisFloat> eigenInputBasis;
   eigenInputBasis.resize(m, n);
   for( int row = 0; row < m; ++row )
   {
      for( int col = 0; col < n; ++col )
      {
         eigenInputBasis(row, col) = inputBasis[row][col];
      }
   }
   constructFromBasis(eigenInputBasis);
}

///
/// @brief Calculate GH(L') where L' is projected lattice {pi_k(b_k), ..., pi_k(b_l)}
/// @details GH(L') = (vol(L')/nu(d))**(1/d)
///          = exp( (log(vol(L')) - log(nu(d))) / d )
///          where d is dimension of projected lattice (l-k+1).
///          vol(L') = \prod_{i=k}^{l} norm(b*i)
/// @param[in] k index of basis
/// @param[in] l index of basis
///
template<typename BasisFloat, typename GSFloat>
double
Lattice<BasisFloat, GSFloat>::projectedGH(
      int k,
      int l
      )
{
   if( l < 0 ) l = m - 1;
   int d = l-k+1;
   long double __logProjectedVolume = logProjectedVolume(k, l);
   long double logUnitSphereVolume = d*std::log(M_PI)*0.5 - std::log(std::tgamma(d*0.5+1.0));
   return std::exp( (__logProjectedVolume-logUnitSphereVolume)/d );
}

///
/// @brief log volume of the projected lattice L' = L({pi_k(b_k), ..., pi_k(b_l)})
/// @return log( vol(L') ) = log( prod( norm(b*i) for k <= i <= l ) )
///                        = sum( log( norm(b*i) for k <= i <= l ) )
/// @param[in] k index of basis
/// @param[in] l index of basis
///
template<typename BasisFloat, typename GSFloat>
long double
Lattice<BasisFloat, GSFloat>::logProjectedVolume(
      int k,
      int l
      )
{
   if( l < 0 ) l = m - 1;
   long double logVolume__ = 0.0;
   for( int i = k; i <= l; ++i ){ logVolume__ += std::log( B(i) ); };
   return logVolume__ * 0.5;
}

///
/// @brief hash of basis
/// @retrn hash value
///
template<typename BasisFloat, typename GSFloat>
size_t
Lattice<BasisFloat, GSFloat>::hash(
   )
{
   size_t seed = 0;
   for( int i = 0; i < basis.size(); ++i )
   {
     BasisFloat elem = *(basis.data() + i);
     seed ^= std::hash<BasisFloat>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
   }
   return seed;
}

///
/// @brief compute and set orthogonalised basis
/// @return
/// @details set mu to GSO coefficient matrix, and
///          set B to squared norms of GSO vectors
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::setGSO(
      int k,
      int l
      )
{
   if( l == -1 ){ l = m-1; }
   if( GSOType == 0 )
      return setGSO_CFA(k, l);
   else
      return setMGSO(k, l);
}

///
/// @brief Calculate coefficients and square norm of
///        GSO b*k,...,b*l by Cholesky decomposition
/// @param[in] k index of basis
/// @param[in] l index of basis
/// @return
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::setGSO_CFA(
      int k,
      int l
      )
{
   int i, j, jj;
   mu.middleRows(k, l-k+1).setZero();
   for( i = k; i <= l; ++i )
      mu(i, i) = 1.0;

   B.segment(k, l-k+1).setZero();
   for( i = k; i <= l; ++i )
   {
      for( j = 0; j < i; ++j )
      {
         r__.coeffRef(j) = basis.row(i).dot(basis.row(j));
         for( jj = 0; jj < j; ++jj )
         {
            r__.coeffRef(j) -= mu.coeff(j,jj) * r__.coeff(jj);
         }
         mu.coeffRef(i,j) = r__.coeff(j)/B.coeff(j);
      }
      B.coeffRef(i) = basis.row(i).squaredNorm();
      for(j=1;j<=i;++j)
      {
         B.coeffRef(i) -= mu.coeff(i,j-1) * r__.coeff(j-1);
      }
   }
   return true;
}

///
/// @brief Calculate coefficients and square norm of
///        GSO b*k,...,b*l by Modified Gram-Schmidt Orthogonalization
/// @param[in] k index of basis
/// @param[in] l index of basis
/// @note r(i, j) = <bj, b*i> / norm(b*i) ( i < j )
///       r(i, i) = norm(b*i)
///       q.col(i) = b*_i / norm(b*i)
///                = (bi - sum(<bi, b*j>b*j for 0 <= j <= i-1) ) / norm(b*i)
///                = (bi - sum(r(j, i)b*j for 0 <= j <= i-1)   ) / norm(b*i)
/// @return false if it detects error for floating else true
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::setMGSO(
      int k,
      int l
      )
{
   int i, j;
   q__.setZero(n, m);
   p__.setZero(m, m);
   mu.middleRows(k, l-k+1).setZero();
   B.segment(k, l-k+1).setZero();

   for( i = k; i <= l; ++i )
   {
      // q__.col(i) = basis.row(i).cast<GSFloat>();
      for( j = 0; j < n; ++j )
         q__.coeffRef(j, i) = basis.coeff(i, j);
      for( j = 0; j < i; ++j )
      {
         p__.coeffRef(j, i) = q__.col(j).dot(q__.col(i));
         q__.col(i) -= p__.coeff(j, i) * q__.col(j);
      }
      p__.coeffRef(i, i) = q__.col(i).norm();
      q__.col(i) /= p__.coeff(i, i);
   }

   for( i = k; i <= l; ++i )
   {
      mu.coeffRef(i, i) = 1.0;
      B.coeffRef(i) = p__.coeff(i, i) * p__.coeff(i, i);
      if( B.coeffRef(i) < LDBL_EPSILON )
      {
         std::cout << "Length error in MGSO" << std::endl;
         return false;
      }
      for( j = 0; j < i; ++j )
      {
         mu.coeffRef(i, j) = p__.coeff(j, i) / p__.coeff(j, j);
      }
   }
   return true;
}

///
/// @brief shortest norm
///
template<typename BasisFloat, typename GSFloat>
double
Lattice<BasisFloat, GSFloat>::shortestNorm(
      )
{
   return std::sqrt(B(0));
}

///
/// @brief Transform b_i so that it is close to being
///        orthogonal to {b_0, ..., b_{m-1}} with keeping GSO vector.
/// @param[in] eta 0.501 -- 0.51
/// @return
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::sizeReduce(
      double eta
      )
{
   return sizeReduce(m-1, eta);
}

///
/// @brief Transform b_i so that it is close to being
///        orthogonal to {b_0, ..., b_{i-1}} with keeping GSO vector.
/// @param[in] i 0 -- m-1
/// @param[in] eta 0.501 -- 0.51
/// @return
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::sizeReduce(
      int i,
      double eta
      )
{
   int j, q, k;
   for( j = i-1; j >= 0; j--)
   {
      if( std::abs(mu.coeff(i, j)) > eta )
      {
         q = std::round(mu.coeff(i,j));
         mu.row(i) -= q * mu.row(j);
         basis.row(i) -= q * basis.row(j);
      }
   }
   return true;
}

///
/// @brief get approximation factor
/// @param[in] norm
/// @param[in] k
/// @return norm / GH
///
template<typename BasisFloat, typename GSFloat>
double
Lattice<BasisFloat, GSFloat>::approxFactor(
      double norm,
      int k
      )
{
   assert( k >= 0 );
   if( k == 0 )
   {
      if( norm < 0 ) norm = shortestNorm();
      return norm / GH;
   }
   else
   {
      if( norm < 0 ) norm = std::sqrt(B[k]);
      return norm / projectedGH(k, m-1);
   }
}

///
/// @brief get hermite factor
/// @return norm / ( vol(L)^(1/m) ) = norm / exp( (1/m) log( vol(L) )
///                                 = exp( log(norm) - (1/m)log( vol(L) ) )
///
template<typename BasisFloat, typename GSFloat>
double
Lattice<BasisFloat, GSFloat>::hermiteFactor(
      double norm
      )
{
   if( norm < 0 ) norm = shortestNorm();
   return norm / std::exp( logVolume / m );
}

///
/// @brief get root hermite factor
/// @return ( hermiteFactor ) ^ (1/m)
///
template<typename BasisFloat, typename GSFloat>
double
Lattice<BasisFloat, GSFloat>::rootHermiteFactor(
      double norm
      )
{
   return std::pow( hermiteFactor(norm), (1.0/m) );
}

///
/// @brief get logarithm of orthogonality defect
/// @return prod( norm(bi) for 0 <= i <= m-1 ) / vol(L)
///         = exp( sum( log(norm(bi)) for 0 <= i <= m-1) - log(vol(L)) )
///
template<typename BasisFloat, typename GSFloat>
double
Lattice<BasisFloat, GSFloat>::logOrthogonalityDefect(
      )
{
   double logProdNorm = 0;
   for( int i = 0; i < m; ++i )
      logProdNorm += std::log( basis.row(i).squaredNorm() );
   return logProdNorm * 0.5 - logVolume;
}

///
/// @brief get enumeration cost of projceted lattice {pi_i(bi), ..., pi_i(bj)}, where i = begin, j = end
/// @details enumCost = 1/2 * sum( H_l  for 0 <= l <= m-1 ), where
///          H_l := nu(R, l+1) / vol(\pi_{m-1-l}(L)),
///          vol(\pi_{s}(L)) := prod( norm(b*i) for s <= i <= m-1 ) and
///          nu(R, s) is the volume of s-dimensional ball with R radius.
/// @note log(nu(R, s)) = s log(R) + log(nu(1, s)) ( because nu(R, s) = R^{s} * nu(1, s) ) and
///       log(vol(\pi_{s}(L))) = sum( log(norm(b*i)) for s <= i <= m-1 ), then we have
///       log(H_l) = (l+1) log(R) + log(nu(1, (l+1))) - sum( log(norm(b*i)) for m-1-l <= i <= m-1 )
/// @param[in] R radius; if it is negative, then R is set to shortest norm;
/// @param[in] begin
/// @param[in] end
///
template<typename BasisFloat, typename GSFloat>
long double
Lattice<BasisFloat, GSFloat>::enumCost(
      double R,
      int begin,
      int end
      )
{
   if( R < 0 ) R = std::sqrt(B(0));
   if( end < 0 ) end = m - 1;
   long double N = 0.0;
   long double __logProjectedVolume = 0.0;
   long double logSphereVolume = 0.0;
   long double logR = std::log(R);
   for( int i = m-1; i > m-1-begin ; --i ){ __logProjectedVolume += std::log(B(i)) * 0.5; }
   for( int l = begin; l <= end; ++l )
   {
      __logProjectedVolume += std::log(B(m-l-1)) * 0.5;
      logSphereVolume = (l+1) * logR + ( (l+1)*std::log(M_PI)*0.5 - std::log(std::tgamma((l+1)*0.5+1.0)) );
      N += std::exp( logSphereVolume - __logProjectedVolume );
   }
   return N * 0.5;
}

///
/// @brief slope of GSA({(i, log2(squared_norm(b*i))} for 0 <= i <= h })
/// @param[in] h index of basis; if it is negative, h is set to m
///
template<typename BasisFloat, typename GSFloat>
double
Lattice<BasisFloat, GSFloat>::slopeGSA(
      int h
      )
{
   if( h < 0 ) h = m;
   VectorB<GSFloat> x(h);
   double slope = 0.0;
   for( int i = 0; i < h; ++i ) x(i) = log2(B(i)) * 0.5;
   for( int i = 0; i < h; ++i ) slope += (2.0*i-h+1)*x(i);
   slope = (6.0*slope) / (h*(h-1)*(h+1));
   return slope;
}

///
/// @brief generate new lattice using basis[:h]
/// @param[in] h index of basis
///
template<typename BasisFloat, typename GSFloat>
Lattice<BasisFloat, GSFloat>
Lattice<BasisFloat, GSFloat>::copy(
      int h
      )
{
   if( h < 0 ) h = m - 1;
   LatticeBasis<BasisFloat> basis__ = basis.block(0, 0, h, n);
   Lattice <BasisFloat, GSFloat> copiedLattice{basis__};
   return copiedLattice;
}

///
/// @brief convert basis to NTL::mat_ZZ
/// @output NTL::mat_ZZ matrix
///
template<typename BasisFloat, typename GSFloat>
NTL::mat_ZZ
Lattice<BasisFloat, GSFloat>::toNLTMat(
      )
{
   NTL::mat_ZZ Mbasis;
   Mbasis.SetDims(m, n);
   for( int i = 0; i < m; i++ )
   {
      for( int j = 0; j < n; j++ )
      {
         Mbasis[i][j] = basis.coeff(i, j);
      }
   }
   return Mbasis;
}

///
/// @brief load basis from NTL::mat_ZZ
///
template<typename BasisFloat, typename GSFloat>
void
Lattice<BasisFloat, GSFloat>::fromNLTMat(
      NTL::mat_ZZ &Mbasis
      )
{
   // remove zero vectors
   // row from 0 to zeroIndex are zerovectors
   int zeroIndex = -1;
   bool zero = true;
   int ntlRows = Mbasis.NumRows();
   int ntlCols = Mbasis.NumCols();
   for( int i = 0; i < ntlRows; i++ )
   {
      for( int j = 0; j < ntlCols; j++ )
         if( NTL::to_int(Mbasis[i][j]) != 0 ){ zero = false; }
      if( zero == true )
         zeroIndex++;
      else
         break; // row i is not a zero vector
   }

   // resize
   ntlRows -= zeroIndex + 1;

   // check
   if( (ntlRows != m) || (ntlCols != n) )
   {
      std::cout
         << "invarid dimension, expected (" << m << ", " << n << "),"
         << "but got (" << ntlRows << ", " << ntlCols << ")"
         << std::endl;
      exit(0);
   }

   for( int i = 0; i < m; i++ )
   {
      for( int j = 0; j < n; j++ )
      {
         basis.coeffRef(i, j)
            = NTL::to_int(Mbasis[i+zeroIndex+1][j]);
      }
   }

   setGSO();
}

///
/// @brief resize of lattice basis
/// @param[in] rows number of resized basis
///
template<typename BasisFloat, typename GSFloat>
void
Lattice<BasisFloat, GSFloat>::resize(
      int rows
      )
{
   assert( rows <= m );
   if( rows == m ){ return; }
   m = rows;
   // error occures when directory resize by basis = basis.topRows(m)
   LatticeBasis<BasisFloat> __basis = basis;
   MatrixMu<GSFloat>       __mu     = mu;
   VectorB<GSFloat>        __B      = B;
   basis = __basis.topRows(m);
   mu    = __mu.topLeftCorner(m, m);
   B     = __B.head(m);
   // GH = projectedGH();
   // logVolume = logProjectedVolume();
}

///
/// @brief randomize basis[begin:end] and execute reduction
/// @param[in] seed        seed of unimodular randomization
/// @param[in] begin       index of basis
/// @param[in] end         index of basis
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::randomize(
      int seed,
      int begin,
      int end
      )
{
   assert( end == -1 || begin <= end );
   if( begin == end ){ return true; }
   if( end < 0 ) end = m - 1;
   int u = end - begin + 1;

   ///
   /// generate unimodular matrix U
   ///
   LatticeBasis<BasisFloat> U(u, u);
   U.setIdentity();
   std::uniform_int_distribution<int> uniform{0, u-1};
   std::mt19937 mt(seed);  ///< randomize generartor

   // 1. permute rows
   int i, a, b, nIter = 4 * u, scale = 3;
   for( i = 0; i < nIter; ++i )
   {
      a = uniform(mt);
      b = a;
      while( b == a ){ b = uniform(mt); }
      U.row(a).swap(U.row(b));
   }

   // 2. triangular transformation matrix with coefficnets in -1, 0, 1
   std::uniform_int_distribution<int> flag{0, 1};
   for( a = 0; a < u - 2; ++a )
   {
      std::uniform_int_distribution<int> uniform_b{a+1, u-1};
      for( i = 0; i < scale; ++i )
      {
         b = uniform_b(mt);
         if( flag(mt) ){ U.row(a) += U.row(b); }
         else          { U.row(a) -= U.row(b); }
      }
   }

   // unimodular conversion
   basis.block(end-u+1,0,u,n) = U * basis.block(end-u+1,0,u,n);
   setGSO();
   return true;
}

///
/// @brief MLLL for part of basis matrix {b_j,...,b_k-1,v,b_k,...,b_l}
/// @param[in] v inserted vector
/// @param[in] j index of basis
/// @param[in] k index of basis
/// @param[in] l index of basis
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::insertMlll(
      LatticeVector<BasisFloat> &v,
      int j,
      int k,
      int l
      )
{
   int i, ii;
   NTL::mat_ZZ Mbasis;
   Mbasis.SetDims(l-j+2, n);
   for( i = j; i < k; i++ )
      for( ii = 0; ii < n; ++ii ){ Mbasis[i-j][ii] = basis(i,ii); }
   for( ii = 0; ii < n; ++ii )   { Mbasis[k-j][ii] = v(ii); }
   for( i = k; i <= l; i++ )
      for( ii = 0; ii < n; ++ii ){ Mbasis[i+1-j][ii] = basis(i,ii); }

   NTL::LLL_FP(Mbasis,MLLLdelta,0,0,0);

   // Mbasis[0] is zero vector
   for( i = j; i <= l; i++ )
      for( ii = 0; ii < n; ++ii ){ basis(i,ii) = NTL::to_int(Mbasis[i+1-j][ii]); }

   setGSO();
   return true;
}

///
/// @brief merge other latice
/// @param[in] other merged lattice
/// @remark runningTime and mergeTime are updated in this function
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::merge(
      Lattice<BasisFloat, GSFloat> &other
      )
{
   assert( n == other.n );
   VectorB<GSFloat> preSqnorm = B;
   LatticeVector<BasisFloat> v;
   for( int i = 0; i < std::min(m, other.m); i++ )
   {
      if( basis.row(i) != other.basis.row(i) )
      {
         v = other.basis.row(i);
         insertMlll(v, 0, 0, m-1);
      }
   }
   if( B != preSqnorm )
      return true;
   else
      return false;
}

///
/// @brief compair two lattice-basis
/// @details two basis are compared by lexicographic order of (B(0), ..., B(m-1))
/// @param[in] other lattice
/// @return bool of ( self <= other )
/// @note If the number of rows of basis is different, the comparison is done according to the smaller number of rows.
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::isMoreReducedThan(
      Lattice &other
      )
{
   int h = std::min(m, other.m);
   for( int i = 0; i < h; ++i )
   {
      if( B(i) < other.B(i) - 1.0 )
      {
         return true;
      }
      if( B(i) > other.B(i) )
         return false;
   }
   return false;
}

///
/// @brief compair two lattice-basis
/// @details two basis are compared by lexicographic order of (B(0), ..., B(m-1))
/// @param[in] other lattice
/// @param[out] index minimum index {i; B(i) < C(i)} where C is other lattice basis
/// @return bool of ( self <= other )
/// @note If the number of rows of basis is different, the comparison is done according to the smaller number of rows.
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::isMoreReducedThan(
      Lattice &other,
      int &index
      )
{
   int h = std::min(m, other.m);
   for( int i = 0; i < h; ++i )
   {
      if( B(i) < other.B(i) - 1.0 )
      {
         index = i;
         return true;
      }
      if( B(i) > other.B(i) )
         return false;
   }
   return false;
}

///
/// @brief output basis
/// @param[in] os
///
template<typename BasisFloat, typename GSFloat>
void
Lattice<BasisFloat, GSFloat>::outputBasis(
      std::ostream *os
      )
{
   *os << toNLTMat();
}

///
/// @brief write basis into textfile
/// @param[in] filename write file path
///
template<typename BasisFloat, typename GSFloat>
void
Lattice<BasisFloat, GSFloat>::writeBasis(
      std::string filename
      )
{
   std::ofstream basefile(filename);
   outputBasis(&basefile);
}

///
/// @brief stringfy lattice
///
template<typename BasisFloat, typename GSFloat>
std::string
Lattice<BasisFloat, GSFloat>::toSimpleString(
      )
{
   std::ostringstream s;
   s << std::endl
     << "Shortest vector      : "
     << basis.row(0)          << std::endl
     << "Shortest norm        : "
     << shortestNorm()        << std::endl
     << "Approximate factor   : "
     << approxFactor()        << std::endl
     << "Hermite factor       : "
     << hermiteFactor()       << std::endl
     << "Root Hermite factor  : "
     << rootHermiteFactor()   << std::endl
     << "Gaussian Heuristics  : "
     << GH                    << std::endl;
   return s.str();
}

///
/// instantiation
///
template class Lattice<int, double>;
template class Lattice<int, long double>;
template class Lattice<long int, double>;
template class Lattice<long int, long double>;

}  // namespace ParaCMapLAP


#ifndef DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP
#define DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP

#include <descartes_light/solvers/bgl/bgl_dijkstra_solver.h>

#include <descartes_light/descartes_macros.h>
DESCARTES_IGNORE_WARNINGS_PUSH
#include <boost/graph/dijkstra_shortest_paths.hpp>
DESCARTES_IGNORE_WARNINGS_POP

namespace descartes_light
{
template <typename FloatType>
SearchResult<FloatType> BGLDijkstraSVSESolver<FloatType>::search()
{
  // Convenience aliases
  auto& graph_ = BGLSolverBase<FloatType>::graph_;
  const auto& source_ = BGLSolverBase<FloatType>::source_;
  auto& predecessor_map_ = BGLSolverBase<FloatType>::predecessor_map_;
  const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;

  // Internal properties
  auto index_prop_map = boost::get(boost::vertex_index, graph_);
  auto weight_prop_map = boost::get(boost::edge_weight, graph_);
  auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph_);
  auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph_);

  predecessor_map_.clear();
  boost::associative_property_map<std::map<VertexDesc<FloatType>, VertexDesc<FloatType>>> predecessor_prop_map(
      predecessor_map_);

  // Perform the search
  boost::dijkstra_shortest_paths(graph_,
                                 source_,
                                 predecessor_prop_map,
                                 distance_prop_map,
                                 weight_prop_map,
                                 index_prop_map,
                                 std::less<>(),
                                 std::plus<>(),
                                 std::numeric_limits<FloatType>::max(),
                                 static_cast<FloatType>(0.0),
                                 boost::default_dijkstra_visitor(),
                                 color_prop_map);

  // Find lowest cost node in last rung
  auto target = std::min_element(ladder_rungs_.back().begin(),
                                 ladder_rungs_.back().end(),
                                 [&](const VertexDesc<FloatType>& a, const VertexDesc<FloatType>& b) {
                                   return graph_[a].distance < graph_[b].distance;
                                 });

  SearchResult<FloatType> result;

  // Reconstruct the path from the predecesor map; remove the artificial start state
  const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, *target);
  result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
  result.trajectory.erase(result.trajectory.begin());

  result.cost = graph_[*target].distance;

  return result;
}

/**
 * @brief Visitor for Dijkstra search that terminates the search once a vertex in the last rung has been encountered
 */
template <typename FloatType>
class DijkstraTerminateEarlyVisitor : public boost::default_dijkstra_visitor
{
public:
  DijkstraTerminateEarlyVisitor(long last_rung_idx) : last_rung_idx_(last_rung_idx) {}

  /**
   * @brief Hook for introspecting the search when a new vertex is opened by the search.
   * @details Throws the vertex descriptor that is the termination of the path once the first vertex in the last rung of
   * the graph is encountered
   */
  void examine_vertex(VertexDesc<FloatType> u, const BGLGraph<FloatType>& g)
  {
    if (g[u].rung_idx == last_rung_idx_)
      throw u;
  }

private:
  const long last_rung_idx_;
};

template <typename FloatType>
SearchResult<FloatType> BGLEfficientDijkstraSVSESolver<FloatType>::search()
{
  // Convenience aliases
  auto& graph_ = BGLSolverBase<FloatType>::graph_;
  const auto& source_ = BGLSolverBase<FloatType>::source_;
  auto& predecessor_map_ = BGLSolverBase<FloatType>::predecessor_map_;
  const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;

  // Internal properties
  auto index_prop_map = boost::get(boost::vertex_index, graph_);
  auto weight_prop_map = boost::get(boost::edge_weight, graph_);
  auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph_);
  auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph_);

  predecessor_map_.clear();
  boost::associative_property_map<std::map<VertexDesc<FloatType>, VertexDesc<FloatType>>> predecessor_prop_map(
      predecessor_map_);

  DijkstraTerminateEarlyVisitor<FloatType> visitor(static_cast<long>(ladder_rungs_.size() - 1));

  // Perform the search
  try
  {
    boost::dijkstra_shortest_paths(graph_,
                                   source_,
                                   predecessor_prop_map,
                                   distance_prop_map,
                                   weight_prop_map,
                                   index_prop_map,
                                   std::less<>(),
                                   std::plus<>(),
                                   std::numeric_limits<FloatType>::max(),
                                   static_cast<FloatType>(0.0),
                                   visitor,
                                   color_prop_map);
  }
  catch (const VertexDesc<FloatType>& target)
  {
    SearchResult<FloatType> result;

    // Reconstruct the path from the predecesor map; remove the artificial start state
    const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, target);
    result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
    result.trajectory.erase(result.trajectory.begin());

    result.cost = graph_[target].distance;

    return result;
  }

  // If the visitor never threw the vertex descriptor, there was an issue with the search
  throw std::runtime_error("Search failed to encounter vertex associated with the last waypoint in the trajectory");
}

}  // namespace descartes_light

#endif  // DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2021, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "slam-precomp.h"  // Precompiled headers
//

// ----------------------------------------------------------------------------------------
// For the theory behind this implementation, see the technical report in:
//
//            https://www.mrpt.org/6D-SLAM
// ----------------------------------------------------------------------------------------

#include <mrpt/math/ops_containers.h>
#include <mrpt/math/utils.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/obs/CActionRobotMovement3D.h>
#include <mrpt/opengl/CEllipsoid3D.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/poses/CPose3DQuatPDFGaussian.h>
#include <mrpt/poses/CPosePDF.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/slam/CRangeBearingKFSLAM.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/os.h>

#include <Eigen/Dense>

using namespace mrpt::slam;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace mrpt::system;
using namespace mrpt;
using namespace std;

/*---------------------------------------------------------------
							Constructor
  ---------------------------------------------------------------*/
CRangeBearingKFSLAM::CRangeBearingKFSLAM()
	: options(),
	  m_action(),
	  m_SF(),
	  m_IDs(),
	  mapPartitioner(),
	  m_SFs(),
	  m_lastPartitionSet()
{
	reset();
}

/*---------------------------------------------------------------
							reset
  ---------------------------------------------------------------*/
void CRangeBearingKFSLAM::reset()
{
	m_action.reset();
	m_SF.reset();
	m_IDs.clear();
	m_SFs.clear();
	mapPartitioner.clear();
	m_lastPartitionSet.clear();

	// -----------------------
	// INIT KF STATE
	m_xkk.assign(get_vehicle_size(), 0);  // State: 6D pose (x,y,z)=(0,0,0)
	m_xkk[3] = 1.0;	 // (qr,qx,qy,qz)=(1,0,0,0)

	// Initial cov:  nullptr diagonal -> perfect knowledge.
	m_pkk.setSize(get_vehicle_size(), get_vehicle_size());
	m_pkk.setZero();
	// -----------------------

	// Use SF-based matching (faster & easier for bearing-range observations
	// with ID).
	mapPartitioner.options.simil_method = smOBSERVATION_OVERLAP;
}

/*---------------------------------------------------------------
							Destructor
  ---------------------------------------------------------------*/
CRangeBearingKFSLAM::~CRangeBearingKFSLAM() = default;
/*---------------------------------------------------------------
							getCurrentRobotPose
  ---------------------------------------------------------------*/
void CRangeBearingKFSLAM::getCurrentRobotPose(
	CPose3DQuatPDFGaussian& out_robotPose) const
{
	MRPT_START

	ASSERT_(m_xkk.size() >= 7);

	// Copy xyz+quat: (explicitly unroll the loop)
	out_robotPose.mean.m_coords[0] = m_xkk[0];
	out_robotPose.mean.m_coords[1] = m_xkk[1];
	out_robotPose.mean.m_coords[2] = m_xkk[2];
	out_robotPose.mean.m_quat[0] = m_xkk[3];
	out_robotPose.mean.m_quat[1] = m_xkk[4];
	out_robotPose.mean.m_quat[2] = m_xkk[5];
	out_robotPose.mean.m_quat[3] = m_xkk[6];

	// and cov:
	out_robotPose.cov = m_pkk.blockCopy<7, 7>(0, 0);

	MRPT_END
}

/*---------------------------------------------------------------
							getCurrentRobotPoseMean
  ---------------------------------------------------------------*/
mrpt::poses::CPose3DQuat CRangeBearingKFSLAM::getCurrentRobotPoseMean() const
{
	CPose3DQuat q(mrpt::math::UNINITIALIZED_QUATERNION);
	ASSERTDEB_(m_xkk.size() >= 7);
	// Copy xyz+quat: (explicitly unroll the loop)
	q.m_coords[0] = m_xkk[0];
	q.m_coords[1] = m_xkk[1];
	q.m_coords[2] = m_xkk[2];
	q.m_quat[0] = m_xkk[3];
	q.m_quat[1] = m_xkk[4];
	q.m_quat[2] = m_xkk[5];
	q.m_quat[3] = m_xkk[6];

	return q;
}

/*---------------------------------------------------------------
							getCurrentState
  ---------------------------------------------------------------*/
void CRangeBearingKFSLAM::getCurrentState(
	CPose3DQuatPDFGaussian& out_robotPose,
	std::vector<TPoint3D>& out_landmarksPositions,
	std::map<unsigned int, CLandmark::TLandmarkID>& out_landmarkIDs,
	CVectorDouble& out_fullState, CMatrixDouble& out_fullCovariance) const
{
	MRPT_START

	ASSERT_(size_t(m_xkk.size()) >= get_vehicle_size());

	// Copy xyz+quat: (explicitly unroll the loop)
	out_robotPose.mean.m_coords[0] = m_xkk[0];
	out_robotPose.mean.m_coords[1] = m_xkk[1];
	out_robotPose.mean.m_coords[2] = m_xkk[2];
	out_robotPose.mean.m_quat[0] = m_xkk[3];
	out_robotPose.mean.m_quat[1] = m_xkk[4];
	out_robotPose.mean.m_quat[2] = m_xkk[5];
	out_robotPose.mean.m_quat[3] = m_xkk[6];

	// and cov:
	out_robotPose.cov = m_pkk.blockCopy<7, 7>(0, 0);

	// Landmarks:
	ASSERT_(((m_xkk.size() - get_vehicle_size()) % get_feature_size()) == 0);
	size_t i, nLMs = (m_xkk.size() - get_vehicle_size()) / get_feature_size();
	out_landmarksPositions.resize(nLMs);
	for (i = 0; i < nLMs; i++)
	{
		out_landmarksPositions[i].x =
			m_xkk[get_vehicle_size() + i * get_feature_size() + 0];
		out_landmarksPositions[i].y =
			m_xkk[get_vehicle_size() + i * get_feature_size() + 1];
		out_landmarksPositions[i].z =
			m_xkk[get_vehicle_size() + i * get_feature_size() + 2];
	}  // end for i

	// IDs:
	out_landmarkIDs = m_IDs.getInverseMap();  // m_IDs_inverse;

	// Full state:
	out_fullState.resize(m_xkk.size());
	std::copy(m_xkk.begin(), m_xkk.end(), out_fullState.begin());
	// Full cov:
	out_fullCovariance = m_pkk;

	MRPT_END
}

/*---------------------------------------------------------------
						processActionObservation
  ---------------------------------------------------------------*/
void CRangeBearingKFSLAM::processActionObservation(
	CActionCollection::Ptr& action, CSensoryFrame::Ptr& SF)
{
	MRPT_START

	m_action = action;
	m_SF = SF;

	// Sanity check:
	ASSERT_(
		m_IDs.size() ==
		(m_pkk.cols() - get_vehicle_size()) / get_feature_size());

	// ===================================================================================================================
	// Here's the meat!: Call the main method for the KF algorithm, which will
	// call all the callback methods as required:
	// ===================================================================================================================
	runOneKalmanIteration();

	// =============================================================
	//  ADD TO SFs SEQUENCE
	// =============================================================
	CPose3DQuatPDFGaussian q(UNINITIALIZED_QUATERNION);
	this->getCurrentRobotPose(q);
	CPose3DPDFGaussian::Ptr auxPosePDF = CPose3DPDFGaussian::Create(q);

	if (options.create_simplemap)
	{ m_SFs.insert(CPose3DPDF::Ptr(auxPosePDF), SF); }

	// =============================================================
	//  UPDATE THE PARTITION GRAPH EXPERIMENT
	// =============================================================
	if (options.doPartitioningExperiment)
	{
		if (options.partitioningMethod == 0)
		{
			// Use spectral-graph technique:
			mapPartitioner.addMapFrame(*SF, *auxPosePDF);

			vector<std::vector<uint32_t>> partitions;
			mapPartitioner.updatePartitions(partitions);
			m_lastPartitionSet = partitions;
		}
		else
		{
			// Fixed partitions every K observations:
			vector<std::vector<uint32_t>> partitions;
			std::vector<uint32_t> tmpCluster;

			ASSERT_(options.partitioningMethod > 1);
			size_t N = options.partitioningMethod;

			for (size_t i = 0; i < m_SFs.size(); i++)
			{
				tmpCluster.push_back(i);
				if ((i % N) == 0)
				{
					partitions.push_back(tmpCluster);
					tmpCluster.clear();
					tmpCluster.push_back(i);  // This observation "i" is shared
					// between both clusters
				}
			}
			m_lastPartitionSet = partitions;
		}

		printf(
			"Partitions: %u\n",
			static_cast<unsigned>(m_lastPartitionSet.size()));
	}

	MRPT_END
}

/** Return the last odometry, as a pose increment.
 */
CPose3DQuat CRangeBearingKFSLAM::getIncrementFromOdometry() const
{
	CActionRobotMovement2D::Ptr actMov2D =
		m_action->getBestMovementEstimation();
	CActionRobotMovement3D::Ptr actMov3D =
		m_action->getActionByClass<CActionRobotMovement3D>();
	if (actMov3D && !options.force_ignore_odometry)
	{ return CPose3DQuat(actMov3D->poseChange.mean); }
	else if (actMov2D && !options.force_ignore_odometry)
	{
		CPose2D estMovMean;
		actMov2D->poseChange->getMean(estMovMean);
		return CPose3DQuat(CPose3D(estMovMean));
	}
	else
	{
		return CPose3DQuat();
	}
}

/** Must return the action vector u.
 * \param out_u The action vector which will be passed to OnTransitionModel
 */
void CRangeBearingKFSLAM::OnGetAction(KFArray_ACT& u) const
{
	// Get odometry estimation:
	const CPose3DQuat theIncr = getIncrementFromOdometry();

	for (KFArray_ACT::Index i = 0;
		 i < static_cast<KFArray_ACT::Index>(u.size()); i++)
		u[i] = theIncr[i];
}

/** This virtual function musts implement the prediction model of the Kalman
 * filter.
 */
void CRangeBearingKFSLAM::OnTransitionModel(
	const KFArray_ACT& u, KFArray_VEH& xv, bool& out_skipPrediction) const
{
	MRPT_START

	// Do not update the vehicle pose & its covariance until we have some
	// landmarks in the map,
	// otherwise, we are imposing a lower bound to the best uncertainty from now
	// on:
	if (size_t(m_xkk.size()) == get_vehicle_size())
	{ out_skipPrediction = true; }

	// Current pose: copy xyz+quat
	CPose3DQuat robotPose = getCurrentRobotPoseMean();

	// Increment pose: copy xyz+quat (explicitly unroll the loop)
	CPose3DQuat odoIncrement(UNINITIALIZED_QUATERNION);
	odoIncrement.m_coords[0] = u[0];
	odoIncrement.m_coords[1] = u[1];
	odoIncrement.m_coords[2] = u[2];
	odoIncrement.m_quat[0] = u[3];
	odoIncrement.m_quat[1] = u[4];
	odoIncrement.m_quat[2] = u[5];
	odoIncrement.m_quat[3] = u[6];

	// Pose composition:
	robotPose += odoIncrement;

	// Output:
	for (size_t i = 0; i < xv.SizeAtCompileTime; i++)
		xv[i] = robotPose[i];

	MRPT_END
}

/** This virtual function musts calculate the Jacobian F of the prediction
 * model.
 */
void CRangeBearingKFSLAM::OnTransitionJacobian(KFMatrix_VxV& F) const
{
	MRPT_START

	// Current pose: copy xyz+quat
	CPose3DQuat robotPose = getCurrentRobotPoseMean();

	// Odometry:
	const CPose3DQuat theIncr = getIncrementFromOdometry();

	// Compute jacobians:
	CMatrixDouble77 df_du(UNINITIALIZED_MATRIX);

	CPose3DQuatPDF::jacobiansPoseComposition(
		robotPose,	// x
		theIncr,  // u
		F,	// df_dx,
		df_du);

	MRPT_END
}

/** This virtual function musts calculate de noise matrix of the prediction
 * model.
 */
void CRangeBearingKFSLAM::OnTransitionNoise(KFMatrix_VxV& Q) const
{
	MRPT_START

	// The uncertainty of the 2D odometry, projected from the current position:
	CActionRobotMovement2D::Ptr act2D = m_action->getBestMovementEstimation();
	CActionRobotMovement3D::Ptr act3D =
		m_action->getActionByClass<CActionRobotMovement3D>();

	if (act3D && act2D)
		THROW_EXCEPTION("Both 2D & 3D odometry are present!?!?");

	CPose3DQuatPDFGaussian odoIncr;

	if ((!act3D && !act2D) || options.force_ignore_odometry)
	{
		// Use constant Q:
		Q.setZero();
		ASSERT_(size_t(options.stds_Q_no_odo.size()) == size_t(Q.cols()));
		for (size_t i = 0; i < get_vehicle_size(); i++)
			Q(i, i) = square(options.stds_Q_no_odo[i]);
		return;
	}
	else
	{
		if (act2D)
		{
			// 2D odometry:
			CPosePDFGaussian odoIncr2D;
			odoIncr2D.copyFrom(*act2D->poseChange);
			odoIncr = CPose3DQuatPDFGaussian(CPose3DPDFGaussian(odoIncr2D));
		}
		else
		{
			// 3D odometry:
			odoIncr = CPose3DQuatPDFGaussian(act3D->poseChange);
		}
	}

	odoIncr.cov(2, 2) += square(options.std_odo_z_additional);

	// Current pose: copy xyz+quat
	CPose3DQuat robotPose = getCurrentRobotPoseMean();

	// Transform from odometry increment to "relative to the robot":
	odoIncr.changeCoordinatesReference(robotPose);

	Q = odoIncr.cov;

	MRPT_END
}

void CRangeBearingKFSLAM::OnObservationModel(
	const std::vector<size_t>& idx_landmarks_to_predict,
	vector_KFArray_OBS& out_predictions) const
{
	MRPT_START

	// Mean of the prior of the robot pose:
	CPose3DQuat robotPose = getCurrentRobotPoseMean();

	// Get the sensor pose relative to the robot:
	CObservationBearingRange::Ptr obs =
		m_SF->getObservationByClass<CObservationBearingRange>();
	ASSERTMSG_(
		obs,
		"*ERROR*: This method requires an observation of type "
		"CObservationBearingRange");
	const CPose3DQuat sensorPoseOnRobot =
		CPose3DQuat(obs->sensorLocationOnRobot);

	/* -------------------------------------------
	   Equations, obtained using matlab, of the relative 3D position of a
	  landmark (xi,yi,zi), relative
		  to a robot 6D pose (x0,y0,z0,y,p,r)
		Refer to technical report "6D EKF derivation...", 2008

		x0 y0 z0 y p r         % Robot's 6D pose
		x0s y0s z0s ys ps rs   % Sensor's 6D pose relative to robot
		xi yi zi % Absolute 3D landmark coordinates:

		Hx : dh_dxv   -> Jacobian of the observation model wrt the robot pose
		Hy : dh_dyi   -> Jacobian of the observation model wrt each landmark
	  mean position

		Sizes:
		 h:  Lx3
		 Hx: 3Lx6
		 Hy: 3Lx3

		  L=# of landmarks in the map (ALL OF THEM)
	  ------------------------------------------- */

	const size_t vehicle_size = get_vehicle_size();
	// const size_t  obs_size  = get_observation_size();
	const size_t feature_size = get_feature_size();

	const CPose3DQuat sensorPoseAbs = robotPose + sensorPoseOnRobot;

	const size_t N = idx_landmarks_to_predict.size();
	out_predictions.resize(N);
	for (size_t i = 0; i < N; i++)
	{
		const size_t row_in = feature_size * idx_landmarks_to_predict[i];

		// Landmark absolute 3D position in the map:
		const TPoint3D mapEst(
			m_xkk[vehicle_size + row_in + 0], m_xkk[vehicle_size + row_in + 1],
			m_xkk[vehicle_size + row_in + 2]);

		// Generate Range, yaw, pitch
		// ---------------------------------------------------
		sensorPoseAbs.sphericalCoordinates(
			mapEst,
			out_predictions[i][0],	// range
			out_predictions[i][1],	// yaw
			out_predictions[i][2]  // pitch
		);
	}

	MRPT_END
}

void CRangeBearingKFSLAM::OnObservationJacobians(
	size_t idx_landmark_to_predict, KFMatrix_OxV& Hx, KFMatrix_OxF& Hy) const
{
	MRPT_START

	// Mean of the prior of the robot pose:
	const CPose3DQuat robotPose = getCurrentRobotPoseMean();

	// Get the sensor pose relative to the robot:
	CObservationBearingRange::Ptr obs =
		m_SF->getObservationByClass<CObservationBearingRange>();
	ASSERTMSG_(
		obs,
		"*ERROR*: This method requires an observation of type "
		"CObservationBearingRange");
	const CPose3DQuat sensorPoseOnRobot =
		CPose3DQuat(obs->sensorLocationOnRobot);

	const size_t vehicle_size = get_vehicle_size();
	// const size_t  obs_size  = get_observation_size();
	const size_t feature_size = get_feature_size();

	// Compute the jacobians, needed below:
	// const CPose3DQuat  sensorPoseAbs= robotPose + sensorPoseOnRobot;
	CPose3DQuat sensorPoseAbs(UNINITIALIZED_QUATERNION);
	CMatrixFixed<kftype, 7, 7> H_senpose_vehpose(UNINITIALIZED_MATRIX);
	CMatrixFixed<kftype, 7, 7> H_senpose_senrelpose(
		UNINITIALIZED_MATRIX);	// Not actually used

	CPose3DQuatPDF::jacobiansPoseComposition(
		robotPose, sensorPoseOnRobot, H_senpose_vehpose, H_senpose_senrelpose,
		&sensorPoseAbs);

	const size_t row_in = feature_size * idx_landmark_to_predict;

	// Landmark absolute 3D position in the map:
	const TPoint3D mapEst(
		m_xkk[vehicle_size + row_in + 0], m_xkk[vehicle_size + row_in + 1],
		m_xkk[vehicle_size + row_in + 2]);

	// The Jacobian wrt the sensor pose must be transformed later on:
	KFMatrix_OxV Hx_sensor;
	double obsData[3];
	sensorPoseAbs.sphericalCoordinates(
		mapEst,
		obsData[0],	 // range
		obsData[1],	 // yaw
		obsData[2],	 // pitch
		&Hy, &Hx_sensor);

	// Chain rule: Hx = d sensorpose / d vehiclepose   * Hx_sensor
	Hx = Hx_sensor * H_senpose_vehpose;

	MRPT_END
}

/** This is called between the KF prediction step and the update step, and the
 * application must return the observations and, when applicable, the data
 * association between these observations and the current map.
 *
 * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N
 * being the number of "observations": how many observed landmarks for a map, or
 * just one if not applicable.
 * \param out_data_association An empty vector or, where applicable, a vector
 * where the i'th element corresponds to the position of the observation in the
 * i'th row of out_z within the system state vector (in the range
 * [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and
 * we want to insert it at the end of this KF iteration.
 * \param in_S The full covariance matrix of the observation predictions (i.e.
 * the "innovation covariance matrix"). This is a M·O x M·O matrix with M=length
 * of "in_lm_indices_in_S".
 * \param in_lm_indices_in_S The indices of the map landmarks (range
 * [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.
 *
 *  This method will be called just once for each complete KF iteration.
 * \note It is assumed that the observations are independent, i.e. there are NO
 * cross-covariances between them.
 */
void CRangeBearingKFSLAM::OnGetObservationsAndDataAssociation(
	vector_KFArray_OBS& Z, std::vector<int>& data_association,
	const vector_KFArray_OBS& all_predictions, const KFMatrix& S,
	const std::vector<size_t>& lm_indices_in_S, const KFMatrix_OxO& R)
{
	MRPT_START

	// static const size_t vehicle_size = get_vehicle_size();
	constexpr size_t obs_size = get_observation_size();

	// Z: Observations
	CObservationBearingRange::TMeasurementList::const_iterator itObs;

	CObservationBearingRange::Ptr obs =
		m_SF->getObservationByClass<CObservationBearingRange>();
	ASSERTMSG_(
		obs,
		"*ERROR*: This method requires an observation of type "
		"CObservationBearingRange");

	const size_t N = obs->sensedData.size();
	Z.resize(N);

	size_t row;
	for (row = 0, itObs = obs->sensedData.begin();
		 itObs != obs->sensedData.end(); ++itObs, ++row)
	{
		// Fill one row in Z:
		Z[row][0] = itObs->range;
		Z[row][1] = itObs->yaw;
		Z[row][2] = itObs->pitch;
	}

	// Data association:
	// ---------------------
	data_association.assign(N, -1);	 // Initially, all new landmarks

	// For each observed LM:
	std::vector<size_t> obs_idxs_needing_data_assoc;
	obs_idxs_needing_data_assoc.reserve(N);

	{
		std::vector<int>::iterator itDA;
		for (row = 0, itObs = obs->sensedData.begin(),
			itDA = data_association.begin();
			 itObs != obs->sensedData.end(); ++itObs, ++itDA, ++row)
		{
			// Fill data asociation: Using IDs!
			if (itObs->landmarkID < 0)
				obs_idxs_needing_data_assoc.push_back(row);
			else
			{
				mrpt::containers::bimap<
					CLandmark::TLandmarkID, unsigned int>::iterator itID;
				if ((itID = m_IDs.find_key(itObs->landmarkID)) != m_IDs.end())
					*itDA = itID->second;  // This row in Z corresponds to the
				// i'th map element in the state
				// vector:
			}
		}
	}

	// ---- Perform data association ----
	//  Only for observation indices in "obs_idxs_needing_data_assoc"
	if (obs_idxs_needing_data_assoc.empty())
	{
		// We don't need to do DA:
		m_last_data_association = TDataAssocInfo();

		// Save them for the case the external user wants to access them:
		for (size_t idxObs = 0; idxObs < data_association.size(); idxObs++)
		{
			int da = data_association[idxObs];
			if (da >= 0)
				m_last_data_association.results.associations[idxObs] =
					data_association[idxObs];
		}
	}
	else
	{
		// Build a Z matrix with the observations that need dat.assoc:
		const size_t nObsDA = obs_idxs_needing_data_assoc.size();

		CMatrixDynamic<kftype> Z_obs_means(nObsDA, obs_size);
		for (size_t i = 0; i < nObsDA; i++)
		{
			const size_t idx = obs_idxs_needing_data_assoc[i];
			for (unsigned k = 0; k < obs_size; k++)
				Z_obs_means(i, k) = Z[idx][k];
		}

		// Vehicle uncertainty
		// KFMatrix_VxV Pxx = m_pkk.extractMatrix<7, 7>(0, 0);

		// Build predictions:
		// ---------------------------
		const size_t nPredictions = lm_indices_in_S.size();
		m_last_data_association.clear();

		// S is the covariance of the predictions:
		m_last_data_association.Y_pred_covs = S;

		// The means:
		m_last_data_association.Y_pred_means.setSize(nPredictions, obs_size);
		for (size_t q = 0; q < nPredictions; q++)
		{
			const size_t i = lm_indices_in_S[q];
			for (size_t w = 0; w < obs_size; w++)
				m_last_data_association.Y_pred_means(q, w) =
					all_predictions[i][w];
			m_last_data_association.predictions_IDs.push_back(
				i);	 // for the conversion of indices...
		}

		// Do Dat. Assoc :
		// ---------------------------
		if (nPredictions)
		{
			// CMatrixDouble Z_obs_cov = CMatrixDouble(R);

			mrpt::slam::data_association_full_covariance(
				Z_obs_means, m_last_data_association.Y_pred_means,
				m_last_data_association.Y_pred_covs,
				m_last_data_association.results, options.data_assoc_method,
				options.data_assoc_metric, options.data_assoc_IC_chi2_thres,
				true,  // Use KD-tree
				m_last_data_association.predictions_IDs,
				options.data_assoc_IC_metric,
				options.data_assoc_IC_ml_threshold);

			// Return pairings to the main KF algorithm:
			for (auto it = m_last_data_association.results.associations.begin();
				 it != m_last_data_association.results.associations.end(); ++it)
				data_association[it->first] = it->second;
		}
	}
	// ---- End of data association ----

	MRPT_END
}

/** This virtual function musts normalize the state vector and covariance matrix
 * (only if its necessary).
 */
void CRangeBearingKFSLAM::OnNormalizeStateVector()
{
	MRPT_START

	// m_xkk[3:6] must be a normalized quaternion:
	const double T = std::sqrt(
		square(m_xkk[3]) + square(m_xkk[4]) + square(m_xkk[5]) +
		square(m_xkk[6]));
	ASSERTMSG_(T > 0, "Vehicle pose quaternion norm is not >0!!");

	const double T_ = (m_xkk[3] < 0 ? -1.0 : 1.0) / T;	// qr>=0
	m_xkk[3] *= T_;
	m_xkk[4] *= T_;
	m_xkk[5] *= T_;
	m_xkk[6] *= T_;

	MRPT_END
}

/*---------------------------------------------------------------
						loadOptions
  ---------------------------------------------------------------*/
void CRangeBearingKFSLAM::loadOptions(const mrpt::config::CConfigFileBase& ini)
{
	// Main
	options.loadFromConfigFile(ini, "RangeBearingKFSLAM");
	KF_options.loadFromConfigFile(ini, "RangeBearingKFSLAM_KalmanFilter");
	// partition algorithm:
	mapPartitioner.options.loadFromConfigFile(ini, "RangeBearingKFSLAM");
}

/*---------------------------------------------------------------
						loadOptions
  ---------------------------------------------------------------*/
void CRangeBearingKFSLAM::TOptions::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& source, const std::string& section)
{
	source.read_vector(section, "stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo);
	ASSERT_(stds_Q_no_odo.size() == 7);
	std_sensor_range =
		source.read_float(section, "std_sensor_range", std_sensor_range);
	std_sensor_yaw = DEG2RAD(source.read_float(
		section, "std_sensor_yaw_deg", RAD2DEG(std_sensor_yaw)));
	std_sensor_pitch = DEG2RAD(source.read_float(
		section, "std_sensor_pitch_deg", RAD2DEG(std_sensor_pitch)));

	std_odo_z_additional = source.read_float(
		section, "std_odo_z_additional", std_odo_z_additional);

	MRPT_LOAD_CONFIG_VAR(doPartitioningExperiment, bool, source, section);
	MRPT_LOAD_CONFIG_VAR(partitioningMethod, int, source, section);

	MRPT_LOAD_CONFIG_VAR(create_simplemap, bool, source, section);

	MRPT_LOAD_CONFIG_VAR(force_ignore_odometry, bool, source, section);

	data_assoc_method = source.read_enum<TDataAssociationMethod>(
		section, "data_assoc_method", data_assoc_method);
	data_assoc_metric = source.read_enum<TDataAssociationMetric>(
		section, "data_assoc_metric", data_assoc_metric);
	data_assoc_IC_metric = source.read_enum<TDataAssociationMetric>(
		section, "data_assoc_IC_metric", data_assoc_IC_metric);

	MRPT_LOAD_CONFIG_VAR(data_assoc_IC_chi2_thres, double, source, section);
	MRPT_LOAD_CONFIG_VAR(data_assoc_IC_ml_threshold, double, source, section);

	MRPT_LOAD_CONFIG_VAR(quantiles_3D_representation, float, source, section);
}

/*---------------------------------------------------------------
						Constructor
  ---------------------------------------------------------------*/
CRangeBearingKFSLAM::TOptions::TOptions()
	: stds_Q_no_odo(get_vehicle_size(), 0),

	  std_sensor_yaw(DEG2RAD(0.2f)),
	  std_sensor_pitch(DEG2RAD(0.2f))

{
	stds_Q_no_odo[0] = stds_Q_no_odo[1] = stds_Q_no_odo[2] = 0.10f;
	stds_Q_no_odo[3] = stds_Q_no_odo[4] = stds_Q_no_odo[5] = stds_Q_no_odo[6] =
		0.05f;
}

/*---------------------------------------------------------------
						dumpToTextStream
  ---------------------------------------------------------------*/
void CRangeBearingKFSLAM::TOptions::dumpToTextStream(std::ostream& out) const
{
	using namespace mrpt::typemeta;

	out << "\n----------- [CRangeBearingKFSLAM::TOptions] ------------ \n\n";

	out << mrpt::format(
		"doPartitioningExperiment                = %c\n",
		doPartitioningExperiment ? 'Y' : 'N');
	out << mrpt::format(
		"partitioningMethod                      = %i\n", partitioningMethod);
	out << mrpt::format(
		"data_assoc_method                       = %s\n",
		TEnumType<TDataAssociationMethod>::value2name(data_assoc_method)
			.c_str());
	out << mrpt::format(
		"data_assoc_metric                       = %s\n",
		TEnumType<TDataAssociationMetric>::value2name(data_assoc_metric)
			.c_str());
	out << mrpt::format(
		"data_assoc_IC_chi2_thres                = %.06f\n",
		data_assoc_IC_chi2_thres);
	out << mrpt::format(
		"data_assoc_IC_metric                    = %s\n",
		TEnumType<TDataAssociationMetric>::value2name(data_assoc_IC_metric)
			.c_str());
	out << mrpt::format(
		"data_assoc_IC_ml_threshold              = %.06f\n",
		data_assoc_IC_ml_threshold);

	out << "\n";
}

void CRangeBearingKFSLAM::OnInverseObservationModel(
	const KFArray_OBS& in_z, KFArray_FEAT& yn, KFMatrix_FxV& dyn_dxv,
	KFMatrix_FxO& dyn_dhn) const
{
	MRPT_START

	CObservationBearingRange::Ptr obs =
		m_SF->getObservationByClass<CObservationBearingRange>();
	ASSERTMSG_(
		obs,
		"*ERROR*: This method requires an observation of type "
		"CObservationBearingRange");
	const CPose3DQuat sensorPoseOnRobot =
		CPose3DQuat(obs->sensorLocationOnRobot);

	// Mean of the prior of the robot pose:
	const CPose3DQuat robotPose = getCurrentRobotPoseMean();

	// const CPose3DQuat  sensorPoseAbs= robotPose + sensorPoseOnRobot;
	CPose3DQuat sensorPoseAbs(UNINITIALIZED_QUATERNION);
	CMatrixFixed<kftype, 7, 7> dsensorabs_dvehpose(UNINITIALIZED_MATRIX);
	CMatrixFixed<kftype, 7, 7> dsensorabs_dsenrelpose(
		UNINITIALIZED_MATRIX);	// Not actually used

	CPose3DQuatPDF::jacobiansPoseComposition(
		robotPose, sensorPoseOnRobot, dsensorabs_dvehpose,
		dsensorabs_dsenrelpose, &sensorPoseAbs);

	kftype hn_r = in_z[0];
	kftype hn_y = in_z[1];
	kftype hn_p = in_z[2];

	kftype chn_y = cos(hn_y);
	kftype shn_y = sin(hn_y);
	kftype chn_p = cos(hn_p);
	kftype shn_p = sin(hn_p);

	/* -------------------------------------------
		syms H h_range h_yaw h_pitch real;
		H=[ h_range ; h_yaw ; h_pitch ];

		syms X xi_ yi_ zi_ real;
		xi_ = h_range * cos(h_yaw) * cos(h_pitch);
		yi_ = h_range * sin(h_yaw) * cos(h_pitch);
		zi_ = -h_range * sin(h_pitch);

		X=[xi_ yi_ zi_];
		jacob_inv_mod=jacobian(X,H)
	  ------------------------------------------- */

	// The new point, relative to the sensor:
	const TPoint3D yn_rel_sensor(
		hn_r * chn_y * chn_p, hn_r * shn_y * chn_p, -hn_r * shn_p);

	// The Jacobian of the 3D point in the coordinate system of the sensor:
	/*
		[ cos(h_pitch)*cos(h_yaw), -h_range*cos(h_pitch)*sin(h_yaw),
	   -h_range*cos(h_yaw)*sin(h_pitch)]
		[ cos(h_pitch)*sin(h_yaw),  h_range*cos(h_pitch)*cos(h_yaw),
	   -h_range*sin(h_pitch)*sin(h_yaw)]
		[           -sin(h_pitch),                                0,
	   -h_range*cos(h_pitch)]
	*/
	const kftype values_dynlocal_dhn[] = {
		chn_p * chn_y,
		-hn_r * chn_p * shn_y,
		-hn_r * chn_y * shn_p,
		chn_p * shn_y,
		hn_r * chn_p * chn_y,
		-hn_r * shn_p * shn_y,
		-shn_p,
		0,
		-hn_r * chn_p};
	const KFMatrix_FxO dynlocal_dhn(values_dynlocal_dhn);

	KFMatrix_FxF jacob_dyn_dynrelsensor(UNINITIALIZED_MATRIX);
	KFMatrix_FxV jacob_dyn_dsensorabs(UNINITIALIZED_MATRIX);

	sensorPoseAbs.composePoint(
		yn_rel_sensor.x, yn_rel_sensor.y,
		yn_rel_sensor.z,  // yn rel. to the sensor
		yn[0], yn[1], yn[2],  // yn in global coords
		&jacob_dyn_dynrelsensor, &jacob_dyn_dsensorabs);

	dyn_dhn = jacob_dyn_dynrelsensor * dynlocal_dhn;

	// dyn_dxv =
	dyn_dxv = jacob_dyn_dsensorabs * dsensorabs_dvehpose;

	MRPT_END
}

/** If applicable to the given problem, do here any special handling of adding a
 * new landmark to the map.
 * \param in_obsIndex The index of the observation whose inverse sensor is to
 * be computed. It corresponds to the row in in_z where the observation can be
 * found.
 * \param in_idxNewFeat The index that this new feature will have in the state
 * vector (0:just after the vehicle state, 1: after that,...). Save this number
 * so data association can be done according to these indices.
 * \sa OnInverseObservationModel
 */
void CRangeBearingKFSLAM::OnNewLandmarkAddedToMap(
	const size_t in_obsIdx, const size_t in_idxNewFeat)
{
	MRPT_START

	CObservationBearingRange::Ptr obs =
		m_SF->getObservationByClass<CObservationBearingRange>();
	ASSERTMSG_(
		obs,
		"*ERROR*: This method requires an observation of type "
		"CObservationBearingRange");

	// ----------------------------------------------
	// introduce in the lists of ID<->index in map:
	// ----------------------------------------------
	ASSERT_(in_obsIdx < obs->sensedData.size());
	if (obs->sensedData[in_obsIdx].landmarkID >= 0)
	{
		// The sensor provides us a LM ID... use it:
		m_IDs.insert(obs->sensedData[in_obsIdx].landmarkID, in_idxNewFeat);
	}
	else
	{
		// Features do not have IDs... use indices:
		m_IDs.insert(in_idxNewFeat, in_idxNewFeat);
	}

	MRPT_END
}

/*---------------------------------------------------------------
						getAs3DObject
  ---------------------------------------------------------------*/
void CRangeBearingKFSLAM::getAs3DObject(
	mrpt::opengl::CSetOfObjects::Ptr& outObj) const
{
	outObj->clear();

	// ------------------------------------------------
	//  Add the XYZ corner for the current area:
	// ------------------------------------------------
	outObj->insert(opengl::stock_objects::CornerXYZ());

	// 3D ellipsoid for robot pose:
	CPointPDFGaussian pointGauss;
	pointGauss.mean.x(m_xkk[0]);
	pointGauss.mean.y(m_xkk[1]);
	pointGauss.mean.z(m_xkk[2]);
	pointGauss.cov = m_pkk.blockCopy<3, 3>(0, 0);

	{
		auto ellip = opengl::CEllipsoid3D::Create();

		ellip->setPose(pointGauss.mean);
		ellip->setCovMatrix(pointGauss.cov);
		ellip->enableDrawSolid3D(false);
		ellip->setQuantiles(options.quantiles_3D_representation);
		ellip->set3DsegmentsCount(10);
		ellip->setColor(1, 0, 0);

		outObj->insert(ellip);
	}

	// 3D ellipsoids for landmarks:
	const size_t nLMs = this->getNumberOfLandmarksInTheMap();
	for (size_t i = 0; i < nLMs; i++)
	{
		pointGauss.mean.x(
			m_xkk[get_vehicle_size() + get_feature_size() * i + 0]);
		pointGauss.mean.y(
			m_xkk[get_vehicle_size() + get_feature_size() * i + 1]);
		pointGauss.mean.z(
			m_xkk[get_vehicle_size() + get_feature_size() * i + 2]);

		pointGauss.cov =
			m_pkk.blockCopy<get_feature_size(), get_feature_size()>(
				get_vehicle_size() + get_feature_size() * i,
				get_vehicle_size() + get_feature_size() * i);

		auto ellip = opengl::CEllipsoid3D::Create();

		ellip->setName(format("%u", static_cast<unsigned int>(i)));
		ellip->enableShowName(true);
		ellip->setPose(pointGauss.mean);
		ellip->setCovMatrix(pointGauss.cov);
		ellip->enableDrawSolid3D(false);
		ellip->setQuantiles(options.quantiles_3D_representation);
		ellip->set3DsegmentsCount(10);

		ellip->setColor(0, 0, 1);

		// Set color depending on partitions?
		if (options.doPartitioningExperiment)
		{
			// This is done for each landmark:
			map<int, bool> belongToPartition;
			const CSimpleMap* SFs =
				&m_SFs;	 // mapPartitioner.getSequenceOfFrames();

			for (size_t p = 0; p < m_lastPartitionSet.size(); p++)
			{
				for (size_t w = 0; w < m_lastPartitionSet[p].size(); w++)
				{
					// Check if landmark #i is in the SF of
					// m_lastPartitionSet[p][w]:
					CLandmark::TLandmarkID i_th_ID = m_IDs.inverse(i);

					// Look for the lm_ID in the SF:
					CPose3DPDF::Ptr pdf;
					CSensoryFrame::Ptr SF_i;
					SFs->get(m_lastPartitionSet[p][w], pdf, SF_i);

					CObservationBearingRange::Ptr obs =
						SF_i->getObservationByClass<CObservationBearingRange>();

					for (auto& o : obs->sensedData)
					{
						if (o.landmarkID == i_th_ID)
						{
							belongToPartition[p] = true;
							break;
						}
					}  // end for o
				}  // end for w
			}  // end for p

			// Build label:
			string strParts("[");

			for (auto it = belongToPartition.begin();
				 it != belongToPartition.end(); ++it)
			{
				if (it != belongToPartition.begin()) strParts += string(",");
				strParts += format("%i", it->first);
			}

			ellip->setName(ellip->getName() + strParts + string("]"));
		}

		outObj->insert(ellip);
	}
}

/*---------------------------------------------------------------
						getLastPartitionLandmarks
  ---------------------------------------------------------------*/
void CRangeBearingKFSLAM::getLastPartitionLandmarksAsIfFixedSubmaps(
	size_t K, std::vector<std::vector<uint32_t>>& landmarksMembership)
{
	// temporary copy:
	std::vector<std::vector<uint32_t>> tmpParts = m_lastPartitionSet;

	// Fake "m_lastPartitionSet":

	// Fixed partitions every K observations:
	vector<std::vector<uint32_t>> partitions;
	std::vector<uint32_t> tmpCluster;

	for (size_t i = 0; i < m_SFs.size(); i++)
	{
		tmpCluster.push_back(i);
		if ((i % K) == 0)
		{
			partitions.push_back(tmpCluster);
			tmpCluster.clear();
			tmpCluster.push_back(
				i);	 // This observation "i" is shared between both clusters
		}
	}
	m_lastPartitionSet = partitions;

	// Call the actual method:
	getLastPartitionLandmarks(landmarksMembership);

	// Replace copy:
	m_lastPartitionSet = tmpParts;	//-V519
}

/*---------------------------------------------------------------
						getLastPartitionLandmarks
  ---------------------------------------------------------------*/
void CRangeBearingKFSLAM::getLastPartitionLandmarks(
	std::vector<std::vector<uint32_t>>& landmarksMembership) const
{
	landmarksMembership.clear();

	// All the computation is made based on "m_lastPartitionSet"

	if (!options.doPartitioningExperiment) return;

	const size_t nLMs = this->getNumberOfLandmarksInTheMap();
	for (size_t i = 0; i < nLMs; i++)
	{
		map<int, bool> belongToPartition;
		const CSimpleMap* SFs =
			&m_SFs;	 // mapPartitioner.getSequenceOfFrames();

		for (size_t p = 0; p < m_lastPartitionSet.size(); p++)
		{
			for (size_t w = 0; w < m_lastPartitionSet[p].size(); w++)
			{
				// Check if landmark #i is in the SF of
				// m_lastPartitionSet[p][w]:
				CLandmark::TLandmarkID i_th_ID = m_IDs.inverse(i);

				// Look for the lm_ID in the SF:
				CPose3DPDF::Ptr pdf;
				CSensoryFrame::Ptr SF_i;
				SFs->get(m_lastPartitionSet[p][w], pdf, SF_i);

				CObservationBearingRange::Ptr obs =
					SF_i->getObservationByClass<CObservationBearingRange>();

				for (auto& o : obs->sensedData)
				{
					if (o.landmarkID == i_th_ID)
					{
						belongToPartition[p] = true;
						break;
					}
				}  // end for o
			}  // end for w
		}  // end for p

		// Build membership list:
		std::vector<uint32_t> membershipOfThisLM;

		for (auto& it : belongToPartition)
			membershipOfThisLM.push_back(it.first);

		landmarksMembership.push_back(membershipOfThisLM);
	}  // end for i
}

/*---------------------------------------------------------------
			computeOffDiagonalBlocksApproximationError
  ---------------------------------------------------------------*/
double CRangeBearingKFSLAM::computeOffDiagonalBlocksApproximationError(
	const std::vector<std::vector<uint32_t>>& landmarksMembership) const
{
	MRPT_START

	// Compute the information matrix:
	CMatrixDynamic<kftype> fullCov(m_pkk);
	size_t i;
	for (i = 0; i < get_vehicle_size(); i++)
		fullCov(i, i) = max(fullCov(i, i), 1e-6);

	CMatrixDynamic<kftype> H(fullCov.inverse_LLt());
	H.array().abs();  // Replace by absolute values:

	double sumOffBlocks = 0;
	unsigned int nLMs = landmarksMembership.size();

	ASSERT_(
		int(get_vehicle_size() + nLMs * get_feature_size()) == fullCov.cols());

	for (i = 0; i < nLMs; i++)
	{
		for (size_t j = i + 1; j < nLMs; j++)
		{
			// Sum the cross cov. between LM(i) and LM(j)??
			// --> Only if there is no common cluster:
			if (0 ==
				math::countCommonElements(
					landmarksMembership[i], landmarksMembership[j]))
			{
				size_t col = get_vehicle_size() + i * get_feature_size();
				size_t row = get_vehicle_size() + j * get_feature_size();
				sumOffBlocks += 2 * H.block<2, 2>(row, col).sum();
			}
		}
	}

	return sumOffBlocks /
		H.asEigen()
			.block(
				get_vehicle_size(), get_vehicle_size(),
				H.rows() - get_vehicle_size(),
				H.cols() - get_vehicle_size())
			.sum();	 // Starting (7,7)-end
	MRPT_END
}

/*---------------------------------------------------------------
			  reconsiderPartitionsNow
  ---------------------------------------------------------------*/
void CRangeBearingKFSLAM::reconsiderPartitionsNow()
{
	// A different buffer for making this
	// thread-safe some day...
	vector<std::vector<uint32_t>> partitions;
	mapPartitioner.updatePartitions(partitions);
	m_lastPartitionSet = partitions;
}

/*---------------------------------------------------------------
			  saveMapAndPathRepresentationAsMATLABFile
  ---------------------------------------------------------------*/
void CRangeBearingKFSLAM::saveMapAndPath2DRepresentationAsMATLABFile(
	const string& fil, float stdCount, const string& styleLandmarks,
	const string& stylePath, const string& styleRobot) const
{
	FILE* f = os::fopen(fil.c_str(), "wt");
	if (!f) return;

	CMatrixDouble cov(2, 2);
	CVectorDouble mean(2);

	// Header:
	os::fprintf(
		f,
		"%%--------------------------------------------------------------------"
		"\n");
	os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
	os::fprintf(
		f,
		"%% "
		"'CRangeBearingKFSLAM::saveMapAndPath2DRepresentationAsMATLABFile'\n");
	os::fprintf(f, "%%\n");
	os::fprintf(f, "%%                        ~ MRPT ~\n");
	os::fprintf(
		f, "%%  Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
	os::fprintf(f, "%%      https://www.mrpt.org/     \n");
	os::fprintf(
		f,
		"%%--------------------------------------------------------------------"
		"\n");

	// Main code:
	os::fprintf(f, "hold on;\n\n");

	const size_t nLMs = this->getNumberOfLandmarksInTheMap();

	for (size_t i = 0; i < nLMs; i++)
	{
		size_t idx = get_vehicle_size() + i * get_feature_size();

		cov(0, 0) = m_pkk(idx + 0, idx + 0);
		cov(1, 1) = m_pkk(idx + 1, idx + 1);
		cov(0, 1) = cov(1, 0) = m_pkk(idx + 0, idx + 1);

		mean[0] = m_xkk[idx + 0];
		mean[1] = m_xkk[idx + 1];

		// Command to draw the 2D ellipse:
		os::fprintf(
			f, "%s",
			math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleLandmarks)
				.c_str());
	}

	// Now: the robot path:
	// ------------------------------
	if (m_SFs.size())
	{
		os::fprintf(f, "\nROB_PATH=[");
		for (size_t i = 0; i < m_SFs.size(); i++)
		{
			CSensoryFrame::Ptr dummySF;
			CPose3DPDF::Ptr pdf3D;
			m_SFs.get(i, pdf3D, dummySF);

			CPose3D p;
			pdf3D->getMean(p);

			os::fprintf(f, "%.04f %.04f", p.x(), p.y());
			if (i < (m_SFs.size() - 1)) os::fprintf(f, ";");
		}
		os::fprintf(f, "];\n");

		os::fprintf(
			f, "plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n", stylePath.c_str());
	}

	// The robot pose:
	cov(0, 0) = m_pkk(0, 0);
	cov(1, 1) = m_pkk(1, 1);
	cov(0, 1) = cov(1, 0) = m_pkk(0, 1);

	mean[0] = m_xkk[0];
	mean[1] = m_xkk[1];

	os::fprintf(
		f, "%s",
		math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleRobot).c_str());

	os::fprintf(f, "\naxis equal;\n");
	os::fclose(f);
}

/** Computes A=A-B, which may need to be re-implemented depending on the
 * topology of the individual scalar components (eg, angles).
 */
void CRangeBearingKFSLAM::OnSubstractObservationVectors(
	KFArray_OBS& A, const KFArray_OBS& B) const
{
	A -= B;
	mrpt::math::wrapToPiInPlace(A[1]);
	mrpt::math::wrapToPiInPlace(A[2]);
}

/** Return the observation NOISE covariance matrix, that is, the model of the
 * Gaussian additive noise of the sensor.
 * \param out_R The noise covariance matrix. It might be non diagonal, but
 * it'll usually be.
 */
void CRangeBearingKFSLAM::OnGetObservationNoise(KFMatrix_OxO& out_R) const
{
	out_R(0, 0) = square(options.std_sensor_range);
	out_R(1, 1) = square(options.std_sensor_yaw);
	out_R(2, 2) = square(options.std_sensor_pitch);
}

/** This will be called before OnGetObservationsAndDataAssociation to allow the
 * application to reduce the number of covariance landmark predictions to be
 * made.
 *  For example, features which are known to be "out of sight" shouldn't be
 * added to the output list to speed up the calculations.
 * \param in_all_prediction_means The mean of each landmark predictions; the
 * computation or not of the corresponding covariances is what we're trying to
 * determined with this method.
 * \param out_LM_indices_to_predict The list of landmark indices in the map
 * [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
 * \note This is not a pure virtual method, so it should be implemented only if
 * desired. The default implementation returns a vector with all the landmarks
 * in the map.
 * \sa OnGetObservations, OnDataAssociation
 */
void CRangeBearingKFSLAM::OnPreComputingPredictions(
	const vector_KFArray_OBS& prediction_means,
	std::vector<size_t>& out_LM_indices_to_predict) const
{
	CObservationBearingRange::Ptr obs =
		m_SF->getObservationByClass<CObservationBearingRange>();
	ASSERTMSG_(
		obs,
		"*ERROR*: This method requires an observation of type "
		"CObservationBearingRange");

#define USE_HEURISTIC_PREDICTION

#ifdef USE_HEURISTIC_PREDICTION
	const double sensor_max_range = obs->maxSensorDistance;
	const double fov_yaw = obs->fieldOfView_yaw;
	const double fov_pitch = obs->fieldOfView_pitch;

	const double max_vehicle_loc_uncertainty =
		4 * std::sqrt(m_pkk(0, 0) + m_pkk(1, 1) + m_pkk(2, 2));
#endif

	out_LM_indices_to_predict.clear();
	for (size_t i = 0; i < prediction_means.size(); i++)
	{
#ifndef USE_HEURISTIC_PREDICTION
		out_LM_indices_to_predict.push_back(i);
#else
		// Heuristic: faster but doesn't work always!
		if (prediction_means[i][0] <
				(15 + sensor_max_range + max_vehicle_loc_uncertainty +
				 4 * options.std_sensor_range) &&
			fabs(prediction_means[i][1]) <
				(30.0_deg + 0.5 * fov_yaw + 4 * options.std_sensor_yaw) &&
			fabs(prediction_means[i][2]) <
				(30.0_deg + 0.5 * fov_pitch + 4 * options.std_sensor_pitch))
		{ out_LM_indices_to_predict.push_back(i); }
#endif
	}
}


// Copyright (c) Facebook, Inc. and its affiliates.

// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory of this source tree.
#include <fstream>
#include <string>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <torch/script.h>
#include <torch/torch.h>

#include "dtt.h"
#include "pinocchio_wrapper.hpp"
#include "rotations.hpp"

extern "C" {

torch::Tensor validTensor(torch::Tensor x) {
  if (x.dim() < 2) {
    x = x.unsqueeze(1);
  }
  return x.to(torch::kDouble);
}

Eigen::VectorXd matrixToVector(Eigen::MatrixXd A) {
  return Eigen::VectorXd(
      Eigen::Map<Eigen::VectorXd>(A.data(), A.cols() * A.rows()));
}

struct RobotModelPinocchio : torch::CustomClassHolder {
  Eigen::VectorXd ik_sol_p_;
  Eigen::VectorXd ik_sol_v_;

  pinocchio_wrapper::State *pinocchio_state_ = nullptr;

  std::string xml_buffer_;
  std::string ee_link_name_;

  RobotModelPinocchio(std::string urdf_filename, std::string ee_link_name) {
    ee_link_name_ = ee_link_name;

    std::ifstream stream(urdf_filename);
    std::stringstream buffer;
    buffer << stream.rdbuf();
    xml_buffer_ = buffer.str();

    initialize();
  }

  RobotModelPinocchio(std::vector<std::string> serialized_state) {
    ee_link_name_ = serialized_state[0];
    xml_buffer_ = serialized_state[1];
    initialize();
  }

  ~RobotModelPinocchio() { pinocchio_wrapper::destroy(pinocchio_state_); }

  void initialize() {
    pinocchio_state_ = pinocchio_wrapper::initialize(ee_link_name_.c_str(),
                                                     xml_buffer_.c_str());
  }

  c10::List<torch::Tensor> get_joint_angle_limits(void) {
    c10::List<torch::Tensor> result;
    int nq = pinocchio_wrapper::get_nq(pinocchio_state_);
    torch::Tensor l_result = torch::zeros(nq, torch::kFloat32);
    torch::Tensor u_result = torch::zeros(nq, torch::kFloat32);

    auto lower_limit =
        pinocchio_wrapper::get_lower_position_limits(pinocchio_state_);
    auto upper_limit =
        pinocchio_wrapper::get_upper_position_limits(pinocchio_state_);

    for (int i = 0; i < nq; i++) {
      l_result[i] = lower_limit[i];
      u_result[i] = upper_limit[i];
    }
    result.push_back(l_result);
    result.push_back(u_result);

    return result;
  }

  torch::Tensor get_joint_velocity_limits(void) {
    int nq = pinocchio_wrapper::get_nq(pinocchio_state_);
    torch::Tensor result = torch::zeros(nq, torch::kFloat32);
    auto velocity_limit =
        pinocchio_wrapper::get_velocity_limits(pinocchio_state_);

    for (int i = 0; i < nq; i++) {
      result[i] = velocity_limit[i];
    }

    return result;
  }

  c10::List<torch::Tensor> forward_kinematics(torch::Tensor joint_positions) {
    c10::List<torch::Tensor> result;
    torch::Tensor pos_result = torch::zeros(3, torch::kFloat32);
    torch::Tensor quat_result = torch::zeros(4, torch::kFloat32);

    joint_positions = validTensor(joint_positions);
    auto result_intermediate = pinocchio_wrapper::forward_kinematics(
        pinocchio_state_,
        matrixToVector(dtt::libtorch2eigen<double>(joint_positions)));

    for (int i = 0; i < 3; i++) {
      pos_result[i] = result_intermediate[i];
    }
    for (int i = 0; i < 4; i++) {
      quat_result[i] = result_intermediate[i + 3];
    }

    result.push_back(pos_result);
    result.push_back(quat_result);

    return result;
  }

  torch::Tensor compute_jacobian(torch::Tensor joint_positions) {
    int nq = pinocchio_wrapper::get_nq(pinocchio_state_);
    joint_positions = validTensor(joint_positions);

    torch::Tensor result = torch::zeros({6, nq}, torch::kFloat64);
    Eigen::Map<dtt::MatrixXrm<double>> J(result.data_ptr<double>(),
                                         result.size(0), result.size(1));
    pinocchio_wrapper::compute_jacobian(
        pinocchio_state_,
        matrixToVector(dtt::libtorch2eigen<double>(joint_positions)), J);

    return result;
  }

  torch::Tensor inverse_dynamics(torch::Tensor joint_positions,
                                 torch::Tensor joint_velocities,
                                 torch::Tensor joint_accelerations) {
    joint_positions = validTensor(joint_positions);
    joint_velocities = validTensor(joint_velocities);
    joint_accelerations = validTensor(joint_accelerations);
    auto q = matrixToVector(dtt::libtorch2eigen<double>(joint_positions));
    auto v = matrixToVector(dtt::libtorch2eigen<double>(joint_velocities));
    auto a = matrixToVector(dtt::libtorch2eigen<double>(joint_accelerations));

    Eigen::Matrix<double, Eigen::Dynamic, 1> tau =
        pinocchio_wrapper::inverse_dynamics(pinocchio_state_, q, v, a);
    std::vector<int64_t> dims = {tau.rows()};
    return torch::from_blob(tau.data(), dims, torch::kFloat64).clone();
  }

  torch::Tensor inverse_kinematics(torch::Tensor ee_pos, torch::Tensor ee_quat,
                                   torch::Tensor rest_pose, double eps = 1e-4,
                                   int64_t max_iters = 1000, double dt = 0.1,
                                   double damping = 1e-12) {
    ee_pos = validTensor(ee_pos);
    Eigen::Vector3d ee_pos_(
        Eigen::Map<Eigen::Vector3d>(ee_pos.data_ptr<double>(), 3));

    auto quat = tensor4ToQuat(ee_quat);
    auto ee_quat_ = Eigen::Quaterniond(quat).cast<double>();

    rest_pose = validTensor(rest_pose);
    ik_sol_p_ = matrixToVector(dtt::libtorch2eigen<double>(rest_pose));

    pinocchio_wrapper::inverse_kinematics(pinocchio_state_, ee_pos_, ee_quat_,
                                          ik_sol_p_, eps, max_iters, dt,
                                          damping);
    std::vector<int64_t> dims = {ik_sol_p_.rows()};
    return torch::from_blob(ik_sol_p_.data(), dims, torch::kFloat64).clone();
  }
};

TORCH_LIBRARY(torchscript_pinocchio, m) {
  m.class_<RobotModelPinocchio>("RobotModelPinocchio")
      .def(torch::init<std::string, std::string>())
      .def("get_joint_angle_limits",
           &RobotModelPinocchio::get_joint_angle_limits)
      .def("get_joint_velocity_limits",
           &RobotModelPinocchio::get_joint_velocity_limits)
      .def("forward_kinematics", &RobotModelPinocchio::forward_kinematics)
      .def("compute_jacobian", &RobotModelPinocchio::compute_jacobian)
      .def("inverse_dynamics", &RobotModelPinocchio::inverse_dynamics)
      .def("inverse_kinematics", &RobotModelPinocchio::inverse_kinematics)
      .def_pickle(
          // __getstate__
          [](const c10::intrusive_ptr<RobotModelPinocchio> &self)
              -> std::vector<std::string> {
            return std::vector<std::string>{self->ee_link_name_,
                                            self->xml_buffer_};
          },
          // __setstate__
          [](std::vector<std::string> state)
              -> c10::intrusive_ptr<RobotModelPinocchio> {
            return c10::make_intrusive<RobotModelPinocchio>(std::move(state));
          });
}

} /* extern "C" */


/*
 * 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.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 *
 * The Original Code is Copyright (C) 2015 Blender Foundation.
 * All rights reserved.
 */

#ifndef __EIGEN3_SVD_C_API_CC__
#define __EIGEN3_SVD_C_API_CC__

/* Eigen gives annoying huge amount of warnings here, silence them! */
#if defined(__GNUC__) && !defined(__clang__)
#  pragma GCC diagnostic ignored "-Wlogical-op"
#endif

#ifdef __EIGEN3_SVD_C_API_CC__ /* quiet warning */
#endif

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SVD>

#include "svd.h"

using Eigen::JacobiSVD;

using Eigen::NoQRPreconditioner;

using Eigen::ComputeThinU;
using Eigen::ComputeThinV;

using Eigen::Map;
using Eigen::MatrixXf;
using Eigen::VectorXf;

using Eigen::Matrix4f;

void EIG_svd_square_matrix(const int size, const float *matrix, float *r_U, float *r_S, float *r_V)
{
  /* Since our matrix is squared, we can use thinU/V. */
  unsigned int flags = (r_U ? ComputeThinU : 0) | (r_V ? ComputeThinV : 0);

  /* Blender and Eigen matrices are both column-major. */
  JacobiSVD<MatrixXf, NoQRPreconditioner> svd(Map<MatrixXf>((float *)matrix, size, size), flags);

  if (r_U) {
    Map<MatrixXf>(r_U, size, size) = svd.matrixU();
  }

  if (r_S) {
    Map<VectorXf>(r_S, size) = svd.singularValues();
  }

  if (r_V) {
    Map<MatrixXf>(r_V, size, size) = svd.matrixV();
  }
}

#endif /* __EIGEN3_SVD_C_API_CC__ */


#include "aikido/constraint/dart/TSR.hpp"

#include <cmath>
#include <random>
#include <stdexcept>
#include <vector>

#include <boost/format.hpp>
#include <dart/common/Console.hpp>
#include <dart/math/Geometry.hpp>

#include "aikido/common/memory.hpp"

using aikido::statespace::SE3;
using boost::format;
using boost::str;

namespace aikido {
namespace constraint {
namespace dart {

class TSRSampleGenerator : public SampleGenerator
{
public:
  TSRSampleGenerator(const TSRSampleGenerator&) = delete;
  TSRSampleGenerator(TSRSampleGenerator&& other) = delete;
  TSRSampleGenerator& operator=(const TSRSampleGenerator& other) = delete;
  TSRSampleGenerator& operator=(TSRSampleGenerator&& other) = delete;
  virtual ~TSRSampleGenerator() = default;

  // Documentation inherited.
  statespace::ConstStateSpacePtr getStateSpace() const override;

  /// Return a transform sampled from this TSR.
  ///
  /// This function uses the provided RNG to create a sample `Tw_s` from the
  /// `Bw` bounds matrix of this TSR, and returns the result:
  /// `T0_w * Tw_s * Tw_e`.
  ///
  /// \param[in] rng Random number generator from which to sample
  /// \return a transform within the bounds of this TSR.
  bool sample(statespace::StateSpace::State* _state) override;

  // Documentation inherited.
  bool canSample() const override;

  // Documentation inherited.
  int getNumSamples() const override;

private:
  // For internal use only.
  TSRSampleGenerator(
      std::unique_ptr<common::RNG> _rng,
      std::shared_ptr<statespace::SE3> _stateSpace,
      const Eigen::Isometry3d& _T0_w,
      const Eigen::Matrix<double, 6, 2>& _Bw,
      const Eigen::Isometry3d& _Tw_e);

  std::unique_ptr<common::RNG> mRng;

  std::shared_ptr<statespace::SE3> mStateSpace;

  /// Transformation from origin frame into "wiggle" frame.
  Eigen::Isometry3d mT0_w;

  /// Bounds on "wiggling" in `x, y, z, roll, pitch, yaw`.
  Eigen::Matrix<double, 6, 2> mBw;

  /// Transformation from "wiggle" frame into end frame.
  Eigen::Isometry3d mTw_e;

  // True for point TSR.
  bool mPointTSR;

  // True if point TSR and has already been sampled.
  bool mPointTSRSampled;

  friend class TSR;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

//==============================================================================
TSR::TSR(
    std::unique_ptr<common::RNG> _rng,
    const Eigen::Isometry3d& _T0_w,
    const Eigen::Matrix<double, 6, 2>& _Bw,
    const Eigen::Isometry3d& _Tw_e,
    double _testableTolerance)
  : mT0_w(_T0_w)
  , mBw(_Bw)
  , mTw_e(_Tw_e)
  , mTestableTolerance(_testableTolerance)
  , mRng(std::move(_rng))
  , mStateSpace(std::make_shared<SE3>())
{
  validate();
}

//==============================================================================
TSR::TSR(
    const Eigen::Isometry3d& _T0_w,
    const Eigen::Matrix<double, 6, 2>& _Bw,
    const Eigen::Isometry3d& _Tw_e,
    double _testableTolerance)
  : mT0_w(_T0_w)
  , mBw(_Bw)
  , mTw_e(_Tw_e)
  , mTestableTolerance(_testableTolerance)
  , mRng(std::unique_ptr<common::RNG>(
        new common::RNGWrapper<std::default_random_engine>(0)))
  , mStateSpace(std::make_shared<SE3>())
{
  validate();
}

//==============================================================================
TSR::TSR(const TSR& other)
  : mT0_w(other.mT0_w)
  , mBw(other.mBw)
  , mTw_e(other.mTw_e)
  , mTestableTolerance(other.mTestableTolerance)
  , mRng(other.mRng->clone())
  , mStateSpace(std::make_shared<SE3>())
{
  validate();
}

//==============================================================================
TSR::TSR(TSR&& other)
  : mT0_w(other.mT0_w)
  , mBw(other.mBw)
  , mTw_e(other.mTw_e)
  , mTestableTolerance(other.mTestableTolerance)
  , mRng(std::move(other.mRng))
  , mStateSpace(std::make_shared<SE3>())
{
  validate();
}

//==============================================================================
TSR& TSR::operator=(const TSR& other)
{

  mT0_w = other.mT0_w;
  mBw = other.mBw;
  mTw_e = other.mTw_e;
  mTestableTolerance = other.mTestableTolerance;
  mRng = other.mRng->clone();

  // Intentionally don't assign StateSpace.

  return *this;
}

//==============================================================================
TSR& TSR::operator=(TSR&& other)
{
  mT0_w = std::move(other.mT0_w);
  mBw = std::move(other.mBw);
  mTw_e = std::move(other.mTw_e);
  mTestableTolerance = other.mTestableTolerance;
  mRng = std::move(other.mRng);
  mStateSpace = std::move(other.mStateSpace);

  return *this;
}

//==============================================================================
std::shared_ptr<const statespace::StateSpace> TSR::getStateSpace() const
{
  return mStateSpace;
}

//==============================================================================
std::shared_ptr<statespace::SE3> TSR::getSE3() const
{
  return mStateSpace;
}

//==============================================================================
std::unique_ptr<SampleGenerator> TSR::createSampleGenerator() const
{
  validate();

  if (!mRng)
    throw std::invalid_argument("Random generator is nullptr.");

  for (int i = 0; i < 6; ++i)
  {
    for (int j = 0; j < 2; ++j)
    {
      if (!std::isfinite(mBw(i, j)))
        throw std::invalid_argument(
            str(format("Sampling requires finite bounds. Bw[%d, %d] is %f.") % i
                % j % mBw(i, j)));
    }
  }

  if (mBw.col(0) == mBw.col(1))
  {
    dtwarn << "[TSR::createSampleGenerator] This is a point TSR that represents"
              " a zero measure set in SE(3). The SampleGenerator<Isometry3d>"
              " returned by this function will sample the same pose infinitely"
              " many times.\n";
  }

  return std::unique_ptr<TSRSampleGenerator>(
      new TSRSampleGenerator(mRng->clone(), mStateSpace, mT0_w, mBw, mTw_e));
}

//==============================================================================
bool TSR::isSatisfied(
    const statespace::StateSpace::State* _s, TestableOutcome* outcome) const
{
  auto defaultOutcomeObject
      = dynamic_cast_or_throw<DefaultTestableOutcome>(outcome);

  Eigen::VectorXd dist;
  getValue(_s, dist);

  bool isSatisfiedResult = dist.norm() < mTestableTolerance;
  if (defaultOutcomeObject)
    defaultOutcomeObject->setSatisfiedFlag(isSatisfiedResult);
  return isSatisfiedResult;
}

//==============================================================================
std::unique_ptr<TestableOutcome> TSR::createOutcome() const
{
  return std::unique_ptr<TestableOutcome>(new DefaultTestableOutcome);
}

//==============================================================================
void TSR::validate() const
{
  // Assertion checks for min, max on bounds
  for (int i = 0; i < 6; i++)
  {
    if (mBw(i, 0) > mBw(i, 1))
    {
      throw std::logic_error(
          str(format("Lower bound exceeds upper bound on dimension %d: %f > %f")
              % i % mBw(i, 0) % mBw(i, 1)));
    }
  }
}

//==============================================================================
void TSR::setRNG(std::unique_ptr<common::RNG> rng)
{
  mRng = std::move(rng);
}

//==============================================================================
std::size_t TSR::getConstraintDimension() const
{
  return 6;
}

//==============================================================================
void TSR::getValue(
    const statespace::StateSpace::State* _s, Eigen::VectorXd& _out) const
{
  using SE3 = statespace::SE3;
  using SE3State = SE3::State;

  auto se3state = static_cast<const SE3State*>(_s);
  Eigen::Isometry3d se3 = se3state->getIsometry();

  using TransformTraits = Eigen::TransformTraits;

  Eigen::Isometry3d T0_w_inv = mT0_w.inverse(TransformTraits::Isometry);
  Eigen::Isometry3d Tw_e_inv = mTw_e.inverse(TransformTraits::Isometry);
  Eigen::Isometry3d Tw_s = T0_w_inv * se3 * Tw_e_inv;

  Eigen::Vector3d translation = Tw_s.translation();
  Eigen::Vector3d eulerOrig = ::dart::math::matrixToEulerZYX(Tw_s.linear());
  Eigen::Vector3d eulerZYX = eulerOrig.reverse();

  _out.resize(6);

  for (int i = 0; i < 3; ++i)
  {
    if (translation(i) < mBw(i, 0))
      _out(i) = std::abs(translation(i) - mBw(i, 0));

    else if (translation(i) > mBw(i, 1))
      _out(i) = std::abs(translation(i) - mBw(i, 1));

    else
      _out(i) = 0;
  }

  for (int i = 3; i < 6; ++i)
  {
    // Find n such that: 2*n*pi <= mBw(i, 0) < 2*(n+1)*pi
    int n = mBw(i, 0) / (2 * M_PI);

    // Map eulerZYX(i-3) to [2*n*pi, 2*(n+1)*pi)
    double angle = M_PI * 2 * n + eulerZYX(i - 3);

    // check if angle is within bound
    if ((angle >= mBw(i, 0) && angle <= mBw(i, 1))
        || (angle + M_PI * 2 >= mBw(i, 0) && angle + M_PI * 2 <= mBw(i, 1))
        || (angle - M_PI * 2 >= mBw(i, 0) && angle - M_PI * 2 <= mBw(i, 1)))
    {
      _out(i) = 0;
      continue;
    }

    // Take min-distance between angle and either side of bound
    if (angle < mBw(i, 0))
      _out(i) = std::min(mBw(i, 0) - angle, angle - (mBw(i, 1) - 2 * M_PI));

    else if (mBw(i, 1) < angle)
      _out(i) = std::min(angle - mBw(i, 1), mBw(i, 0) + 2 * M_PI - angle);
  }
}

//==============================================================================
void TSR::getJacobian(
    const statespace::StateSpace::State* _s, Eigen::MatrixXd& _out) const
{
  using SE3 = statespace::SE3;
  using SE3State = SE3::State;

  _out.resize(6, 6);

  auto se3state = static_cast<const SE3State*>(_s);
  Eigen::Isometry3d se3 = se3state->getIsometry();

  Eigen::Vector6d twist = ::dart::math::logMap(se3);

  Eigen::Vector6d posit(twist), negat(twist);

  static constexpr double eps = 1e-5;

  auto se3posit = mStateSpace->createState();
  auto se3negat = mStateSpace->createState();

  // Finite Differencing.
  for (int i = 0; i < 6; ++i)
  {
    posit(i) = twist(i) + eps;
    negat(i) = twist(i) - eps;

    se3posit.setIsometry(::dart::math::expMap(posit));
    se3negat.setIsometry(::dart::math::expMap(negat));

    Eigen::VectorXd positValue, negatValue;
    getValue(se3posit, positValue);
    getValue(se3negat, negatValue);

    Eigen::Vector6d diff = positValue - negatValue;
    _out.col(i) = diff / (2 * eps);

    posit(i) = twist(i);
    negat(i) = twist(i);
  }
}

//==============================================================================
std::vector<ConstraintType> TSR::getConstraintTypes() const
{
  return std::vector<ConstraintType>(6, ConstraintType::INEQUALITY);
}

//==============================================================================
bool TSR::project(
    const statespace::StateSpace::State* /*_s*/,
    statespace::StateSpace::State* /*_out*/) const
{
  // TODO
  return false;
}

//==============================================================================
TSRSampleGenerator::TSRSampleGenerator(
    std::unique_ptr<common::RNG> _rng,
    std::shared_ptr<SE3> _stateSpace,
    const Eigen::Isometry3d& _T0_w,
    const Eigen::Matrix<double, 6, 2>& _Bw,
    const Eigen::Isometry3d& _Tw_e)
  : mRng(std::move(_rng))
  , mStateSpace(std::move(_stateSpace))
  , mT0_w(_T0_w)
  , mBw(_Bw)
  , mTw_e(_Tw_e)
{
  if (!mRng)
  {
    throw std::invalid_argument("Random generator is empty.");
  }

  if (mBw.col(0) == mBw.col(1))
    mPointTSR = true;
  else
    mPointTSR = false;

  mPointTSRSampled = false;
}

//==============================================================================
statespace::ConstStateSpacePtr TSRSampleGenerator::getStateSpace() const
{
  return mStateSpace;
}

//==============================================================================
bool TSRSampleGenerator::sample(statespace::StateSpace::State* _state)
{
  if (mPointTSR && mPointTSRSampled)
    return false;

  using statespace::SE3;

  Eigen::Vector3d translation;
  Eigen::Vector3d angles;

  if (mPointTSR)
  {
    translation = mBw.block(0, 0, 3, 1);
    angles = mBw.block(3, 0, 3, 1);

    mPointTSRSampled = true;
  }
  else
  {
    std::vector<std::uniform_real_distribution<double> > distributions;
    for (int i = 0; i < 6; i++)
      distributions.emplace_back(mBw(i, 0), mBw(i, 1));

    for (int i = 0; i < 3; i++)
      translation(i) = distributions.at(i)(*mRng);

    for (int i = 0; i < 3; i++)
      angles(i) = distributions.at(i + 3)(*mRng);
  }

  Eigen::Isometry3d Tw_s;
  Tw_s.setIdentity();
  Tw_s.translation() = translation;
  Tw_s.linear() = ::dart::math::eulerZYXToMatrix(angles.reverse());

  Eigen::Isometry3d T0_s(mT0_w * Tw_s * mTw_e);
  mStateSpace->setIsometry(static_cast<SE3::State*>(_state), T0_s);

  return true;
}

//==============================================================================
double TSR::getTestableTolerance()
{
  return mTestableTolerance;
}

//==============================================================================
void TSR::setTestableTolerance(double _testableTolerance)
{
  mTestableTolerance = _testableTolerance;
}

//==============================================================================
bool TSRSampleGenerator::canSample() const
{
  if (mPointTSR && mPointTSRSampled)
    return false;

  return true;
}

//==============================================================================
int TSRSampleGenerator::getNumSamples() const
{
  if (mPointTSR && !mPointTSRSampled)
    return 1;

  if (mPointTSR && mPointTSRSampled)
    return 0;

  return NO_LIMIT;
}

} // namespace dart
} // namespace constraint
} // namespace aikido