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

#include <opengv/relative_pose/methods.hpp>
#include <opengv/Indices.hpp>

#include <Eigen/NonLinearOptimization>
#include <Eigen/NumericalDiff>
#include <Eigen/KroneckerProduct>
#include <Eigen/QR>

#include <opengv/OptimizationFunctor.hpp>
#include <opengv/math/arun.hpp>
#include <opengv/math/cayley.hpp>
#include <opengv/relative_pose/modules/main.hpp>
#include <opengv/triangulation/methods.hpp>

#include <iostream>

opengv::translation_t
opengv::relative_pose::twopt(
    const RelativeAdapterBase & adapter,
    bool unrotate,
    const std::vector<int> & indices )
{
  assert(indices.size()>1);
  return twopt( adapter, unrotate, indices[0], indices[1] );
};

opengv::translation_t
opengv::relative_pose::twopt(
    const RelativeAdapterBase & adapter,
    bool unrotate,
    size_t index0,
    size_t index1 )
{
  bearingVector_t f1 = adapter.getBearingVector1(index0);
  bearingVector_t f1prime = adapter.getBearingVector2(index0);
  bearingVector_t f2 = adapter.getBearingVector1(index1);
  bearingVector_t f2prime = adapter.getBearingVector2(index1);

  if(unrotate)
  {
    rotation_t R12 = adapter.getR12();
    f1prime = R12 * f1prime;
    f2prime = R12 * f2prime;
  }

  Eigen::Vector3d normal1 = f1.cross(f1prime);
  Eigen::Vector3d normal2 = f2.cross(f2prime);

  translation_t translation = normal1.cross(normal2);
  translation = translation/translation.norm();

  Eigen::Vector3d opticalFlow = f1 - f1prime;
  if( opticalFlow.dot(translation) < 0 )
    translation = -translation;

  return translation;
};

opengv::rotation_t
opengv::relative_pose::twopt_rotationOnly(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  assert(indices.size() > 1);
  return twopt_rotationOnly( adapter, indices[0], indices[1] );
};

opengv::rotation_t
opengv::relative_pose::twopt_rotationOnly(
    const RelativeAdapterBase & adapter,
    size_t index0,
    size_t index1)
{
  Eigen::Vector3d pointsCenter1 =
      adapter.getBearingVector1(index0) + adapter.getBearingVector1(index1);
  Eigen::Vector3d pointsCenter2 =
      adapter.getBearingVector2(index0) + adapter.getBearingVector2(index1);
  pointsCenter1 = pointsCenter1/3.0;
  pointsCenter2 = pointsCenter2/3.0;

  Eigen::MatrixXd Hcross(3,3);
  Hcross = Eigen::Matrix3d::Zero();

  Eigen::Vector3d f = adapter.getBearingVector1(index0) - pointsCenter1;
  Eigen::Vector3d fprime = adapter.getBearingVector2(index0) - pointsCenter2;
  Hcross += fprime * f.transpose();
  f = adapter.getBearingVector1(index1) - pointsCenter1;
  fprime = adapter.getBearingVector2(index1) - pointsCenter2;
  Hcross += fprime * f.transpose();

  return math::arun(Hcross);
};

namespace opengv
{
namespace relative_pose
{

rotation_t rotationOnly(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 2);

  Eigen::Vector3d pointsCenter1 = Eigen::Vector3d::Zero();
  Eigen::Vector3d pointsCenter2 = Eigen::Vector3d::Zero();

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    pointsCenter1 += adapter.getBearingVector1(indices[i]);
    pointsCenter2 += adapter.getBearingVector2(indices[i]);
  }

  pointsCenter1 = pointsCenter1 / numberCorrespondences;
  pointsCenter2 = pointsCenter2 / numberCorrespondences;

  Eigen::MatrixXd Hcross(3,3);
  Hcross = Eigen::Matrix3d::Zero();

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    Eigen::Vector3d f = adapter.getBearingVector1(indices[i]) - pointsCenter1;
    Eigen::Vector3d fprime =
        adapter.getBearingVector2(indices[i]) - pointsCenter2;
    Hcross += fprime * f.transpose();
  }

  return math::arun(Hcross);
};

}
}

opengv::rotation_t
opengv::relative_pose::rotationOnly( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return rotationOnly(adapter,idx);
};

opengv::rotation_t
opengv::relative_pose::rotationOnly(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return rotationOnly(adapter,idx);
};

namespace opengv
{
namespace relative_pose
{

complexEssentials_t fivept_stewenius(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 4);

  Eigen::MatrixXd Q(numberCorrespondences,9);
  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    //bearingVector_t f = adapter.getBearingVector1(indices[i]);
    //bearingVector_t fprime = adapter.getBearingVector2(indices[i]);
    //Stewenius' algorithm is computing the inverse transformation, so we simply
    //invert the input here
    bearingVector_t f = adapter.getBearingVector2(indices[i]);
    bearingVector_t fprime = adapter.getBearingVector1(indices[i]);
    Eigen::Matrix<double,1,9> row;
    row <<  f[0]*fprime[0], f[1]*fprime[0], f[2]*fprime[0],
        f[0]*fprime[1], f[1]*fprime[1], f[2]*fprime[1],
        f[0]*fprime[2], f[1]*fprime[2], f[2]*fprime[2];
    Q.row(i) = row;
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVD(Q, Eigen::ComputeFullV );
  Eigen::Matrix<double,9,4> EE = SVD.matrixV().block(0,5,9,4);
  complexEssentials_t complexEssentials;
  modules::fivept_stewenius_main(EE,complexEssentials);
  return complexEssentials;
};

}
}

opengv::complexEssentials_t
opengv::relative_pose::fivept_stewenius( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return fivept_stewenius(adapter,idx);
};

opengv::complexEssentials_t
opengv::relative_pose::fivept_stewenius(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return fivept_stewenius(adapter,idx);
};

namespace opengv
{
namespace relative_pose
{

essentials_t fivept_nister(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 4);

  Eigen::MatrixXd Q(numberCorrespondences,9);
  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    //bearingVector_t f = adapter.getBearingVector1(indices[i]);
    //bearingVector_t fprime = adapter.getBearingVector2(indices[i]);
    //Nister's algorithm is computing the inverse transformation, so we simply
    //invert the input here
    bearingVector_t f = adapter.getBearingVector2(indices[i]);
    bearingVector_t fprime = adapter.getBearingVector1(indices[i]);
    Eigen::Matrix<double,1,9> row;
    row <<  f[0]*fprime[0], f[1]*fprime[0], f[2]*fprime[0],
        f[0]*fprime[1], f[1]*fprime[1], f[2]*fprime[1],
        f[0]*fprime[2], f[1]*fprime[2], f[2]*fprime[2];
    Q.row(i) = row;
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVD(Q, Eigen::ComputeFullV );
  Eigen::Matrix<double,9,4> EE = SVD.matrixV().block(0,5,9,4);
  essentials_t essentials;
  modules::fivept_nister_main(EE,essentials);

  return essentials;
};

}
}

opengv::essentials_t
opengv::relative_pose::fivept_nister( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return fivept_nister(adapter,idx);
};

opengv::essentials_t
opengv::relative_pose::fivept_nister(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return fivept_nister(adapter,idx);
};

opengv::rotations_t
opengv::relative_pose::fivept_kneip(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences == 5);

  Eigen::Matrix<double,3,5> f1;
  Eigen::Matrix<double,3,5> f2;

  for(size_t i = 0; i < numberCorrespondences; i++)
  {
    f1.col(i) = adapter.getBearingVector1(indices[i]);
    f2.col(i) = adapter.getBearingVector2(indices[i]);
  }

  rotations_t rotations;
  modules::fivept_kneip_main( f1, f2, rotations );
  return rotations;
}

namespace opengv
{
namespace relative_pose
{

essentials_t sevenpt(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 6);

  Eigen::MatrixXd A(numberCorrespondences,9);

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    //bearingVector_t f1 = adapter.getBearingVector1(indices[i]);
    //bearingVector_t f2 = adapter.getBearingVector2(indices[i]);
    //The seven-point is computing the inverse transformation, which is why we
    //invert the input
    bearingVector_t f1 = adapter.getBearingVector2(indices[i]);
    bearingVector_t f2 = adapter.getBearingVector1(indices[i]);

    A.block<1,3>(i,0) = f2[0] * f1.transpose();
    A.block<1,3>(i,3) = f2[1] * f1.transpose();
    A.block<1,3>(i,6) = f2[2] * f1.transpose();
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVD(
      A,
      Eigen::ComputeFullU | Eigen::ComputeFullV );

  Eigen::Matrix<double,9,1> f1 = SVD.matrixV().col(8);
  Eigen::Matrix<double,9,1> f2 = SVD.matrixV().col(7);

  Eigen::MatrixXd F1_temp(3,3);
  F1_temp.col(0) = f1.block<3,1>(0,0);
  F1_temp.col(1) = f1.block<3,1>(3,0);
  F1_temp.col(2) = f1.block<3,1>(6,0);
  essential_t F1 = F1_temp.transpose();

  Eigen::MatrixXd F2_temp(3,3);
  F2_temp.col(0) = f2.block<3,1>(0,0);
  F2_temp.col(1) = f2.block<3,1>(3,0);
  F2_temp.col(2) = f2.block<3,1>(6,0);
  essential_t F2 = F2_temp.transpose();

  double eps = 0.00000001;
  essentials_t essentials;

  if( fabs(F1.determinant()) < eps || numberCorrespondences > 7 )
  {
    essentials.push_back(F1);
  }
  else
  {
    essential_t M = F2.inverse() * F1;
    Eigen::EigenSolver< essential_t > Eig(M,true);
    Eigen::Matrix< std::complex<double>,3,1 > D = Eig.eigenvalues();

    double val1 = fabs(D(0,0).imag());
    double val2 = fabs(D(1,0).imag());
    double val3 = fabs(D(2,0).imag());

    if( val1 < eps && val2 < eps && val3 < eps )
    {
      essentials.push_back( F1 - D(0,0).real() * F2 );
      essentials.push_back( F1 - D(1,0).real() * F2 );
      essentials.push_back( F1 - D(2,0).real() * F2 );
    }
    else
    {
      double min = val1;
      int minIndex = 0;
      if( val2 < min )
      {
        min = val2;
        minIndex = 1;
      }
      if( val3 < min )
        minIndex = 2;

      essentials.push_back( F1 - D(minIndex,0).real() * F2 );
    }
  }

  return essentials;
}

}
}

opengv::essentials_t
opengv::relative_pose::sevenpt( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return sevenpt(adapter,idx);
}

opengv::essentials_t
opengv::relative_pose::sevenpt(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return sevenpt(adapter,idx);
}

namespace opengv
{
namespace relative_pose
{

essential_t eightpt(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 7);

  Eigen::MatrixXd A(numberCorrespondences,9);

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    //bearingVector_t f1 = adapter.getBearingVector1(indices[i]);
    //bearingVector_t f2 = adapter.getBearingVector2(indices[i]);
    //The eight-point essentially computes the inverse transformation, which is
    //why we invert the input here
    bearingVector_t f1 = adapter.getBearingVector2(indices[i]);
    bearingVector_t f2 = adapter.getBearingVector1(indices[i]);

    A.block<1,3>(i,0) = f2[0] * f1.transpose();
    A.block<1,3>(i,3) = f2[1] * f1.transpose();
    A.block<1,3>(i,6) = f2[2] * f1.transpose();
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVD(
      A,
      Eigen::ComputeFullU | Eigen::ComputeFullV );
  Eigen::Matrix<double,9,1> f = SVD.matrixV().col(8);

  Eigen::MatrixXd F_temp(3,3);
  F_temp.col(0) = f.block<3,1>(0,0);
  F_temp.col(1) = f.block<3,1>(3,0);
  F_temp.col(2) = f.block<3,1>(6,0);
  essential_t F = F_temp.transpose();

  Eigen::JacobiSVD< Eigen::MatrixXd > SVD2(
      F,
      Eigen::ComputeFullU | Eigen::ComputeFullV );
  Eigen::Matrix3d S = Eigen::Matrix3d::Zero();
  S(0,0) = SVD2.singularValues()[0];
  S(1,1) = SVD2.singularValues()[1];

  Eigen::Matrix3d U = SVD2.matrixU();
  Eigen::Matrix3d Vtr = SVD2.matrixV().transpose();

  essential_t essential = U * S * Vtr;
  return essential;
}

}
}

opengv::essential_t
opengv::relative_pose::eightpt( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return eightpt(adapter,idx);
}

opengv::essential_t
opengv::relative_pose::eightpt(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return eightpt(adapter,idx);
}

namespace opengv
{
namespace relative_pose
{

rotation_t eigensolver(
    const RelativeAdapterBase & adapter,
    const Indices & indices,
    eigensolverOutput_t & output,
    bool useWeights )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 4);

  Eigen::Matrix3d xxF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d yyF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d zzF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d xyF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d yzF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d zxF = Eigen::Matrix3d::Zero();

  //compute the norm of all the scores
  double norm = 0.0;
  for(size_t i=0; i < numberCorrespondences; i++)
    norm += pow(adapter.getWeight(indices[i]),2);
  norm = sqrt(norm);

  //Fill summation terms
  for(size_t i=0; i < numberCorrespondences; i++)
  {
    bearingVector_t f1 = adapter.getBearingVector1(indices[i]);
    bearingVector_t f2 = adapter.getBearingVector2(indices[i]);
    Eigen::Matrix3d F = f2*f2.transpose();

    double weight = 1.0;
    if( useWeights )
      weight = adapter.getWeight(indices[i])/norm;

    xxF = xxF + weight*f1[0]*f1[0]*F;
    yyF = yyF + weight*f1[1]*f1[1]*F;
    zzF = zzF + weight*f1[2]*f1[2]*F;
    xyF = xyF + weight*f1[0]*f1[1]*F;
    yzF = yzF + weight*f1[1]*f1[2]*F;
    zxF = zxF + weight*f1[2]*f1[0]*F;
  }

  //Do minimization
  modules::eigensolver_main(xxF,yyF,zzF,xyF,yzF,zxF,output);

  //Correct the translation
  bearingVector_t f1 = adapter.getBearingVector1(indices[0]);
  bearingVector_t f2 = adapter.getBearingVector2(indices[0]);
  f2 = output.rotation * f2;
  Eigen::Vector3d opticalFlow = f1 - f2;
  if( opticalFlow.dot(output.translation) < 0.0 )
    output.translation = -output.translation;

  return output.rotation;
}

}
}

opengv::rotation_t
opengv::relative_pose::eigensolver(
    const RelativeAdapterBase & adapter,
    eigensolverOutput_t & output,
    bool useWeights )
{
  Indices idx(adapter.getNumberCorrespondences());
  return eigensolver(adapter,idx,output,useWeights);
}

opengv::rotation_t
opengv::relative_pose::eigensolver(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices,
    eigensolverOutput_t & output,
    bool useWeights )
{
  Indices idx(indices);
  return eigensolver(adapter,idx,output,useWeights);
}

opengv::rotation_t
opengv::relative_pose::eigensolver(
    const RelativeAdapterBase & adapter,
    bool useWeights )
{
  eigensolverOutput_t output;
  output.rotation = adapter.getR12();
  return eigensolver(adapter,output,useWeights);
}

opengv::rotation_t
opengv::relative_pose::eigensolver(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices,
    bool useWeights )
{
  eigensolverOutput_t output;
  output.rotation = adapter.getR12();
  return eigensolver(adapter,indices,output,useWeights);
}

namespace opengv
{
namespace relative_pose
{

rotations_t sixpt(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences == 6);

  Eigen::Matrix<double,6,6> L1;
  Eigen::Matrix<double,6,6> L2;

  for(size_t i = 0; i < numberCorrespondences; i++)
  {
    bearingVector_t f1 =
        adapter.getCamRotation1(indices[i]) * adapter.getBearingVector1(indices[i]);
    bearingVector_t f2 =
        adapter.getCamRotation2(indices[i]) * adapter.getBearingVector2(indices[i]);

    L1.block<3,1>(0,i) = f1;
    L2.block<3,1>(0,i) = f2;

    L1.block<3,1>(3,i) = f1.cross(adapter.getCamOffset1(indices[i]));
    L2.block<3,1>(3,i) = f2.cross(adapter.getCamOffset2(indices[i]));
  }

  rotations_t solutions;
  modules::sixpt_main( L1, L2, solutions );
  return solutions;
}

}
}

opengv::rotations_t
opengv::relative_pose::sixpt(
    const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return sixpt(adapter,idx);
}

opengv::rotations_t
opengv::relative_pose::sixpt(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return sixpt(adapter,idx);
}

namespace opengv
{
namespace relative_pose
{

transformation_t ge(
    const RelativeAdapterBase & adapter,
    const Indices & indices,
    geOutput_t & output,
    bool useWeights )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 5);

  Eigen::Matrix3d xxF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d yyF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d zzF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d xyF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d yzF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d zxF = Eigen::Matrix3d::Zero();

  Eigen::Matrix<double,3,9> x1P = Eigen::Matrix<double,3,9>::Zero();
  Eigen::Matrix<double,3,9> y1P = Eigen::Matrix<double,3,9>::Zero();
  Eigen::Matrix<double,3,9> z1P = Eigen::Matrix<double,3,9>::Zero();
  Eigen::Matrix<double,3,9> x2P = Eigen::Matrix<double,3,9>::Zero();
  Eigen::Matrix<double,3,9> y2P = Eigen::Matrix<double,3,9>::Zero();
  Eigen::Matrix<double,3,9> z2P = Eigen::Matrix<double,3,9>::Zero();

  Eigen::Matrix<double,9,9> m11P = Eigen::Matrix<double,9,9>::Zero();
  Eigen::Matrix<double,9,9> m12P = Eigen::Matrix<double,9,9>::Zero();
  Eigen::Matrix<double,9,9> m22P = Eigen::Matrix<double,9,9>::Zero();

  //compute the norm of all the scores
  double norm = 0.0;
  for(size_t i=0; i < numberCorrespondences; i++)
    norm += pow(adapter.getWeight(indices[i]),2);
  norm = sqrt(norm);

  //Fill summation terms
  for(size_t i=0; i < numberCorrespondences; i++)
  {
    //get the weight of this feature
    double weight = 1.0;
    if( useWeights )
      weight = adapter.getWeight(indices[i])/norm;

    //unrotate the bearing vectors
    bearingVector_t f1 = adapter.getCamRotation1(indices[i]) *
        adapter.getBearingVector1(indices[i]);
    bearingVector_t f2 = adapter.getCamRotation2(indices[i]) *
        adapter.getBearingVector2(indices[i]);

    //compute the standard summation terms
    Eigen::Matrix3d F = f2*f2.transpose();

    xxF = xxF + weight*f1[0]*f1[0]*F;
    yyF = yyF + weight*f1[1]*f1[1]*F;
    zzF = zzF + weight*f1[2]*f1[2]*F;
    xyF = xyF + weight*f1[0]*f1[1]*F;
    yzF = yzF + weight*f1[1]*f1[2]*F;
    zxF = zxF + weight*f1[2]*f1[0]*F;

    //now compute the "cross"-summation terms
    Eigen::Vector3d t1 = adapter.getCamOffset1(indices[i]);
    Eigen::Vector3d t2 = adapter.getCamOffset2(indices[i]);

    Eigen::Matrix<double,1,9> f2_19;
    double temp = f1[1]*t1[2]-f1[2]*t1[1];
    f2_19(0,0) = f2[0] * temp;
    f2_19(0,1) = f2[1] * temp;
    f2_19(0,2) = f2[2] * temp;
    temp = f1[2]*t1[0]-f1[0]*t1[2];
    f2_19(0,3) = f2[0] * temp;
    f2_19(0,4) = f2[1] * temp;
    f2_19(0,5) = f2[2] * temp;
    temp = f1[0]*t1[1]-f1[1]*t1[0];
    f2_19(0,6) = f2[0] * temp;
    f2_19(0,7) = f2[1] * temp;
    f2_19(0,8) = f2[2] * temp;

    Eigen::Matrix<double,1,9> f1_19;
    temp = f2[1]*t2[2]-f2[2]*t2[1];
    f1_19(0,0) = f1[0] * temp;
    f1_19(0,1) = f1[1] * temp;
    f1_19(0,2) = f1[2] * temp;
    temp = f2[2]*t2[0]-f2[0]*t2[2];
    f1_19(0,3) = f1[0] * temp;
    f1_19(0,4) = f1[1] * temp;
    f1_19(0,5) = f1[2] * temp;
    temp = f2[0]*t2[1]-f2[1]*t2[0];
    f1_19(0,6) = f1[0] * temp;
    f1_19(0,7) = f1[1] * temp;
    f1_19(0,8) = f1[2] * temp;

    if( useWeights )
    {
      x1P = x1P + ( (weight * f1[0]) * f2 ) * f1_19;
      y1P = y1P + ( (weight * f1[1]) * f2 ) * f1_19;
      z1P = z1P + ( (weight * f1[2]) * f2 ) * f1_19;

      x2P = x2P + ( (weight * f1[0]) * f2 ) * f2_19;
      y2P = y2P + ( (weight * f1[1]) * f2 ) * f2_19;
      z2P = z2P + ( (weight * f1[2]) * f2 ) * f2_19;

      m11P = m11P - ( weight * f1_19.transpose() ) * f1_19;
      m22P = m22P - ( weight * f2_19.transpose() ) * f2_19;
      m12P = m12P - ( weight * f2_19.transpose() ) * f1_19;
    }
    else
    {
      x1P = x1P + ( f1[0] * f2 ) * f1_19;
      y1P = y1P + ( f1[1] * f2 ) * f1_19;
      z1P = z1P + ( f1[2] * f2 ) * f1_19;

      x2P = x2P + ( f1[0]) * f2 * f2_19;
      y2P = y2P + ( f1[1]) * f2 * f2_19;
      z2P = z2P + ( f1[2]) * f2 * f2_19;

      m11P = m11P - f1_19.transpose() * f1_19;
      m22P = m22P - f2_19.transpose() * f2_19;
      m12P = m12P - f2_19.transpose() * f1_19;
    }
  }

  Eigen::Vector3d pointsCenter1 = Eigen::Vector3d::Zero();
  Eigen::Vector3d pointsCenter2 = Eigen::Vector3d::Zero();

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    pointsCenter1 += adapter.getCamRotation1(indices[i]) *
        adapter.getBearingVector1(indices[i]);
    pointsCenter2 += adapter.getCamRotation2(indices[i]) *
        adapter.getBearingVector2(indices[i]);
  }

  pointsCenter1 = pointsCenter1 / numberCorrespondences;
  pointsCenter2 = pointsCenter2 / numberCorrespondences;

  Eigen::MatrixXd Hcross(3,3);
  Hcross = Eigen::Matrix3d::Zero();

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    Eigen::Vector3d f =      adapter.getCamRotation1(indices[i]) *
        adapter.getBearingVector1(indices[i]) - pointsCenter1;
    Eigen::Vector3d fprime = adapter.getCamRotation2(indices[i]) *
        adapter.getBearingVector2(indices[i]) - pointsCenter2;
    Hcross += fprime * f.transpose();
  }

  rotation_t startingRotation = math::arun(Hcross);

  //Do minimization
  modules::ge_main2(
      xxF, yyF, zzF, xyF, yzF, zxF,
      x1P, y1P, z1P, x2P, y2P, z2P,
      m11P, m12P, m22P, math::rot2cayley(startingRotation), output);

  transformation_t transformation;
  transformation.block<3,3>(0,0) = output.rotation;
  transformation.col(3) = output.translation.block<3,1>(0,0);
  return transformation;
}

}
}

opengv::transformation_t
opengv::relative_pose::ge(
    const RelativeAdapterBase & adapter,
    geOutput_t & output,
    bool useWeights )
{
  Indices idx(adapter.getNumberCorrespondences());
  return ge(adapter,idx,output,useWeights);
}

opengv::transformation_t
opengv::relative_pose::ge(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices,
    geOutput_t & output,
    bool useWeights )
{
  Indices idx(indices);
  return ge(adapter,idx,output,useWeights);
}

opengv::transformation_t
opengv::relative_pose::ge( const RelativeAdapterBase & adapter, bool useWeights )
{
  geOutput_t output;
  //output.rotation = adapter.getR12(); //finding starting value using arun
  return ge(adapter,output,useWeights);
}

opengv::transformation_t
opengv::relative_pose::ge(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices,
    bool useWeights )
{
  geOutput_t output;
  //output.rotation = adapter.getR12(); //finding starting value using arun
  return ge(adapter,indices,output,useWeights);
}

namespace opengv
{
namespace relative_pose
{

transformation_t seventeenpt(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 16);

  Eigen::MatrixXd AE(numberCorrespondences,9);
  Eigen::MatrixXd AR(numberCorrespondences,9);

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    bearingVector_t d1 = adapter.getBearingVector1(indices[i]);
    bearingVector_t d2 = adapter.getBearingVector2(indices[i]);
    translation_t v1 = adapter.getCamOffset1(indices[i]);
    translation_t v2 = adapter.getCamOffset2(indices[i]);
    rotation_t R1 = adapter.getCamRotation1(indices[i]);
    rotation_t R2 = adapter.getCamRotation2(indices[i]);

    //unrotate the bearing-vectors to express everything in the body frame
    d1 = R1*d1;
    d2 = R2*d2;

    //generate the Plücker line coordinates
    Eigen::Matrix<double,6,1> l1;
    l1.block<3,1>(0,0) = d1;
    l1.block<3,1>(3,0) = v1.cross(d1);
    Eigen::Matrix<double,6,1> l2;
    l2.block<3,1>(0,0) = d2;
    l2.block<3,1>(3,0) = v2.cross(d2);

    //fill line of matrix A
    AE(i,0) = l2[0]*l1[0];
    AE(i,1) = l2[0]*l1[1];
    AE(i,2) = l2[0]*l1[2];
    AE(i,3) = l2[1]*l1[0];
    AE(i,4) = l2[1]*l1[1];
    AE(i,5) = l2[1]*l1[2];
    AE(i,6) = l2[2]*l1[0];
    AE(i,7) = l2[2]*l1[1];
    AE(i,8) = l2[2]*l1[2];

    AR(i,0) = l2[0]*l1[3]+l2[3]*l1[0];
    AR(i,1) = l2[0]*l1[4]+l2[3]*l1[1];
    AR(i,2) = l2[0]*l1[5]+l2[3]*l1[2];
    AR(i,3) = l2[1]*l1[3]+l2[4]*l1[0];
    AR(i,4) = l2[1]*l1[4]+l2[4]*l1[1];
    AR(i,5) = l2[1]*l1[5]+l2[4]*l1[2];
    AR(i,6) = l2[2]*l1[3]+l2[5]*l1[0];
    AR(i,7) = l2[2]*l1[4]+l2[5]*l1[1];
    AR(i,8) = l2[2]*l1[5]+l2[5]*l1[2];
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVDARP(
      AR,
      Eigen::ComputeThinU | Eigen::ComputeThinV );

  Eigen::VectorXd sigma_ = SVDARP.singularValues();
  double pinvtoler_ =
      sigma_(0)*numberCorrespondences*NumTraits<double>::epsilon();
  Eigen::MatrixXd SigmaInverse_(9,9);
  SigmaInverse_ = Eigen::MatrixXd::Zero(9,9);
  for ( size_t i=0; i < 9; ++i)
  {
    double temp = sigma_(i);
    if( temp > pinvtoler_ )
      SigmaInverse_(i,i) = 1.0/temp;
  }

  Eigen::MatrixXd ARP(9,numberCorrespondences);
  ARP = SVDARP.matrixV()*SigmaInverse_*SVDARP.matrixU().transpose();

  Eigen::MatrixXd B(numberCorrespondences,numberCorrespondences);
  B = -Eigen::MatrixXd::Identity(numberCorrespondences,numberCorrespondences);
  B = B + AR*ARP;

  Eigen::MatrixXd C = B*AE;

  Eigen::JacobiSVD< Eigen::MatrixXd > SVDE(
      C,
      Eigen::ComputeThinU | Eigen::ComputeThinV );
  Eigen::Matrix<double,9,1> e = SVDE.matrixV().col(8);

  Eigen::MatrixXd E_temp(3,3);
  E_temp.col(0) = e.block<3,1>(0,0);
  E_temp.col(1) = e.block<3,1>(3,0);
  E_temp.col(2) = e.block<3,1>(6,0);
  essential_t E = E_temp.transpose();

  Eigen::JacobiSVD< Eigen::MatrixXd > SVDR(
      E,
      Eigen::ComputeFullV | Eigen::ComputeFullU );

  Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
  W(0,1) = -1.0;
  W(1,0) = 1.0;
  W(2,2) = 1.0;

  // get possible rotation and translation vectors
  rotation_t Ra = SVDR.matrixU() * W * SVDR.matrixV().transpose();
  rotation_t Rb = SVDR.matrixU() * W.transpose() * SVDR.matrixV().transpose();

  // change sign if det = -1
  if( Ra.determinant() < 0 ) Ra = -Ra;
  if( Rb.determinant() < 0 ) Rb = -Rb;

  Ra.transposeInPlace();
  Rb.transposeInPlace();

  Eigen::MatrixXd A_tra(numberCorrespondences,3);
  Eigen::MatrixXd A_trb(numberCorrespondences,3);
  Eigen::VectorXd b_tra(numberCorrespondences);
  Eigen::VectorXd b_trb(numberCorrespondences);

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    bearingVector_t d1 = adapter.getBearingVector1(indices[i]);
    bearingVector_t d2 = adapter.getBearingVector2(indices[i]);
    translation_t v1 = adapter.getCamOffset1(indices[i]);
    translation_t v2 = adapter.getCamOffset2(indices[i]);
    rotation_t R1 = adapter.getCamRotation1(indices[i]);
    rotation_t R2 = adapter.getCamRotation2(indices[i]);

    //unrotate the bearing-vectors to express everything in the body frame
    d1 = R1*d1;
    d2 = R2*d2;

    A_tra(i,0) = d1[2]*d2[0]*Ra(1,0)+d1[2]*d2[1]*Ra(1,1)+d1[2]*d2[2]*Ra(1,2)
                -d1[1]*d2[0]*Ra(2,0)-d1[1]*d2[1]*Ra(2,1)-d1[1]*d2[2]*Ra(2,2);
    A_tra(i,1) = d1[0]*d2[0]*Ra(2,0)+d1[0]*d2[1]*Ra(2,1)+d1[0]*d2[2]*Ra(2,2)
                -d1[2]*d2[0]*Ra(0,0)-d1[2]*d2[1]*Ra(0,1)-d1[2]*d2[2]*Ra(0,2);
    A_tra(i,2) = d1[1]*d2[0]*Ra(0,0)+d1[1]*d2[1]*Ra(0,1)+d1[1]*d2[2]*Ra(0,2)
                -d1[0]*d2[0]*Ra(1,0)-d1[0]*d2[1]*Ra(1,1)-d1[0]*d2[2]*Ra(1,2);

    A_trb(i,0) = d1[2]*d2[0]*Rb(1,0)+d1[2]*d2[1]*Rb(1,1)+d1[2]*d2[2]*Rb(1,2)
                -d1[1]*d2[0]*Rb(2,0)-d1[1]*d2[1]*Rb(2,1)-d1[1]*d2[2]*Rb(2,2);
    A_trb(i,1) = d1[0]*d2[0]*Rb(2,0)+d1[0]*d2[1]*Rb(2,1)+d1[0]*d2[2]*Rb(2,2)
                -d1[2]*d2[0]*Rb(0,0)-d1[2]*d2[1]*Rb(0,1)-d1[2]*d2[2]*Rb(0,2);
    A_trb(i,2) = d1[1]*d2[0]*Rb(0,0)+d1[1]*d2[1]*Rb(0,1)+d1[1]*d2[2]*Rb(0,2)
                -d1[0]*d2[0]*Rb(1,0)-d1[0]*d2[1]*Rb(1,1)-d1[0]*d2[2]*Rb(1,2);

    Eigen::Vector3d temp1 = v1.cross(d1);
    Eigen::Vector3d temp2 = v2.cross(d2);
    b_tra(i) = -d1.dot(Ra*temp2) -temp1.dot(Ra*d2);
    b_trb(i) = -d1.dot(Rb*temp2) -temp1.dot(Rb*d2);
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVDa(
      A_tra,
      Eigen::ComputeThinU | Eigen::ComputeThinV );

  Eigen::VectorXd sigma = SVDa.singularValues();
  double pinvtoler =
      numberCorrespondences*sigma(0)*NumTraits<double>::epsilon();
  Eigen::MatrixXd SigmaInverse(3,3);
  SigmaInverse = Eigen::MatrixXd::Zero(3,3);
  for ( size_t i=0; i < 3; ++i)
  {
    double temp = sigma(i);
    if( temp > pinvtoler )
      SigmaInverse(i,i) = 1.0/temp;
  }

  Eigen::MatrixXd PI(3,numberCorrespondences);
  PI = SVDa.matrixV()*SigmaInverse*SVDa.matrixU().transpose();
  Eigen::Vector3d ta = PI*b_tra;

  Eigen::JacobiSVD< Eigen::MatrixXd > SVDb(
      A_trb,
      Eigen::ComputeThinU | Eigen::ComputeThinV );

  sigma = SVDb.singularValues();
  pinvtoler = numberCorrespondences*sigma(0)*NumTraits<double>::epsilon();
  SigmaInverse = Eigen::MatrixXd::Zero(3,3);
  for ( size_t i=0; i < 3; ++i)
  {
    double temp = sigma(i);
    if( temp > pinvtoler )
      SigmaInverse(i,i) = 1.0/temp;
  }

  PI = SVDb.matrixV()*SigmaInverse*SVDb.matrixU().transpose();
  Eigen::Vector3d tb = PI*b_trb;

  Eigen::VectorXd fita = A_tra * ta - b_tra;
  Eigen::VectorXd fitb = A_trb * tb - b_trb;

  transformation_t transformation;
  if( fita.norm() < fitb.norm() )
  {
    transformation.block<3,3>(0,0) = Ra;
    transformation.col(3) = ta;
  }
  else
  {
    transformation.block<3,3>(0,0) = Rb;
    transformation.col(3) = tb;
  }

  return transformation;
}

}
}

opengv::transformation_t
opengv::relative_pose::seventeenpt( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return seventeenpt(adapter,idx);
}

opengv::transformation_t
opengv::relative_pose::seventeenpt(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return seventeenpt(adapter,idx);
}

namespace opengv
{
namespace relative_pose
{

struct OptimizeNonlinearFunctor1 : OptimizationFunctor<double>
{
  RelativeAdapterBase & _adapter;
  const Indices & _indices;

  OptimizeNonlinearFunctor1(
      RelativeAdapterBase & adapter,
      const Indices & indices ) :
      OptimizationFunctor<double>(6,indices.size()),
      _adapter(adapter),
      _indices(indices) {}

  int operator()(const VectorXd &x, VectorXd &fvec) const
  {
    assert( x.size() == 6 );
    assert( (unsigned int) fvec.size() == _indices.size());

    //compute the current position
    translation_t translation = x.block<3,1>(0,0);
    cayley_t cayley = x.block<3,1>(3,0);
    rotation_t rotation = math::cayley2rot(cayley);

    Eigen::Matrix<double,4,1> p_hom;
    p_hom[3] = 1.0;

    for( size_t i = 0; i < _indices.size(); i++ )
    {
      translation_t cam1Offset = _adapter.getCamOffset1(_indices[i]);
      rotation_t cam1Rotation = _adapter.getCamRotation1(_indices[i]);
      translation_t cam2Offset = _adapter.getCamOffset2(_indices[i]);
      rotation_t cam2Rotation = _adapter.getCamRotation2(_indices[i]);

      translation_t directTranslation =
          cam1Rotation.transpose() *
          ((translation - cam1Offset) + rotation * cam2Offset);
      rotation_t directRotation =
          cam1Rotation.transpose() * rotation * cam2Rotation;

      _adapter.sett12(directTranslation);
      _adapter.setR12(directRotation);

      transformation_t inverseSolution;
      inverseSolution.block<3,3>(0,0) = directRotation.transpose();
      inverseSolution.col(3) =
          -inverseSolution.block<3,3>(0,0)*directTranslation;

      p_hom.block<3,1>(0,0) =
          opengv::triangulation::triangulate2(_adapter,_indices[i]);
      bearingVector_t reprojection1 = p_hom.block<3,1>(0,0);
      bearingVector_t reprojection2 = inverseSolution * p_hom;
      reprojection1 = reprojection1 / reprojection1.norm();
      reprojection2 = reprojection2 / reprojection2.norm();
      bearingVector_t f1 = _adapter.getBearingVector1(_indices[i]);
      bearingVector_t f2 = _adapter.getBearingVector2(_indices[i]);

      //bearing-vector based outlier criterium (select threshold accordingly):
      //1-(f1'*f2) = 1-cos(alpha) \in [0:2]
      double reprojError1 = 1.0 - (f1.transpose() * reprojection1);
      double reprojError2 = 1.0 - (f2.transpose() * reprojection2);
      double factor = 1.0;
      fvec[i] = factor*(reprojError1 + reprojError2);
    }

    return 0;
  }
};

transformation_t optimize_nonlinear(
    RelativeAdapterBase & adapter,
    const Indices & indices )
{
  const int n=6;
  VectorXd x(n);

  x.block<3,1>(0,0) = adapter.gett12();
  x.block<3,1>(3,0) = math::rot2cayley(adapter.getR12());

  OptimizeNonlinearFunctor1 functor( adapter, indices );
  NumericalDiff<OptimizeNonlinearFunctor1> numDiff(functor);
  LevenbergMarquardt< NumericalDiff<OptimizeNonlinearFunctor1> >
      lm(numDiff);

  lm.resetParameters();
  lm.parameters.ftol = 1.E1*NumTraits<double>::epsilon();
  lm.parameters.xtol = 1.E1*NumTraits<double>::epsilon();
  lm.parameters.maxfev = 1000;
  lm.minimize(x);

  transformation_t transformation;
  transformation.col(3) = x.block<3,1>(0,0);
  transformation.block<3,3>(0,0) = math::cayley2rot(x.block<3,1>(3,0));
  return transformation;
}

}
}

opengv::transformation_t
opengv::relative_pose::optimize_nonlinear( RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return optimize_nonlinear(adapter,idx);
}

opengv::transformation_t
opengv::relative_pose::optimize_nonlinear(
    RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return optimize_nonlinear(adapter,idx);
}

Eigen::MatrixXd opengv::relative_pose::block_matrix(const RelativeAdapterBase& adapter, Eigen::MatrixXd &L1, Eigen::MatrixXd &L2,
						    int numberCorrespondences)
{
  //std::cout << "Enters block matrix function: " << std::endl;
  Eigen::MatrixXd M = Eigen::MatrixXd::Constant(36,36, 0.0);
  L1 = Eigen::MatrixXd::Constant(6, numberCorrespondences, 0.0);
  L2 = Eigen::MatrixXd::Constant(6, numberCorrespondences, 0.0);
  Eigen::Matrix<double, 6, 1> l1 = Eigen::MatrixXd::Constant(6,1, 0.0);
  Eigen::Matrix<double, 6, 1> l2 = Eigen::MatrixXd::Constant(6,1, 0.0);
  Eigen::MatrixXd M1 = Eigen::MatrixXd::Constant(6,6, 0.0);
  Eigen::MatrixXd M2 = Eigen::MatrixXd::Constant(6,6, 0.0);
  for( size_t i = 0; i < (unsigned) numberCorrespondences; i++ )
    {
      bearingVector_t d1 = adapter.getBearingVector1(i);
      bearingVector_t d2 = adapter.getBearingVector2(i);
      translation_t v1 = adapter.getCamOffset1(i);
      translation_t v2 = adapter.getCamOffset2(i);
      rotation_t R1 = adapter.getCamRotation1(i);
      rotation_t R2 = adapter.getCamRotation2(i);
      //unrotate the bearing-vectors to express everything in the body frame
      d1 = R1*d1;
      d2 = R2*d2;
      //generate the Plucker line coordinates
      l1.block<3,1>(0,0) = d1;
      l1.block<3,1>(3,0) = v1.cross(d1);
      l2.block<3,1>(0,0) = d2;
      l2.block<3,1>(3,0) = v2.cross(d2);
      L1.block<6,1>(0,i) = l1;
      L2.block<6,1>(0,i) = l2;
      M1 = l1 * l1.transpose();
      M2 = l2 * l2.transpose();
      M = M + Eigen::kroneckerProduct(M2,M1);
    }
  Eigen::MatrixXd A(36,18);
  A.block<36,3>(0,0) = M.block<36,3>(0,0);
  A.block<36,3>(0,3) = M.block<36,3>(0,6);
  A.block<36,3>(0,6) = M.block<36,3>(0,12);
  A.block<36,3>(0,9) = M.block<36,3>(0,3) + M.block<36,3>(0,18);
  A.block<36,3>(0,12) = M.block<36,3>(0,9) + M.block<36,3>(0,24);
  A.block<36,3>(0,15) = M.block<36,3>(0,15) + M.block<36,3>(0,30);
  Eigen::MatrixXd AE(A.block<36,9>(0,0));
  Eigen::MatrixXd AR(A.block<36,9>(0,9));
  Eigen::MatrixXd AE_inv_aux(AE.transpose() * AE);
  AE_inv_aux = AE_inv_aux.inverse() * AE.transpose() * (-1.0);
  Eigen::MatrixXd blockM(AE_inv_aux * AR);
  //std::cout << "\nMatrix at the end of block matrix function" << std::endl << blockM << std::endl;
  return blockM;

}

Eigen::Matrix3d opengv::relative_pose::exp_R( Eigen::Matrix3d & X )
{

  double phi = X.norm()/std::sqrt(2);
  Eigen::Matrix3d X1 = X/phi;
  Eigen::Matrix3d I = Eigen::Matrix< double, 3, 3 >::Identity();
  I = I + std::sin(phi)*X1 + ( 1 - std::cos(phi) )*X1*X1;
  return I;

}

double opengv::relative_pose::f_obj( Eigen::Matrix3d & essential_matrix, const Eigen::Matrix3d & R, const Eigen::MatrixXd & M,
				     const Eigen::MatrixXd & L1, const Eigen::MatrixXd & L2)
{
  Eigen::Matrix3d A11 = M.block<3,3>(0,0);Eigen::Matrix3d A12 = M.block<3,3>(0,3);Eigen::Matrix3d A13 = M.block<3,3>(0,6);
  Eigen::Matrix3d A21 = M.block<3,3>(3,0);Eigen::Matrix3d A22 = M.block<3,3>(3,3);Eigen::Matrix3d A23 = M.block<3,3>(3,6);
  Eigen::Matrix3d A31 = M.block<3,3>(6,0);Eigen::Matrix3d A32 = M.block<3,3>(6,3);Eigen::Matrix3d A33 = M.block<3,3>(6,6);

  Eigen::Matrix3d M11 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M11(0,0) = 1;
  Eigen::Matrix3d M12 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M12(1,0) = 1;
  Eigen::Matrix3d M13 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M13(2,0) = 1;
  Eigen::Matrix3d M21 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M21(0,1) = 1;
  Eigen::Matrix3d M22 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M22(1,1) = 1;
  Eigen::Matrix3d M23 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M23(2,1) = 1;
  Eigen::Matrix3d M31 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M31(0,2) = 1;
  Eigen::Matrix3d M32 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M32(1,2) = 1;
  Eigen::Matrix3d M33 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M33(2,2) = 1;

  essential_matrix = A11 * R * M11 + A12 * R * M12 + A13 * R * M13 + A21 * R * M21 + A22 * R * M22 + A23 * R * M23 + A31 * R * M31 + A32 * R * M32 + A33 * R * M33;

  int points = L1.cols();
  Vector3d d1(0.0, 0.0, 0.0);
  Vector3d m1(0.0, 0.0, 0.0);
  Vector3d d2(0.0, 0.0, 0.0);
  Vector3d m2(0.0, 0.0, 0.0);
  Eigen::Matrix<double, 1, 1> epsilon = Eigen::MatrixXd::Constant(1,1,0.0);
  double f = 0;
  for(int i = 0; i < points; ++i){
    d1 = L1.block<3,1>(0,i);
    m1 = L1.block<3,1>(3,i);
    d2 = L2.block<3,1>(0,i);
    m2 = L2.block<3,1>(3,i);

    epsilon = d1.transpose() * essential_matrix * d2 + d1.transpose() * R * m2 + m1.transpose() * R * d2;
    epsilon = epsilon * epsilon;
    f = f + epsilon(0,0);
  }
  return f;
}

Eigen::Matrix3d opengv::relative_pose::D_x(const Eigen::Matrix3d & R, const Eigen::MatrixXd & M, const Eigen::MatrixXd & L1, const Eigen::MatrixXd & L2)
{
  Eigen::Matrix3d A11 = M.block<3,3>(0,0);Eigen::Matrix3d A12 = M.block<3,3>(0,3);Eigen::Matrix3d A13 = M.block<3,3>(0,6);
  Eigen::Matrix3d A21 = M.block<3,3>(3,0);Eigen::Matrix3d A22 = M.block<3,3>(3,3);Eigen::Matrix3d A23 = M.block<3,3>(3,6);
  Eigen::Matrix3d A31 = M.block<3,3>(6,0);Eigen::Matrix3d A32 = M.block<3,3>(6,3);Eigen::Matrix3d A33 = M.block<3,3>(6,6);

  Eigen::Matrix3d M11 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M11(0,0) = 1;
  Eigen::Matrix3d M12 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M12(1,0) = 1;
  Eigen::Matrix3d M13 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M13(2,0) = 1;
  Eigen::Matrix3d M21 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M21(0,1) = 1;
  Eigen::Matrix3d M22 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M22(1,1) = 1;
  Eigen::Matrix3d M23 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M23(2,1) = 1;
  Eigen::Matrix3d M31 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M31(0,2) = 1;
  Eigen::Matrix3d M32 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M32(1,2) = 1;
  Eigen::Matrix3d M33 = Matrix<double, 3, 3 >::Constant(3,3,0.0);M33(2,2) = 1;

  int points = L1.cols();
  Vector3d d1(0.0, 0.0, 0.0);
  Vector3d m1(0.0, 0.0, 0.0);
  Vector3d d2(0.0, 0.0, 0.0);
  Vector3d m2(0.0, 0.0, 0.0);

  Eigen::Vector3d d1_11(0.0, 0.0, 0.0);
  Eigen::Vector3d d1_12(0.0, 0.0, 0.0);
  Eigen::Vector3d d1_13(0.0, 0.0, 0.0);
  Eigen::Vector3d d1_21(0.0, 0.0, 0.0);
  Eigen::Vector3d d1_22(0.0, 0.0, 0.0);
  Eigen::Vector3d d1_23(0.0, 0.0, 0.0);
  Eigen::Vector3d d1_31(0.0, 0.0, 0.0);
  Eigen::Vector3d d1_32(0.0, 0.0, 0.0);
  Eigen::Vector3d d1_33(0.0, 0.0, 0.0);

  Eigen::Vector3d d2_11(0.0, 0.0, 0.0);
  Eigen::Vector3d d2_12(0.0, 0.0, 0.0);
  Eigen::Vector3d d2_13(0.0, 0.0, 0.0);
  Eigen::Vector3d d2_21(0.0, 0.0, 0.0);
  Eigen::Vector3d d2_22(0.0, 0.0, 0.0);
  Eigen::Vector3d d2_23(0.0, 0.0, 0.0);
  Eigen::Vector3d d2_31(0.0, 0.0, 0.0);
  Eigen::Vector3d d2_32(0.0, 0.0, 0.0);
  Eigen::Vector3d d2_33(0.0, 0.0, 0.0);
  Eigen::Matrix3d devEpsilon = Eigen::Matrix3d::Constant(3,3,0.0);
  Eigen::Matrix<double, 1, 1> epsilon = Eigen::MatrixXd::Constant(1,1,0.0);
  Eigen::Matrix3d euclidean_gradient = Eigen::Matrix3d::Constant(3,3, 0.0);
  Eigen::Matrix3d essential_matrix = A11 * R * M11 + A12 * R * M12 + A13 * R * M13 + A21 * R * M21 + A22 * R * M22 + A23 * R * M23 + A31 * R * M31 + A32 * R * M32 + A33 * R * M33;
  for(int i = 0; i < points; ++i){
    d1 = L1.block<3,1>(0,i);
    m1 = L1.block<3,1>(3,i);
    d2 = L2.block<3,1>(0,i);
    m2 = L2.block<3,1>(3,i);

    d1_11 = A11.transpose() * d1; d1_12 = A12.transpose() * d1; d1_13 = A13.transpose() * d1;
    d1_21 = A21.transpose() * d1; d1_22 = A22.transpose() * d1; d1_23 = A23.transpose() * d1;
    d1_31 = A31.transpose() * d1; d1_32 = A32.transpose() * d1; d1_33 = A33.transpose() * d1;

    d2_11 = M11 * d2; d2_12 = M12 * d2; d2_13 = M13 * d2;
    d2_21 = M21 * d2; d2_22 = M22 * d2; d2_23 = M23 * d2;
    d2_31 = M31 * d2; d2_32 = M32 * d2; d2_33 = M33 * d2;

    devEpsilon = d1_11 * d2_11.transpose() + d1_12 * d2_12.transpose() + d1_13 * d2_13.transpose() + d1_21 * d2_21.transpose() + d1_22 * d2_22.transpose() + d1_23 * d2_23.transpose() + d1_31 * d2_31.transpose() + d1_32 * d2_32.transpose() + d1_33 * d2_33.transpose() + d1 * m2.transpose() + m1 * d2.transpose();
    epsilon = (d1.transpose() * essential_matrix * d2 ) + (d1.transpose() * R * m2) + (m1.transpose() * R * d2);
    euclidean_gradient = euclidean_gradient + epsilon(0,0) * devEpsilon;
  }
  euclidean_gradient = 2 * euclidean_gradient;
  return euclidean_gradient;
}

Eigen::MatrixXd opengv::relative_pose::egea(const RelativeAdapterBase & adapter, double & tol, Eigen::Matrix3d & initial_guess, int numberCorrespondences)
{

  Eigen::MatrixXd L1 = Eigen::MatrixXd::Constant(3,3,0.0);
  Eigen::MatrixXd L2 = Eigen::MatrixXd::Constant(3,3,0.0);
  Eigen::MatrixXd M = block_matrix(adapter, L1, L2, numberCorrespondences);
  Eigen::Matrix3d essential_matrix = Eigen::Matrix3d::Identity(3,3);
  //std::cout << "L1: " << std::endl << L1 << std::endl;
  //std::cout << "L2: " << std::endl << L2 << std::endl;
  // Initial Gess
  double g = 1.0;
  double erro = 1.0;
  int k = 0;

  // temp variables
  Eigen::MatrixXd GE = Eigen::MatrixXd::Constant(3, 4, 0.0);
  Eigen::Matrix3d Xf0, Xf1, DX, Z, P, Pt, Q, Qt, Y, S, R;
  double zz;
  Eigen::Matrix3d X = initial_guess;
  //std::cout << "\n\nInitial guess" << std::endl << X << std::endl;
  while( erro > tol && k < 1e5 )
    {
      Xf0 = X;
      DX  = D_x( X, M, L1, L2 );
      Z   = DX*X.transpose() - X*DX.transpose();
      zz  = 0.5*( Z*Z.transpose() ).trace();
      Pt  = -g*Z;
      P   = exp_R( Pt );
      Q   = P*P; // this seems strange
      Qt  = Q*X;

      while( ( f_obj( essential_matrix, X, M, L1, L2 ) - f_obj( essential_matrix, Qt, M, L1, L2 ) ) >= g*zz  )
	{

	  g   = 2*g;
	  P   = Q;
	  Q   = P*P; // this seems strange
	  Qt  = Q*X;

	}
      Qt = P*X;
      while( f_obj( essential_matrix, X, M, L1, L2) - f_obj(essential_matrix, Qt, M, L1, L2 ) < 0.5*g*zz)
	{

	  //   if ( f_obj( M, N, X, beta) - f_obj( M, N, Qt, beta ) < tol )
	  //     break;

	  g  = 0.5*g;
	  Pt = -g*Z;
	  P  = exp_R( Pt );
	  Qt = P*X;

	}
      double f = f_obj( essential_matrix, X, M, L1, L2);
      X    = P*X;
      Xf1  = X;
      erro = ( Xf1 - Xf0 ).norm();
      /*std::cout << "\nIteration: " << k << std::endl;
	std::cout << "Rotation: "    << std::endl << X << std::endl;
	std::cout << "Euclidean grad: " << std::endl << DX << std::endl;
	std::cout << "Function value: " << f << std::endl;
	std::cout << "Essential matrix: " << essential_matrix << std::endl;*/
      k++;
    }

  R=X;
  double fobj = f_obj(essential_matrix, R, M, L1, L2);

  Eigen::Matrix3d skew = essential_matrix * R.transpose();
  // double fobj = f_obj( M, N, X, beta );
  // std::cout << "objective function result: " << fobj << ", for the number of iterations: " << k << std::endl << "     and error: " << erro << std::endl;
  //std::cout << "Rotation matrix obtained: " << std::endl << R << std::endl;
  GE(0,0) = R(0,0); GE(0,1) = R(0,1); GE(0,2) = R(0,2);
  GE(1,0) = R(1,0); GE(1,1) = R(1,1); GE(1,2) = R(1,2);
  GE(2,0) = R(2,0); GE(2,1) = R(2,1); GE(2,2) = R(2,2);
  GE(0,3) = skew(2,1);
  GE(1,3) = skew(0,2);
  GE(2,3) = skew(1,0);

  return GE;

}


/*************************************************************************\
License
    Copyright (c) 2017 Kavvadias Ioannis.

    This file is part of SPHSimulator.

    Licensed under the MIT License. See LICENSE file in the project root for
    full license information.

\************************************************************************/

#include <iostream>
#include <glm/glm.hpp>
#include <glm/gtx/norm.hpp>
#include <numeric>

#include "SPHSolver.hpp"
#include "Settings.hpp"
#include "Kernels.hpp"
#include "Reorderer.hpp"
#include "Statistics/Statistics.hpp"

#include "Parallel/Parallel.hpp"

#include <boost/range/combine.hpp>
#include <boost/range/algorithm/for_each.hpp>

//********************************************************************************
void SPHSolver::init()
//********************************************************************************
{
  neibhs_.setHashTable(BoundaryConditions::bndBox.minPos(),BoundaryConditions::bndBox.delta());
  //neibhs_.writeGridRAW("neiGrid");

  cloud_.reserve(SPHSettings::NParticles);

  if (SPHSettings::SPHstep==SPHSettings::Solver::PCISPH)
  {
    //calculate delta
    glm::dvec2 cntParticle(0.0);
    glm::dvec2 offParticle(0.0);
    glm::dvec2 gradWij(0.0);
    double     dotGradWij(0.0);
    for (int i=-2;i<=2;i++)
    {
      for (int j=-2;j<=2;j++)
      {
        if (i==0&&j==0) continue;
        offParticle = glm::dvec2(i*SPHSettings::initDx,j*SPHSettings::initDx);
        glm::dvec2 rij = cntParticle-offParticle;
        double dist = glm::length(rij);
        glm::dvec2 tmpGradWij = Kernel::poly6::gradW(rij,dist)
          *Kernel::poly6::gradW_coeff();
        gradWij += tmpGradWij;
        dotGradWij += glm::dot(tmpGradWij,tmpGradWij);
      }
    }
    double beta = SPHSettings::particleMass * SimulationSettings::dt * SPHSettings::dParticleDensity;
    beta *= beta;
    beta *= 2.0;

    delta_ =  - 1./(beta*(-glm::dot(gradWij,gradWij)-dotGradWij));
  }

  if (InitialConditions::particleGeneration==InitialConditions::ALLIN)
  {
    //initialize field Particles
    unsigned nPart = 0;
    unsigned i = 0;
    while (nPart<SPHSettings::NParticles)
    {
      for (unsigned j=0;j<SPHSettings::LParticles;j++)
      {
        if (cloud_.size()>=SPHSettings::NParticles) break;
        Particle p;
        p.get<Attr::ePosition>() = glm::dvec2
          (
           InitialConditions::particleInitPos.x+(SPHSettings::initDx*(double(j)+0.5)),
           InitialConditions::particleInitPos.y+SPHSettings::initDx*(double(i)+0.5)
          );
        p.get<Attr::eVelocity>() = InitialConditions::particleInitVel;
        p.get<Attr::eDensErr>()  = SPHSettings::particleDensity;
        p.get<Attr::eDDensity>() = SPHSettings::dParticleDensity;
        p.get<Attr::eMass>()     = SPHSettings::particleMass;
        nPart++;
        cloud_.push_back(p);
      }
      i++;
    }
  }
  else
  {
    //particles will be generated in generateParticles()
  }

  neibhs_.clear();
  neibhs_.findNei(cloud_);
  updateNei();
  calcDensity();
}

//********************************************************************************
void SPHSolver::WCSPHStep()
//********************************************************************************
{
  calcDensity();
  calcPressure();
  calcNormal();

  //calc forces
  calcPressForces();
  calcViscForces();
  calcSurfForces();
  calcOtherForces();

  //combine forces and update values
  {
    static auto updatePosTimerID  = Statistics::createTimer("SPHSolver::WCSPH::updatePosTimer");
    Statistics::TimerGuard g(updatePosTimerID);

    auto& particlePos = cloud_.get<Attr::ePosition>();
    auto& particleVel = cloud_.get<Attr::eVelocity>();
    const auto& particleFPress = cloud_.get<Attr::ePressForce>();
    const auto& particleFVisc  = cloud_.get<Attr::eViscForce >();
    const auto& particleFSurf  = cloud_.get<Attr::eSurfForce >();
    const auto& particleFOther = cloud_.get<Attr::eOtherForce>();
    const auto& particleDDens  = cloud_.get<Attr::eDDensity  >();

    auto& particleFTotal = cloud_.get<Attr::eTotalForce>();

    Parallel::For (cloud_.size(), [ &particlePos,   &particleVel,    &particleFPress, &particleFVisc,
                                    &particleFSurf, &particleFOther, &particleDDens,  &particleFTotal
                                  ] (size_t iPart)
    {
      particleFTotal[iPart] = particleFPress[iPart] + particleFVisc [iPart]
                            + particleFSurf [iPart] + particleFOther[iPart];

      particleVel[iPart] += SimulationSettings::dt*particleDDens[iPart]*particleFTotal[iPart];
      particlePos[iPart] += SimulationSettings::dt*particleVel[iPart];
    });
  }
}

//********************************************************************************
void SPHSolver::PCISPHStep()
//********************************************************************************
{
  auto& particlePos = cloud_.get<Attr::ePosition>();
  auto& particleVel = cloud_.get<Attr::eVelocity>();
  auto& particleFTot = cloud_.get<Attr::eTotalForce>();
  const auto& particleFPress  = cloud_.get<Attr::ePressForce>();
  const auto& particleDDens   = cloud_.get<Attr::eDDensity>();
  const auto& particleDensErr = cloud_.get<Attr::eDensErr>();

  calcDensity();
  calcNormal();

  //calc forces
  calcViscForces();
  calcSurfForces();
  calcOtherForces();

  const size_t nPart = cloud_.size();
  {
    static auto updatePosTimerID  = Statistics::createTimer("SPHSolver::PCISPH::updatePosTimer");
    Statistics::TimerGuard g(updatePosTimerID);

    const auto& particleFVisc  = cloud_.get<Attr::eViscForce >();
    const auto& particleFSurf  = cloud_.get<Attr::eSurfForce >();
    const auto& particleFOther = cloud_.get<Attr::eOtherForce>();

    auto& particleFTotal = cloud_.get<Attr::eTotalForce>();

    Parallel::For (nPart, [ &particleFTotal, &particleVel,    &particlePos, &particleFVisc,
                            &particleFSurf,  &particleFOther, &particleDDens
                          ](size_t iPart){
      particleFTotal[iPart] = particleFVisc [iPart]
                            + particleFSurf [iPart]
                            + particleFOther[iPart];

      particleVel[iPart] += SimulationSettings::dt*particleDDens[iPart]*particleFTotal[iPart];
      particlePos[iPart] += SimulationSettings::dt*particleVel[iPart];
    });
  }

  double densErr=SPHSettings::particleDensity;
  int iter=0;

  {
    static auto pressForceTimerID = Statistics::createTimer("SPHSolver::PCISPH::pressForceTimer");
    Statistics::TimerGuard g(pressForceTimerID);

    //calculate pressure and pressure force
    initPressure();
    const double targetDensErr = SPHSettings::densityErr*SPHSettings::particleDensity;
    while(densErr>targetDensErr && iter<500)
    {
      updateNei();
      calcDensity();
      calcDensityErr();
      updatePressure();

      auto itMax = std::max_element(particleDensErr.begin(), particleDensErr.end());
      densErr = (itMax != particleDensErr.end()) ? (*itMax) : 0.;

      if (iter==0)
      iter++;

      calcPressForces();

      const double dt = SimulationSettings::dt;

      Parallel::For (nPart, [ &particleFTot, &particleFPress, &particleVel,
                              &particlePos,  &particleDDens, dt
                            ] (size_t iPart){
        const glm::dvec2 iPress = particleFPress[iPart];
        particleFTot[iPart] += iPress;

        glm::dvec2 update = dt*particleDDens[iPart]*iPress;
        particleVel[iPart] += update;
        particlePos[iPart] += dt*update;
      });
    }
  }
}

//********************************************************************************
bool SPHSolver::step()
//********************************************************************************
{
  static int iReorder = 0;
  iReorder++;

  auto& particleNei = cloud_.get<Attr::eNei>();
  boost::for_each(particleNei, [](auto& v){ v.clear();});

  if(iReorder == 100)
  {
    Reorderer::reorderCloud(cloud_);
    iReorder = 0;
  }

  generateParticles();

  neibhs_.clear();

  neibhs_.findNei(cloud_);

  updateNei();

  {
    static auto stepTimerID      = Statistics::createTimer("SPHSolver::Step::SPHsolver");
    Statistics::TimerGuard g(stepTimerID);

    switch (SPHSettings::SPHstep)
    {
      case (SPHSettings::Solver::WCSPH):
        {
          WCSPHStep();
          break;
        }
      case (SPHSettings::Solver::PCISPH):
        {
          PCISPHStep();
          break;
        }
      default:
        {
          std::cerr<<"Invalid SPHSettings::Solver"<<std::endl;
          exit(1);
        }
    }
    SimulationSettings::updateSimTime();
  }

  return (!SimulationSettings::breakLoop());
}

//********************************************************************************
void SPHSolver::generateParticles()
//********************************************************************************
{
  static double genTime = 0;
  static double dtGenFaucet = 0.51*Kernel::SmoothingLength::h
                   /(glm::length(InitialConditions::particleInitVel)+1.e-8);

  if (cloud_.size()>=SPHSettings::NParticles) return;

  switch(InitialConditions::particleGeneration)
  {
    case (InitialConditions::FAUCET):
    {
      if (SimulationSettings::simTime-genTime>=dtGenFaucet)
      {
        glm::dvec2 initVelNormalized = glm::normalize(InitialConditions::particleInitVel);
        glm::dvec2 generationDirection = glm::dvec2(-initVelNormalized.y,initVelNormalized.x);
        //std::cout<<"generating"<<std::endl;
        if (cloud_.size()<SPHSettings::NParticles)
        {
          for (unsigned i=0;i<SPHSettings::LParticles;i++)
          {
            if (cloud_.size()>=SPHSettings::NParticles) break;
            Particle active;
            active.get<Attr::ePosition>() = InitialConditions::particleInitPos + i*SPHSettings::initDx*generationDirection;
            active.get<Attr::eVelocity>() = InitialConditions::particleInitVel;
            active.get<Attr::eMass>()     = SPHSettings::particleMass;
            cloud_.push_back(active);
          }
        }
        genTime = SimulationSettings::simTime;
      }
      break;
    }
    case (InitialConditions::DRIPPING):
    {
      if (SimulationSettings::simTime-genTime>=InitialConditions::particleGenTime)
      {
        //std::cout<<"generating"<<std::endl;
        if (cloud_.size()<SPHSettings::NParticles)
        {
          //double     radius = 0.3*double(SPHSettings::LParticles)*SPHSettings::initDx;
          double     radius = 0.4*double(SPHSettings::LParticles)*SPHSettings::initDx;
          double     offset = 0.5*SPHSettings::initDx*double(SPHSettings::LParticles-1);
          glm::dvec2 center = InitialConditions::particleInitPos + glm::dvec2(offset,offset);

          for (unsigned i=0;i<SPHSettings::LParticles;i++)
          {
            for (unsigned j=0;j<SPHSettings::LParticles;j++)
            {
              if (cloud_.size()>=SPHSettings::NParticles) break;
              glm::dvec2 pos(InitialConditions::particleInitPos + glm::dvec2(SPHSettings::initDx*i,SPHSettings::initDx*j));
              double dist = glm::length(pos-center);
              if (dist>radius) continue;
              Particle active;
              active.get<Attr::ePosition>() = pos;
              active.get<Attr::eVelocity>() = InitialConditions::particleInitVel;
              active.get<Attr::eMass    >() = SPHSettings::particleMass;
              cloud_.push_back(active);
            }
          }
        }
        genTime = SimulationSettings::simTime;
      }
      break;
    }
    case (InitialConditions::ALLIN):
    {
      std::cerr<<"SPHSolver::generateParticles::ALLIN - Should be in here!!"<<std::endl;
      exit(1);
    }
    default: {}
  }
}

//********************************************************************************
void SPHSolver::updateNei()
//********************************************************************************
{
  static auto updateNeiTimerID = Statistics::createTimer("SPHSolver::updateNeiTime");
  Statistics::TimerGuard updateNeiTimerGuard(updateNeiTimerID);

  const auto& particlePos = cloud_.get<Attr::ePosition>();
  auto& particleNei = cloud_.get<Attr::eNei>();

  Parallel::For (cloud_.size(), [ &particlePos, &particleNei ] (size_t iPart) {
    const glm::dvec2 iPos = particlePos[iPart];
    for ( Neigbhor& iPartNeiI :  particleNei[iPart] )
    {
        const glm::dvec2 dir = iPos-particlePos[iPartNeiI.ID];
        iPartNeiI.dir  = dir;
        iPartNeiI.dist = glm::length(dir);
    }
  });
}

//********************************************************************************
void SPHSolver::calcDensity()
//********************************************************************************
{
  static auto densCalcTimerID   = Statistics::createTimer("SPHSolver::densCalcTimer");
  Statistics::TimerGuard densGuard(densCalcTimerID);

  const auto& particleMass = cloud_.get<Attr::eMass>();
  const auto& particleNei  = cloud_.get<Attr::eNei>();

  auto& particleDens  = cloud_.get<Attr::eDensity>();
  auto& particleDDens = cloud_.get<Attr::eDDensity>();

  const double Wcoeff = Kernel::poly6::W_coeff();

  Parallel::For (cloud_.size(), [ &particleMass, &particleNei,
                                  &particleDens, &particleDDens, Wcoeff
                                ] (size_t iPart) {
    const auto& iNei = particleNei[iPart];

    double dens = 0.0;

    for (const Neigbhor& nei : iNei)
    {
        unsigned jPart = nei.ID;

        dens += particleMass[jPart]*Kernel::poly6::W(nei.dist);
    }
    dens*=Wcoeff;

    particleDens[iPart]  = dens;
    particleDDens[iPart] = 1./dens;
  });
}

//********************************************************************************
void SPHSolver::calcDensityErr()
//********************************************************************************
{
  static auto densErrCalcTimerID   = Statistics::createTimer("SPHSolver::densErrCalcTimer");
  Statistics::TimerGuard densErrGuard(densErrCalcTimerID);

  const auto& particleDens = cloud_.get<Attr::eDensity>();
  auto& particleDensErr = cloud_.get<Attr::eDensErr>();

  const double pDens = SPHSettings::particleDensity;

  Parallel::For (cloud_.size(), [ &particleDensErr, &particleDens, pDens ] (size_t iPart) {
    particleDensErr[iPart] = particleDens[iPart] - pDens;
  });
}

//********************************************************************************
void SPHSolver::initPressure()
//********************************************************************************
{
  auto& particlePress = cloud_.get<Attr::ePressure>();
  Parallel::For (cloud_.size(), [ &particlePress ] (size_t iPart) {
    particlePress[iPart]=0.0;
  });
}

//********************************************************************************
void SPHSolver::calcPressure()
//********************************************************************************
{
  static auto pressCalcTimerID   = Statistics::createTimer("SPHSolver::pressureCalcTimer");
  Statistics::TimerGuard pressGuard(pressCalcTimerID);

  const auto& particleDens = cloud_.get<Attr::eDensity>();
  auto& particlePress = cloud_.get<Attr::ePressure>();

  const double stiff = SPHSettings::stiffness;
  const double pDens = SPHSettings::particleDensity;

  Parallel::For (cloud_.size(), [ &particlePress, &particleDens, stiff, pDens ] (size_t iPart) {
    //p = k(rho-rho0)
    particlePress[iPart] = fmax(stiff*(particleDens[iPart] - pDens), 0.0);
  });
}

//********************************************************************************
void SPHSolver::calcNormal()
//********************************************************************************
{
  static auto normalCalcTimerID   = Statistics::createTimer("SPHSolver::normalCalcTimer");
  Statistics::TimerGuard normalGuard(normalCalcTimerID);

  const auto& particleMass  = cloud_.get<Attr::eMass>();
  const auto& particleDDens = cloud_.get<Attr::eDDensity>();
  const auto& particleNei  = cloud_.get<Attr::eNei>();
  auto& particleNormal = cloud_.get<Attr::eNormal>();

  Parallel::For (cloud_.size(), [ &particleMass, &particleDDens, &particleNei, &particleNormal] (size_t iPart) {
    const auto& iNei = particleNei[iPart];

    glm::dvec2 norm (0.0);

    for (const Neigbhor& nei : iNei)
    {
        unsigned jPart = nei.ID;

        norm += particleMass[jPart]*particleDDens[jPart]
              *Kernel::poly6::gradW(nei.dir, nei.dist);
    }
    norm *= Kernel::poly6::gradW_coeff()*Kernel::SmoothingLength::h;
    particleNormal[iPart] = norm;
  });
}

//********************************************************************************
void SPHSolver::updatePressure()
//********************************************************************************
{
  static auto updatePressTimerID   = Statistics::createTimer("SPHSolver::updatePressure");
  Statistics::TimerGuard updatePressGuard(updatePressTimerID);

  const auto& particleDensErr = cloud_.get<Attr::eDensErr>();
  auto& particlePress = cloud_.get<Attr::ePressure>();

  const double delta = delta_;

  Parallel::For (cloud_.size(), [ &particlePress, &particleDensErr, delta] (size_t iPart) {
    particlePress[iPart] = fmax(delta*particleDensErr[iPart],0.0);
    //particlePress[iPart] = delta*particleDensErr[iPart];
  });
}

//********************************************************************************
void SPHSolver::calcOtherForces()
//********************************************************************************
{
  static auto otherForceTimerID = Statistics::createTimer("SPHSolver::otherForceTimer");
  Statistics::TimerGuard otherForceGuard(otherForceTimerID);

  const auto& particlePos = cloud_.get<Attr::ePosition>();
  const auto& particleDens = cloud_.get<Attr::eDensity>();
  auto& particleVel = cloud_.get<Attr::eVelocity>();
  auto& particleFOther = cloud_.get<Attr::eOtherForce>();

  const glm::dvec2 grav = SPHSettings::grav;

  Parallel::For (cloud_.size(), [ &particlePos, &particleDens, &particleVel, &particleFOther, grav] (size_t iPart) {
    //gravity
    particleFOther[iPart] = grav;

    //boundary forces
    glm::dvec2 iPos = particlePos[iPart];
    glm::dvec2 bndPos(0.0,0.0);
    glm::dvec2 tmpiPos(0.0,0.0);
    double W0 = Kernel::poly6::W(0.0)*Kernel::poly6::W_coeff();
    double mult = 2.0;
    double scaledh  = mult*Kernel::SmoothingLength::h;
    double dScaledh = 1./scaledh;
    if (iPos.x-BoundaryConditions::bndBox.minX()<scaledh)
    {
      if (iPos.x>BoundaryConditions::bndBox.minX())
      {
        tmpiPos = glm::dvec2(iPos.x,0.0)*dScaledh;
        bndPos  = glm::dvec2(BoundaryConditions::bndBox.minX(),0.0)*dScaledh;
        double dist = glm::length(tmpiPos-bndPos);
        particleFOther[iPart].x+=BoundaryConditions::bndCoeff
                                *Kernel::poly6::W(dist)
                                *Kernel::poly6::W_coeff();
      }
      else
      {
        particleVel[iPart].x=0.;
        particleFOther[iPart].x+=BoundaryConditions::bndCoeff*W0;
      }
    }
    else if (BoundaryConditions::bndBox.maxX()-iPos.x<scaledh)
    {
      if (iPos.x<BoundaryConditions::bndBox.maxX())
      {
        tmpiPos = glm::dvec2(iPos.x,0.0)*dScaledh;
        bndPos  = glm::dvec2(BoundaryConditions::bndBox.maxX(),0.0)*dScaledh;
        double dist = glm::length(tmpiPos-bndPos);
        particleFOther[iPart].x-=BoundaryConditions::bndCoeff
                                *Kernel::poly6::W(dist)
                                *Kernel::poly6::W_coeff();
      }
      else
      {
        particleVel[iPart].x=0.;
        particleFOther[iPart].x-=BoundaryConditions::bndCoeff*W0;
      }
    }
    if (iPos.y-BoundaryConditions::bndBox.minY()<scaledh)
    {
      if (iPos.y>BoundaryConditions::bndBox.minY())
      {
        tmpiPos = glm::dvec2(0.0,iPos.y)*dScaledh;
        bndPos  = glm::dvec2(0.0,BoundaryConditions::bndBox.minY())*dScaledh;
        double dist = glm::length(tmpiPos-bndPos);
        particleFOther[iPart].y+=BoundaryConditions::bndCoeff
                               *Kernel::poly6::W(dist)
                               *Kernel::poly6::W_coeff();
      }
      else
      {
        particleVel[iPart].y=0.;
        particleFOther[iPart].y+=BoundaryConditions::bndCoeff*W0;
      }
    }
    else if (BoundaryConditions::bndBox.maxY()-iPos.y<scaledh)
    {
      if (iPos.y>BoundaryConditions::bndBox.maxY())
      {
        tmpiPos = glm::dvec2(0.0,iPos.y)*dScaledh;
        bndPos  = glm::dvec2(0.0,BoundaryConditions::bndBox.maxY())*dScaledh;
        double dist = glm::length(tmpiPos-bndPos);
        particleFOther[iPart].y-=BoundaryConditions::bndCoeff
                                *Kernel::poly6::W(dist)
                                *Kernel::poly6::W_coeff();
      }
      else
      {
        particleVel[iPart].y=0.;
        particleFOther[iPart].y-=BoundaryConditions::bndCoeff*W0;
      }
    }

    particleFOther[iPart] *= particleDens[iPart];
  });
}

//********************************************************************************
void SPHSolver::calcPressForces()
//********************************************************************************
{
  static auto pressForceTimerID = Statistics::createTimer("SPHSolver::pressForceTimer");
  Statistics::TimerGuard pressForceGuard(pressForceTimerID);

  const auto& particleMass  = cloud_.get<Attr::eMass>();
  const auto& particleDDens = cloud_.get<Attr::eDDensity>();
  const auto& particlePress = cloud_.get<Attr::ePressure>();
  const auto& particleNei   = cloud_.get<Attr::eNei>();
  auto& particleFPress = cloud_.get<Attr::ePressForce>();

  Parallel::For (cloud_.size(), [ &particleMass,  &particleDDens, &particlePress,
                                  &particleNei, &particleFPress
                                ] (size_t iPart) {
    const auto& iNei = particleNei[iPart];

    glm::dvec2 Fp = glm::dvec2(0.0);
    const double iPress = particlePress[iPart];

    for (const Neigbhor& nei : iNei)
    {
      unsigned jPart = nei.ID;
      if (iPart==jPart) continue;

      Fp+=particleMass[jPart]*particleDDens[jPart]
         *(iPress + particlePress[jPart])
         *Kernel::spiky::gradW(nei.dir,nei.dist);
    }

    Fp*=-0.5*Kernel::spiky::gradW_coeff();

    particleFPress[iPart] = Fp;
  });
}

//********************************************************************************
void SPHSolver::calcViscForces()
//********************************************************************************
{
  static auto viscForceTimerID  = Statistics::createTimer("SPHSolver::viscForceTimer");
  Statistics::TimerGuard viscForceGuard(viscForceTimerID);

  const auto& particleVel   = cloud_.get<Attr::eVelocity>();
  const auto& particleMass  = cloud_.get<Attr::eMass>();
  const auto& particleDDens = cloud_.get<Attr::eDDensity>();
  const auto& particleNei   = cloud_.get<Attr::eNei>();
  auto& particleFVisc = cloud_.get<Attr::eViscForce>();

  Parallel::For (cloud_.size(), [ &particleVel, &particleMass, &particleDDens,
                                  &particleNei, &particleFVisc
                                ] (size_t iPart) {
    glm::dvec2 Fv = glm::dvec2(0.0);

    glm::dvec2 iVel = particleVel[iPart];
    const auto& iNei = particleNei[iPart];

    for (const Neigbhor& nei : iNei)
    {
      unsigned jPart = nei.ID;
      if (iPart==jPart) continue;

      Fv+= particleMass[jPart]
         *particleDDens[jPart]
         *(particleVel[jPart]-iVel)
         *Kernel::visc::laplW(nei.dist);
    }
    Fv*=SPHSettings::viscosity*Kernel::visc::laplW_coeff();

    particleFVisc[iPart] = Fv;
  });
}

//********************************************************************************
void SPHSolver::calcSurfForces()
//********************************************************************************
{
  static auto surfForceTimerID  = Statistics::createTimer("SPHSolver::surfForceTimer");
  Statistics::TimerGuard surfForceGuard(surfForceTimerID);

  const auto& particleNormal = cloud_.get<Attr::eNormal>();
  const auto& particleMass = cloud_.get<Attr::eMass>();
  const auto& particleDens = cloud_.get<Attr::eDensity>();
  const auto& particleNei  = cloud_.get<Attr::eNei>();
  auto& particleFSurf = cloud_.get<Attr::eSurfForce>();

  Parallel::For (cloud_.size(), [ &particleNormal, &particleMass, &particleDens,
                                  &particleNei, &particleFSurf
                                ] (size_t iPart) {
    glm::dvec2 Fcohesion  = glm::dvec2(0.0);
    glm::dvec2 Fcurvature = glm::dvec2(0.0);
    double correction = 0.0;

    const glm::dvec2 iNorm = particleNormal[iPart];
    const double iMass = particleMass[iPart];
    const double iDens = particleDens[iPart];
    const auto&  iNei  = particleNei[iPart];

    for (const Neigbhor& nei : iNei)
    {
      const unsigned jPart = nei.ID;
      //std::cout<<" "<<jPart;
      if (iPart==jPart) continue;

      correction = 2.*SPHSettings::particleDensity/(iDens + particleDens[jPart]);

      Fcohesion+= correction
             *iMass*particleMass[jPart]
             *Kernel::surface::C(nei.dist)
             *nei.dir/nei.dist;

      Fcurvature+= correction
             *iMass
             *(iNorm - particleNormal[jPart]);
    }

    Fcohesion *= Kernel::surface::C_coeff();

    particleFSurf[iPart] = -SPHSettings::surfTension
                         * iDens
                         * (Fcohesion+Fcurvature);
  });
}

//********************************************************************************
double SPHSolver::calcCFL() const
//********************************************************************************
{
  static auto clfTimerID  = Statistics::createTimer("SPHSolver::calcCFL");
  Statistics::TimerGuard clfGuard(clfTimerID);

  const auto& particleVel = cloud_.get<Attr::eVelocity>();

  double vMax = 0.0;

  for (size_t iPart = 0, nPart = cloud_.size(); iPart<nPart; ++iPart)
  {
    double vLen2 = glm::length2(particleVel[iPart]);
    if (vLen2 > vMax) vMax = vLen2;
  }

  double maxDt = 0.4*Kernel::SmoothingLength::h/(sqrt(vMax)+1.e-5);
  double CFL = SimulationSettings::dt/maxDt;
  return CFL;
}

//********************************************************************************
double SPHSolver::calcKineticEnergy() const
//********************************************************************************
{
  static auto kineticTimerID  = Statistics::createTimer("SPHSolver::calcKineticEnergy");
  Statistics::TimerGuard kinetickGuard(kineticTimerID);

  const auto& particleVel  = cloud_.get<Attr::eVelocity>();
  const auto& particleMass = cloud_.get<Attr::eMass>();
  auto accumRange = boost::combine(particleMass, particleVel);

  double kEnergy = 0.5*std::accumulate(accumRange.begin(), accumRange.end(), 0.,
    [](double k, const auto& elem) {
      const double mass = boost::get<0>(elem);
      const glm::dvec2& vel = boost::get<1>(elem);
      return k + mass*glm::dot(vel, vel);
    }
  );

  return kEnergy;
}


#include "drake/examples/kuka_iiwa_arm/kuka_torque_controller.h"

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/examples/kuka_iiwa_arm/iiwa_common.h"
#include "drake/multibody/parsing/parser.h"

namespace drake {
namespace examples {
namespace kuka_iiwa_arm {

using drake::systems::BasicVector;

namespace {
Eigen::VectorXd CalcGravityCompensationTorque(
    const multibody::MultibodyPlant<double>& plant,
    const Eigen::VectorXd& q,
    Eigen::MatrixXd* H = nullptr) {

  // Compute gravity compensation torque.
  std::unique_ptr<systems::Context<double>> plant_context =
      plant.CreateDefaultContext();
  Eigen::VectorXd zero_velocity = Eigen::VectorXd::Zero(kIiwaArmNumJoints);
  plant.SetPositions(plant_context.get(), q);
  plant.SetVelocities(plant_context.get(), zero_velocity);

  if (H) {
    plant.CalcMassMatrixViaInverseDynamics(*plant_context, H);
  }

  multibody::MultibodyForces<double> external_forces(plant);
  plant.CalcForceElementsContribution(*plant_context, &external_forces);
  return plant.CalcInverseDynamics(*plant_context, zero_velocity,
                                   external_forces);
}

}  // namespace

GTEST_TEST(KukaTorqueControllerTest, GravityCompensationTest) {
  const std::string kIiwaUrdf =
    "manipulation/models/iiwa_description/urdf/"
    "iiwa14_polytope_collision.urdf";
  multibody::MultibodyPlant<double> plant(0.0);
  multibody::Parser(&plant).AddModelFromFile(kIiwaUrdf);
  plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"));
  plant.Finalize();

  // Set stiffness and damping to zero.
  VectorX<double> stiffness(kIiwaArmNumJoints);
  stiffness.setZero();
  VectorX<double> damping(kIiwaArmNumJoints);
  damping.setZero();

  // Choose an arbitrary state.
  VectorX<double> q(7);
  q << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
  VectorX<double> v(7);
  v << 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1;
  VectorX<double> q_des(7);
  q_des << -0.1, -0.2, -0.3, -0.4, -0.5, -0.6, -0.7;
  VectorX<double> v_des(7);
  v_des << -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1;
  VectorX<double> torque_des(kIiwaArmNumJoints);
  torque_des.setZero();

  // Compute controller output.
  KukaTorqueController<double> controller(plant, stiffness, damping);

  std::unique_ptr<systems::Context<double>> context =
      controller.CreateDefaultContext();
  std::unique_ptr<systems::SystemOutput<double>> output =
      controller.AllocateOutput();

  VectorX<double> estimated_state_input(2 * kIiwaArmNumJoints);
  estimated_state_input << q, v;

  VectorX<double> desired_state_input(2 * kIiwaArmNumJoints);
  desired_state_input << q_des, v_des;

  VectorX<double> desired_torque_input(kIiwaArmNumJoints);
  desired_torque_input << torque_des;

  controller.get_input_port_estimated_state().FixValue(context.get(),
                                                       estimated_state_input);
  controller.get_input_port_desired_state().FixValue(context.get(),
                                                     desired_state_input);
  controller.get_input_port_commanded_torque().FixValue(context.get(),
                                                        desired_torque_input);

  Eigen::VectorXd expected_torque =
      CalcGravityCompensationTorque(plant, q);

  // Check output.
  controller.CalcOutput(*context, output.get());
  const BasicVector<double>* output_vector = output->get_vector_data(0);
  EXPECT_TRUE(CompareMatrices(expected_torque, output_vector->get_value(),
                              1e-10, MatrixCompareType::absolute));
}

GTEST_TEST(KukaTorqueControllerTest, SpringTorqueTest) {
  const std::string kIiwaUrdf =
    "manipulation/models/iiwa_description/urdf/"
    "iiwa14_polytope_collision.urdf";
  multibody::MultibodyPlant<double> plant(0.0);
  multibody::Parser(&plant).AddModelFromFile(kIiwaUrdf);
  plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"));
  plant.Finalize();

  // Set nonzero stiffness and zero damping.
  VectorX<double> stiffness(kIiwaArmNumJoints);
  stiffness << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
  VectorX<double> damping(kIiwaArmNumJoints);
  damping.setZero();

  // Choose an arbitrary state
  VectorX<double> q(7);
  q << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
  VectorX<double> v(7);
  v << 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1;
  VectorX<double> q_des(7);
  q_des << -0.1, -0.2, -0.3, -0.4, -0.5, -0.6, -0.7;
  VectorX<double> v_des(7);
  v_des << -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1;
  VectorX<double> torque_des(kIiwaArmNumJoints);
  torque_des.setZero();

  // Compute controller output.
  KukaTorqueController<double> controller(plant, stiffness, damping);

  std::unique_ptr<systems::Context<double>> context =
      controller.CreateDefaultContext();
  std::unique_ptr<systems::SystemOutput<double>> output =
      controller.AllocateOutput();

  VectorX<double> estimated_state_input(2 * kIiwaArmNumJoints);
  estimated_state_input << q, v;

  VectorX<double> desired_state_input(2 * kIiwaArmNumJoints);
  desired_state_input << q_des, v_des;

  VectorX<double> desired_torque_input(kIiwaArmNumJoints);
  desired_torque_input << torque_des;

  controller.get_input_port_estimated_state().FixValue(context.get(),
                                                       estimated_state_input);
  controller.get_input_port_desired_state().FixValue(context.get(),
                                                     desired_state_input);
  controller.get_input_port_commanded_torque().FixValue(context.get(),
                                                        desired_torque_input);

  // Compute gravity compensation torque.
  Eigen::VectorXd expected_torque =
      CalcGravityCompensationTorque(plant, q);

  // Compute spring torque.
  expected_torque += -((q - q_des).array() * stiffness.array()).matrix();

  // Check output.
  controller.CalcOutput(*context, output.get());
  const BasicVector<double>* output_vector = output->get_vector_data(0);
  EXPECT_TRUE(CompareMatrices(expected_torque, output_vector->get_value(),
                              1e-10, MatrixCompareType::absolute));
}

GTEST_TEST(KukaTorqueControllerTest, DampingTorqueTest) {
  const std::string kIiwaUrdf =
    "manipulation/models/iiwa_description/urdf/"
    "iiwa14_polytope_collision.urdf";
  multibody::MultibodyPlant<double> plant(0.0);
  multibody::Parser(&plant).AddModelFromFile(kIiwaUrdf);
  plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"));
  plant.Finalize();

  // Set arbitrary stiffness and damping.
  VectorX<double> stiffness(kIiwaArmNumJoints);
  stiffness << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
  VectorX<double> damping(kIiwaArmNumJoints);
  damping << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;

  // Choose an arbitrary state
  VectorX<double> q(7);
  q << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
  VectorX<double> v(7);
  v << 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1;
  VectorX<double> q_des(7);
  q_des << -0.1, -0.2, -0.3, -0.4, -0.5, -0.6, -0.7;
  VectorX<double> v_des(7);
  v_des << -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1;
  VectorX<double> torque_des(kIiwaArmNumJoints);
  torque_des.setZero();

  // Compute controller output.
  KukaTorqueController<double> controller(plant, stiffness, damping);

  std::unique_ptr<systems::Context<double>> context =
      controller.CreateDefaultContext();
  std::unique_ptr<systems::SystemOutput<double>> output =
      controller.AllocateOutput();

  VectorX<double> estimated_state_input(2 * kIiwaArmNumJoints);
  estimated_state_input << q, v;

  VectorX<double> desired_state_input(2 * kIiwaArmNumJoints);
  desired_state_input << q_des, v_des;

  VectorX<double> desired_torque_input(kIiwaArmNumJoints);
  desired_torque_input << torque_des;

  controller.get_input_port_estimated_state().FixValue(context.get(),
                                                       estimated_state_input);
  controller.get_input_port_desired_state().FixValue(context.get(),
                                                     desired_state_input);
  controller.get_input_port_commanded_torque().FixValue(context.get(),
                                                        desired_torque_input);

  // Compute gravity compensation torque and mass matrix.
  Eigen::MatrixXd H(kIiwaArmNumJoints, kIiwaArmNumJoints);
  Eigen::VectorXd expected_torque =
      CalcGravityCompensationTorque(plant, q, &H);

  // Compute spring torque.
  expected_torque += -((q - q_des).array() * stiffness.array()).matrix();

  // Compute damping torque.
  Eigen::VectorXd damping_torque(7);
  for (int i = 0; i < kIiwaArmNumJoints; i++) {
    damping_torque(i) =
        -v(i) * damping(i) * 2 * std::sqrt(H(i, i) * stiffness(i));
  }
  expected_torque += damping_torque;

  // Check output.
  controller.CalcOutput(*context, output.get());
  const BasicVector<double>* output_vector = output->get_vector_data(0);
  EXPECT_TRUE(CompareMatrices(expected_torque, output_vector->get_value(),
                              1e-10, MatrixCompareType::absolute));
}

}  // namespace kuka_iiwa_arm
}  // namespace examples
}  // namespace drake


// Copyright 2017 Yahoo Holdings. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.

#include "distancetopathfeature.h"
#include "utils.h"
#include <vespa/searchlib/fef/matchdata.h>
#include <vespa/searchlib/fef/properties.h>
#include <vespa/document/datatype/positiondatatype.h>
#include <vespa/vespalib/geo/zcurve.h>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <cmath>
#include <sstream>

#include <vespa/log/log.h>
LOG_SETUP(".features.distancetopathfeature");

namespace search {
namespace features {

const feature_t DistanceToPathExecutor::DEFAULT_DISTANCE(6400000000.0);

DistanceToPathExecutor::DistanceToPathExecutor(std::vector<Vector2> &path,
                                               const search::attribute::IAttributeVector *pos) :
    search::fef::FeatureExecutor(),
    _intBuf(),
    _path(),
    _pos(pos)
{
    if (_pos != NULL) {
        _intBuf.allocate(_pos->getMaxValueCount());
    }
    _path.swap(path); // avoid copy
}

void
DistanceToPathExecutor::execute(uint32_t docId)
{
    if (_path.size() > 1 && _pos != NULL) {
        double pos = -1, trip = 0, product = 0;
        double minSqDist = std::numeric_limits<double>::max();
        _intBuf.fill(*_pos, docId);

        // For each line segment, do
        for (uint32_t seg = 1; seg < _path.size(); ++seg) {
            const Vector2 &p1 = _path[seg - 1];
            const Vector2 &p2 = _path[seg];
            double len2 = (p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y);
            double len = std::sqrt(len2);

            // For each document location, do
            for (uint32_t loc = 0; loc < _intBuf.size(); ++loc) {
                int32_t x = 0, y = 0;
                vespalib::geo::ZCurve::decode(_intBuf[loc], &x, &y);

                double u = 0, dx, dy;
                if (len < 1e-6) {
                    dx = p1.x - x; // process as point
                    dy = p1.y - y;
                } else {
                    u = std::min(1.0, std::max(0.0, (((x - p1.x) * (p2.x - p1.x)) + ((y - p1.y) * (p2.y - p1.y))) / len2));
                    if (u == 0) {
                        dx = p1.x - x; // intersection before segment
                        dy = p1.y - y;
                    } else if (u == 1) {
                        dx = p2.x - x; // intersection after segment
                        dy = p2.y - y;
                    } else {
                        dx = p1.x + u * (p2.x - p1.x) - x;
                        dy = p1.y + u * (p2.y - p1.y) - y;
                    }
                }

                double sqDist = dx * dx + dy * dy;
                if (sqDist < minSqDist) {
                    minSqDist = sqDist;
                    pos = trip + u * len;
                    product = (p2.x - p1.x) * dy - (p2.y - p1.y) * dx;
                }
            }
            trip += len;
        }

        outputs().set_number(0, static_cast<feature_t>(std::sqrt(static_cast<feature_t>(minSqDist))));
        outputs().set_number(1, static_cast<feature_t>(pos > -1 ? (trip > 0 ? pos / trip : 0) : 1));
        outputs().set_number(2, static_cast<feature_t>(product));
    } else {
        outputs().set_number(0, DEFAULT_DISTANCE);
        outputs().set_number(1, 1);
        outputs().set_number(2, 0);
    }
}

DistanceToPathBlueprint::DistanceToPathBlueprint() :
    Blueprint("distanceToPath"),
    _posAttr()
{
}

DistanceToPathBlueprint::~DistanceToPathBlueprint()
{
}

void
DistanceToPathBlueprint::visitDumpFeatures(const search::fef::IIndexEnvironment &,
                                           search::fef::IDumpFeatureVisitor &) const
{
}

search::fef::Blueprint::UP
DistanceToPathBlueprint::createInstance() const
{
    return Blueprint::UP(new DistanceToPathBlueprint());
}

bool
DistanceToPathBlueprint::setup(const search::fef::IIndexEnvironment & env,
                               const search::fef::ParameterList & params)
{
    _posAttr = params[0].getValue();
    describeOutput("distance", "The euclidian distance from the query path.");
    describeOutput("traveled", "The normalized distance traveled along the path before intersection.");
    describeOutput("product",  "The cross-product of the intersecting line segment and the intersection-to-document vector.");
    env.hintAttributeAccess(_posAttr);
    env.hintAttributeAccess(document::PositionDataType::getZCurveFieldName(_posAttr));
    return true;
}

search::fef::FeatureExecutor &
DistanceToPathBlueprint::createExecutor(const search::fef::IQueryEnvironment &env, vespalib::Stash &stash) const
{
    // Retrieve path from query using the name of this and "path" as property.
    std::vector<Vector2> path;
    search::fef::Property pro = env.getProperties().lookup(getName(), "path");
    if (pro.found()) {
        vespalib::string str = pro.getAt(0);
        uint32_t len = str.size();
        if (str[0] == '(' && len > 1 && str[len - 1] == ')') {
            str = str.substr(1, len - 1); // remove braces
            std::vector<vespalib::string> arr;
            boost::split(arr, str, boost::is_any_of(","));
            len = arr.size() - 1;
            for (uint32_t i = 0; i < len; i += 2) {
                double x = util::strToNum<double>(arr[i]);
                double y = util::strToNum<double>(arr[i + 1]);
                path.push_back(Vector2(x, y));
            }
        }
    }

    // Lookup the attribute vector that holds document positions.
    const search::attribute::IAttributeVector *pos = NULL;
    if (path.size() > 1) {
        pos = env.getAttributeContext().getAttribute(_posAttr);
        if (pos == NULL) {
            pos = env.getAttributeContext().getAttribute(document::PositionDataType::getZCurveFieldName(_posAttr));
        }
        if (pos != NULL) {
            if (!pos->isIntegerType()) {
                LOG(warning, "The position attribute '%s' is not an integer attribute. Will use default distance.",
                    pos->getName().c_str());
                pos = NULL;
            } else if (pos->getCollectionType() == attribute::CollectionType::WSET) {
                LOG(warning, "The position attribute '%s' is a weighted set attribute. Will use default distance.",
                    pos->getName().c_str());
                pos = NULL;
            }
        } else {
            LOG(warning, "The position attribute '%s' was not found. Will use default distance.", _posAttr.c_str());
        }
    } else {
        LOG(warning, "No path given in query. Will use default distance.");
    }

    // Create and return a compatible executor.
    return stash.create<DistanceToPathExecutor>(path, pos);
}

} // namespace features
} // namespace search


#ifndef PROP_HPP
# define PROP_HPP

#include <Eigen/Dense>

using Eigen::Matrix;
using Eigen::Vector3d;
using Eigen::Matrix3d;
using Eigen::DiagonalMatrix;

#include "frames.hpp"
#include "gravity.hpp"
#include "vector_math.hpp"
#include "nav_types.hpp"

class Prop {
public:
  /* State */
  double time;
  Vector3d r_imu_inrtl;
  Vector3d v_imu_inrtl;
  Vector3d b_acc;
  Vector3d b_gyro;

  /* Attitude State */
  Matrix3d T_inrtl_to_body; /* (--) rotation matrix from inrtl frame to body frame */

  /* State Transition */
  Matrix<double,9,9>  Phi_xx_dot; /* (--) product of dynamics matrix and state transition
				   *      matrix */
  Matrix<double,9,21> Phi_xb_dot;

  Matrix<double,9,9>  Phi_xx;     /* (--) state transition matrix from previous update to
		                   *      present */
  Matrix<double,9,21> Phi_xb;
  DiagonalMatrix<double,21> Phi_bb; /* (--) ECRV portion of the state transition matrix */

  /** @brief Initialize propagation from a previous update cycle.
   *
   */
  Prop(const double& kf_time,
       const State& X_posterior)
    : time(kf_time)
    , Phi_xx_dot(Matrix9d::Zero())
    , Phi_xb_dot(Matrix<double,9,21>::Zero())
    , Phi_xx(Matrix9d::Identity())
    , Phi_xb(Matrix<double,9,21>::Zero())
  {
    for (size_t ii = 0; ii < 21; ++ii) {
      Phi_bb.diagonal()(ii) = 1 / -TAU;
    }

    // Perform reset
    T_inrtl_to_body = rotvec_to_matrix(X_posterior.block<3,1>(6,0)) * T_inrtl_to_body;

    // Copy out states for propagation
    for (size_t ii = 0; ii < 3; ++ii) {
      r_imu_inrtl[ii] = X_posterior[ii];
      v_imu_inrtl[ii] = X_posterior[ii+3];
      b_acc[ii]       = X_posterior[ii+9];
      b_gyro[ii]      = X_posterior[ii+12];
    }
  }

  /** @brief Propagation method for incorporating IMU measurements
   **        into the predicted state and covariance
   *
   * References:
   *
   * 1. D’Souza, C., & Hanak, C. (2011). The state transition and
   *    process noise matrices in the Orion FILTNAV and EKF. Houston,
   *    TX, US.
   *
   * @param[in]  dv_meas      (m/s) IMU acceleration measurement
   * @param[in]  dtheta_meas  (r) IMU angular velocity measurement
   */
  void propagate(const Vector3d& dv_meas,
		 const Vector3d& dtheta_meas,
		 const GravBody& planet,
		 const Vector3d& w_planet)
  {
    Matrix3d T_inrtl_to_pcpf = compute_T_inrtl_to_pcpf(time, w_planet);
    Vector3d r_imu_pcpf = T_inrtl_to_pcpf * r_imu_inrtl;

    // Compute gravitational acceleration and gravity gradient.
    Vector3d a_imu_pcpf;
    Matrix3d G_pcpf;
    planet.accel(r_imu_pcpf, a_imu_pcpf, G_pcpf);
    Matrix3d G = T_inrtl_to_pcpf.transpose() * G_pcpf;
    Vector3d a_grav_imu = T_inrtl_to_pcpf.transpose() * a_imu_pcpf;
    Vector3d a_nongrav_imu = T_inrtl_to_body.transpose() * (dv_meas / PROP_DT - b_acc);
    Vector3d a_total_imu = a_grav_imu + a_nongrav_imu;

    // Discretized attitude propagation
    Vector3d dtheta = (dtheta_meas / PROP_DT - b_gyro);
    Matrix3d T_inrtl_to_bodynext = rotvec_to_matrix(dtheta) * T_inrtl_to_body;

    // State propagation
    r_imu_inrtl += v_imu_inrtl * PROP_DT + a_total_imu * 0.5 * PROP_DT*PROP_DT + T_inrtl_to_body.transpose() * (Matrix3d::Identity() + skew(dtheta / 3.0)) * a_nongrav_imu * 0.5;
    v_imu_inrtl += a_total_imu * PROP_DT + T_inrtl_to_body.transpose() * (Matrix3d::Identity() + skew(dtheta * 0.5)) * a_nongrav_imu / PROP_DT;

    Matrix3d dvdot_dphi = Matrix3d::Zero(); // FIXME: Moment arm needed

    Matrix3d wx = skew(dtheta_meas / PROP_DT);

    // The four pieces of Phi_dot are:
    // Axx Phixb     Axx Phixb + Axb Phibb
    // 0             Abb Phibb

    // XX component of Phi_dot

    // Set first row
    Phi_xx_dot.block<3,9>(0,0) = Phi_xx.block<3,9>(3,0) * PROP_DT; // rr, rv, rphi

    // Set second row
    Phi_xx_dot.block<3,3>(3,0) = G * Phi_xx.block<3,3>(0,0) * PROP_DT;
    Phi_xx_dot.block<3,3>(3,3) = G * Phi_xx.block<3,3>(0,3) * PROP_DT;
    Phi_xx_dot.block<3,3>(3,6) = (G * Phi_xx.block<3,3>(0,6) + dvdot_dphi * Phi_xx.block<3,3>(6,6)) * PROP_DT;

    // Set third row
    Phi_xx_dot.block<3,3>(6,6) = wx * Phi_xx.block<3,3>(6,6) * PROP_DT;

    Matrix3d Phi_bb_gyro;
    Phi_bb_gyro << Phi_bb.diagonal()(3), 0.0, 0.0,
      0.0, Phi_bb.diagonal()(4), 0.0,
      0.0, 0.0, Phi_bb.diagonal()(5);

    // XB component of Phi_dot

    // Set first row
    Phi_xb_dot.block<3,6>(0,0) = Phi_xb.block<3,6>(3,0) * PROP_DT;

    // Set second row
    Phi_xb_dot.block<3,3>(3,0) = (G * Phi_xb.block<3,3>(0,0) + dvdot_dphi * Phi_xb.block<3,3>(6,0)) * PROP_DT;
    Phi_xb_dot.block<3,3>(3,3) = (G * Phi_xb.block<3,3>(0,3) + dvdot_dphi * Phi_xb.block<3,3>(6,3)) * PROP_DT;

    // Set third row
    Phi_xb_dot.block<3,3>(6,0) = -wx * Phi_xb.block<3,3>(6,0) * PROP_DT;
    Phi_xb_dot.block<3,3>(6,3) = (-wx * Phi_xb.block<3,3>(6,3) - Phi_bb_gyro) * PROP_DT;

    // Update state transition matrix
    Phi_xx += Phi_xx_dot;
    Phi_xb += Phi_xb_dot;

    // Prepare attitude for next propagation
    T_inrtl_to_body = T_inrtl_to_bodynext;

    // Update the time
    time += PROP_DT;

  }

};

#endif