#include "geometry.h"
#include "Eigen/Dense"
#include "Mathematics/ApprCylinder3.h"
#include "Mathematics/ApprOrthogonalPlane3.h"
#include "Mathematics/IntrLine3Plane3.h"
#include "Mathematics/ApprCone3.h"
#include "ApprConeFixAngle3.h"
#include <cmath>

using namespace gte;

#define NITER (5) // number of iterations with outliers
#define OUTLIERS (8) // from 1 to 10 : 10 keeps all approximation points, 1 keeps roughly 10%
//#define _PRINTDEBUG_

/*

class point_coordinate {
public :

double X;
double Y;
double Z;
};

class  cloud{
public :

point_coordinate* coordinate;//table of coordinate of the points
int N;//number of point in the cloud
};
*/


//gives the coordinate of the centroid of a point cloud in a vector [X,Y,Z] 
Eigen::Vector3d getcentroid (const std::vector<npoint3> &cloud);

//gives the covariance matrix of a point cloud, need centroid of the cloud point
Eigen::Matrix3d getcovariance (const std::vector<npoint3> &cloud,Eigen::Vector3d centroid);


//gives the coordinate of the centroid of a point cloud in a vector [X,Y,Z] 
Eigen::Vector3d getcentroid (const std::vector<npoint3> &cloud)
{
    Eigen::Vector3d centroid;
    centroid << 0,0,0;
    for (int i=0;i<cloud.size();i++)
    {
        centroid(0)+=cloud[i].x()/cloud.size();
        centroid(1)+=cloud[i].y()/cloud.size();
        centroid(2)+=cloud[i].z()/cloud.size();
    }
    return centroid;
}

//gives the covariance matrix of a point cloud, need centroid of the cloud point
Eigen::Matrix3d getcovariance (const std::vector<npoint3> &cloud,Eigen::Vector3d centroid)
{
    Eigen::Matrix3d cov;
         cov << 0, 0, 0,
         0, 0, 0,
         0, 0, 0;
    
    for (int i=0;i<cloud.size();i++)
    {
        Eigen::Vector3d it_vector;
        it_vector << cloud[i].x(),cloud[i].y(),cloud[i].z();
        cov +=  (it_vector-centroid)*(it_vector.transpose()-centroid.transpose());
    }
    cov = cov/cloud.size();
    return cov;
}


void get_center(const std::vector<npoint3> pts,npoint3 &center, npoint3 &vec)
{
  if (pts.size())
  {
    Eigen::Vector3d pt = getcentroid(pts);//centroid target point
    Eigen::Matrix3d cov = getcovariance(pts,pt);//covariance matrix target point
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov,Eigen::ComputeFullU);
    #ifdef _PRINTDEBUG_
    std::cout << "Here is the matrix cov:" << std::endl << cov << std::endl;
    std::cout << "Its singular values are:" << std::endl << svd.singularValues() << std::endl;
    std::cout << "Its left singular vectors are the columns of the U matrix:" << std::endl << svd.matrixU() << std::endl;
    #endif
    for (int i=0;i<3;++i) center[i]=pt[i];
    #ifdef _PRINTDEBUG_
    std::cout << center << std::endl;
    #endif
  }
}

void get_frame(const npoint3 &vec,npoint3 &vec1,npoint3 &vec2)
{
  npoint3 vec0=vec;
  vec0.normalize();
  int ii=0;
  double best=fabs(vec0[0]);
  for (int i =1;i<3;++i)
  {
    if (fabs(vec0[i])<best) best=fabs(vec0[i]);ii=i;
  }
  double psca =vec[ii]; 
  for (int i=0;i<3;++i) if (i==ii) vec1[i]=1.0-psca*vec0[i];else vec1[i]=-psca*vec0[i];
  vec1.normalize();
  for (int i=0;i<3;++i) vec2[i]=vec0[(i+1)%3]*vec1[(i+2)%3]-vec0[(i+2)%3]*vec1[(i+1)%3];  
}


bool get_cyl_fine(const std::vector<npoint3> &pts,npoint3 &center, npoint3 &vec, double &radius, std::vector<npoint3> &ptsout, bool hint,bool outliers)
{
 if (pts.size())
  {
    ptsout.clear();
    ptsout.reserve(pts.size());
    std::vector<std::pair<double,int> > errors(pts.size());
    std::vector<npoint3> pts_use;
    pts_use=pts;
    for (int iter=0;iter<(NITER);++iter)
    {
      std::vector<double> rs(pts_use.size());
      std::vector<std::vector<double> > drs(pts_use.size(),std::vector<double>(4));
      std::vector<std::vector<std::vector<double> > > 
                ddrs(pts_use.size(),std::vector<std::vector<double> >(4,std::vector<double>(4)));
      Eigen::Vector3d pt = getcentroid(pts_use);//centroid target point
      Eigen::Matrix3d eigv;
      double det;
      if (hint)
      {
        npoint3 v1,v2;
        get_frame(vec,v1,v2);
        for(int i=0;i<3;++i)
        {
          eigv(i,0)=vec[i];
          eigv(i,1)=v1[i];
          eigv(i,2)=v2[i];
        }
        det=eigv.determinant();
      }
      else
      {
        Eigen::Matrix3d cov = getcovariance(pts_use,pt);//covariance matrix target point
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig(cov,Eigen::ComputeEigenvectors);
        Eigen::Vector3d sing=eig.eigenvalues();
        eigv=eig.eigenvectors();
        det=eigv.determinant();
        if (det<0) {eigv*=-1;det*=-1;}
      }
      
      Eigen::Matrix3d inv_eigv=eigv.inverse();
      Eigen::Vector4d delta(0.,0.,0.,0.);
      double rr;
      double var=-1.;
      double norm=-1.;
      #ifdef _PRINTDEBUG_
      std::cout << "entering newton" << std::endl;
      #endif
      for (int i=0;i<25;++i) // boucle convergence newton
      {
        #ifdef _PRINTDEBUG_
        std::cout << " i " << i << std::endl;
        #endif
        rr=0;
        std::vector<double> drr(4,0.);
        std::vector<std::vector<double> > ddrr(4,std::vector<double>(4,0.));
        std::vector<double> dvar(4,0.);
        std::vector<std::vector<double> > ddvar(4,std::vector<double>(4,0.));
        Eigen::Matrix3d rotz,roty,rotzi,rotyi,transf,invtransf;
        Eigen::Vector3d translate;
        rotz<< cos(delta[0]),-sin(delta[0]),0,sin(delta[0]),cos(delta[0]),0,0,0,1;
        roty<< cos(delta[1]),0,sin(delta[1]),0,1,0,-sin(delta[1]),0,cos(delta[1]);
        rotzi<< cos(-delta[0]),-sin(-delta[0]),0,sin(-delta[0]),cos(-delta[0]),0,0,0,1;
        rotyi<< cos(-delta[1]),0,sin(-delta[1]),0,1,0,-sin(-delta[1]),0,cos(-delta[1]);
        transf=rotz*roty*inv_eigv;
        invtransf=eigv*rotyi*rotzi;
        translate=eigv.col(2)*delta[2]+eigv.col(1)*delta[3];
        for (int k=0;k<pts_use.size();++k)
        {
          Eigen::Vector3d point,pointm;
          std::vector<Eigen::Vector3d> dpointm(4);
          std::vector<std::vector<Eigen::Vector3d> > ddpointm(4,std::vector<Eigen::Vector3d>(4));
          point << pts_use[k].x(),pts_use[k].y(),pts_use[k].z();

          pointm=point-pt;
          pointm+=translate;
          pointm=transf*pointm;
          // derivees
          dpointm[0] << -pointm[1],pointm[0],0; // der petit angle autour de z
          dpointm[1] << pointm[2],0,-pointm[0]; // der petit angle autour de y
          dpointm[2] << 0,0,1; // der petit depl le long de z
          dpointm[3] << 0,1,0; // der petit depl le long de y

          
          // derivees secondes
          ddpointm[0][0] << -pointm[0],-pointm[1],0; // der petit angle autour de zz
          ddpointm[1][1] << -pointm[0],0,-pointm[2]; // der petit angle autour de yy
          ddpointm[0][1] << 0,pointm[2],pointm[1]; // der petit angle autour de zy
          ddpointm[1][0] << 0,pointm[2],pointm[1]; // der petit angle autour de zy
          ddpointm[2][2] << 0,0,1; // der petit depl le long de zz
          ddpointm[3][3] << 0,1,0; // der petit depl le long de zz
          ddpointm[2][0] << 0,0,0; ddpointm[2][1] << 0,0,0; ddpointm[2][3] << 0,0,0; 
          ddpointm[3][0] << 0,0,0; ddpointm[3][1] << 0,0,0; ddpointm[3][2] << 0,0,0; 
          
          rs[k]=pointm[1]*pointm[1]+pointm[2]*pointm[2];
          
          for (int l=0;l<4;++l) drs[k][l]=2*pointm[1]*dpointm[l][1]+2*pointm[2]*dpointm[l][2];
          for (int l=0;l<4;++l)
            for (int m=0;m<4;++m)
              ddrs[k][l][m]=2*(dpointm[l][1]*dpointm[m][1]+pointm[1]*ddpointm[l][m][1])+2*(dpointm[l][2]*dpointm[m][2]+pointm[2]*ddpointm[l][m][2]);

          rr+=rs[k]/pts_use.size();
          
          for (int l=0;l<4;++l) drr[l]+=drs[k][l]/pts_use.size();
          
          for (int l=0;l<4;++l)
            for (int m=0;m<4;++m)
              ddrr[l][m]+=ddrs[k][l][m]/pts_use.size();
          
        }
        double nvar=0.;
        for (int k=0;k<pts_use.size();++k)
        {
          nvar+=((rs[k]-rr)*(rs[k]-rr))/(pts_use.size()-1);
          for (int l=0;l<4;++l)
            dvar[l]+=2*((rs[k]-rr)*(drs[k][l]-drr[l]))/(pts_use.size()-1);
          for (int l=0;l<4;++l)
            for (int m=0;m<4;++m)
              ddvar[l][m]+=2*((rs[k]-rr)*(ddrs[k][l][m]-ddrr[l][m])+(drs[k][l]-drr[l])*(drs[k][m]-drr[m]))/(pts_use.size()-1);
        }

      

        double nnorm=0;
        for (int l=0;l<4;++l) nnorm+=dvar[l]*dvar[l];
        nnorm=sqrt(nnorm);
        if (std::isnan(nvar)) break;//return false; // often because we are already at the minimum...
        #ifdef _PRINTDEBUG_
        std::cout << "Pour i=" << i << " : var=" << nvar << " normdvar=" << nnorm << " delta " << delta[0] << " " << delta[1] << " " <<delta[2] << " " <<delta[3] << std::endl;
        #endif
        norm=nnorm;
        if (var<0) var=nvar;
        else
          if (nvar>var)
            break;//return false;
          else var=nvar;
        
        if (norm<1e-12)
        {
          Eigen::Vector3d centerm=pt-translate;
          for (int i=0;i<3;++i) center[i]=centerm[i];
          Eigen::Vector3d axis,axism;
          axis << 1,0,0;
          axism=invtransf*axis;
          for (int i=0;i<3;++i) vec[i]=axism[i];
          radius=rr;
          break;
        }
        
        Eigen::Vector4d der;
        Eigen::Matrix4d hess;

        for (int l=0;l<4;++l) der[l]=dvar[l];
        for (int l=0;l<4;++l)
          for (int m=0;m<4;++m)
            hess(l,m)=ddvar[l][m];

        Eigen::LDLT<Eigen::Ref<Eigen::Matrix4d> > ldlt(hess);
        delta= delta - ldlt.solve(der);
      }
      for (int i=0;i<pts.size();++i)
      {
        npoint3 diff = pts[i] - center;
        double dot1=diff.dotprod(diff);
        double dot2=diff.dotprod(vec);
        double r2=dot1-dot2*dot2;
        double err=fabs(radius*radius-r2);
        errors[i]=std::make_pair(sqrt(err),i);
      }
      std::sort(errors.begin(),errors.end());
      pts_use.clear();
      if (pts.size()<4) break;
      double maxerr;
      if (iter<(NITER)-1) ptsout.clear();
      if (outliers)
      for (int i=0;i<(pts.size()*(OUTLIERS))/10;++i)
      { 
        if (iter<(NITER)-1) ptsout.push_back(pts[errors[i].second]);
        pts_use.push_back(pts[errors[i].second]);
        maxerr=errors[i].first;
      }
      else break;
      #ifdef _PRINTDEBUG_
      std::cout << " error : " << maxerr << std::endl;
      #endif
    }
    #ifdef _PRINTDEBUG_
    std::cout << "center : " << center << std::endl;
    std::cout << "vec : " << vec << std::endl;
    std::cout << "radius : " << radius << std::endl;
    #endif
    return true;
  }
  return false;
}


bool get_cyl(const std::vector<npoint3> &pts,npoint3 &center, npoint3 &vec, double &radius,std::vector<npoint3> &ptsout,bool outliers)
{
  ptsout.clear();
  ptsout.reserve(pts.size());
  std::vector<std::pair<double,int> > errors(pts.size());
//    ApprCylinder3<double> fitter(8, 64, 32); // approx
    ApprCylinder3<double> fitter(8, 256, 128); // OK pour ~ 5 µm
//    ApprCylinder3<double> fitter(8,512,256);//, 1024, 1024); // OK pour ~ 1 µm
/*    ApprCylinder3<double> fitter0(0);
    ApprCylinder3<double> fitter1(1);
    ApprCylinder3<double> fitter2(2);*/
   std::vector<Vector3<double> > positions;
   for (unsigned int i = 0; i < pts.size(); ++i)
    {
        Vector3<double> data;
        data[0]=pts[i].x();
        data[1]=pts[i].y();
        data[2]=pts[i].z();
        positions.push_back(data);
    }

    unsigned int numVertices = static_cast<unsigned int>(positions.size());
    Cylinder3<double> cylinder;
    double minError;

    for (int iter=0;iter<(NITER);++iter)
    {
      minError = fitter(numVertices, positions.data(), cylinder);
      for (int i=0;i<pts.size();++i)
      {
        Vector3<double> v;
        v[0]=pts[i].x();v[1]=pts[i].y();v[2]=pts[i].z();
        Vector3<double> diff = v - cylinder.axis.origin;
        double dot1=Dot(diff,diff);
        double dot2=Dot(diff, cylinder.axis.direction);
        double r2=dot1-dot2*dot2;
        double err=fabs(cylinder.radius*cylinder.radius-r2);
        errors[i]=std::make_pair(sqrt(err),i);
      }
      std::sort(errors.begin(),errors.end());
      #ifdef _PRINTDEBUG_
      for (int i=0;i<pts.size();++i)
        std::cout << errors[i].first << " " << errors[i].second << std::endl;
      #endif
      positions.clear();
      if (pts.size()<4) break;
      double maxerr;
      if (iter<(NITER)-1) ptsout.clear();
      if (outliers)
      {
      for (int i=0;i<(pts.size()*(OUTLIERS))/10;++i)
      { 
        if (iter<(NITER)-1) ptsout.push_back(pts[errors[i].second]);
        Vector3<double> data;
        data[0]=pts[errors[i].second].x();
        data[1]=pts[errors[i].second].y();
        data[2]=pts[errors[i].second].z();
        positions.push_back(data);
        maxerr=errors[i].first;
      }
      }
      else break;
      #ifdef _PRINTDEBUG_
      std::cout << maxerr << std::endl;
      std::cout << "center = "
          << cylinder.axis.origin[0] << " "
          << cylinder.axis.origin[1] << " "
          << cylinder.axis.origin[2] << std::endl;
      std::cout << "direction = "
          << cylinder.axis.direction[0] << " "
          << cylinder.axis.direction[1] << " "
          << cylinder.axis.direction[2] << std::endl;
      #endif
    }
    center[0]=cylinder.axis.origin[0];
    center[1]=cylinder.axis.origin[1];
    center[2]=cylinder.axis.origin[2];
    vec[0]=cylinder.axis.direction[0];
    vec[1]=cylinder.axis.direction[1];
    vec[2]=cylinder.axis.direction[2];
    radius=cylinder.radius;
    return true;
}

bool get_cone(const std::vector<npoint3> &pts,npoint3 &center, npoint3 &vec,double & angle,std::vector<npoint3> &ptsout)
{
  const int niter=5;
  Vector3<double> coneVertex;
  Vector3<double> coneAxis;
  double coneAngle;
  std::vector<std::pair<double,int> > errors(pts.size());
  get_plane(pts,center, vec,ptsout); // starting point
  coneVertex[0]=center[0];
  coneVertex[1]=center[1];
  coneVertex[2]=center[2];
  coneAxis[0]=vec[0];
  coneAxis[1]=vec[1];
  coneAxis[2]=vec[2];
  coneAngle=1.047197551; //60°
//  coneAngle=0.01;
  ptsout.clear();
  ptsout.reserve(pts.size());
  std::vector<Vector3<double> > positions;
  for (unsigned int i = 0; i < pts.size(); ++i)
  {
    Vector3<double> data;
    data[0]=pts[i].x();
    data[1]=pts[i].y();
    data[2]=pts[i].z();
    positions.push_back(data);
  }
  ApprConeFixAngle3<double> fitter(false,60.*3.1415926565/180.);
//  ApprConeFixAngle3<double> fitter(false,0.02);
  size_t const maxIterations = 10;
  double const updateLengthTolerance = 1e-04;
  double const errorDifferenceTolerance = 1e-07;
  double const lambdaFactor = 0.01;
  double const lambdaAdjust = 10.0;
  size_t const maxAdjustments = 8;
  bool useConeInputAsInitialGuess = true;
  bool res;

  for (int iter=0;iter<(niter);++iter)  {
    unsigned int numVertices = static_cast<unsigned int>(positions.size());
    fitter(numVertices, positions.data(),
        maxIterations, updateLengthTolerance, errorDifferenceTolerance,
       /* lambdaFactor, lambdaAdjust, maxAdjustments,*/
          useConeInputAsInitialGuess, coneVertex, coneAxis, coneAngle);
    for (int i=0;i<pts.size();++i)
    {
      Vector3<double> v;
      
      v[0]=pts[i].x();v[1]=pts[i].y();v[2]=pts[i].z();
      Vector3<double> diff = v - coneVertex;
      Vector3<double> diff2 = diff2;
      double l=Normalize(diff2);
      double err = fabs(Dot(diff, coneAxis)/l-cos(coneAngle));
      errors[i]=std::make_pair(err,i);
    }
    std::sort(errors.begin(),errors.end());
    positions.clear();
    if (pts.size()<10) break;
    double maxerr;
    if (iter<(niter)-1) ptsout.clear();
    for (int i=0;i<(pts.size()*(OUTLIERS))/10;++i)
    {
        Vector3<double> data;
        if (iter<(niter)-1) ptsout.push_back(pts[errors[i].second]);
        data[0]=pts[errors[i].second].x();
        data[1]=pts[errors[i].second].y();
        data[2]=pts[errors[i].second].z();
        positions.push_back(data);
        maxerr=errors[i].first;
    }
  }
  #ifdef _PRINTDEBUG_
  std::cout << "cone" << std::endl;
  std::cout << coneVertex[0] << " " << coneVertex[1] << " " << coneVertex[2] << std::endl ;
  std::cout << coneAxis[0] << " " << coneAxis[1] << " " << coneAxis[2] << std::endl;
  std::cout << coneAngle*180./3.1415926565 << std::endl;
  #endif //_PRINTDEBUG_
  center[0]=coneVertex[0];
  center[1]=coneVertex[1];
  center[2]=coneVertex[2];
  vec[0]=coneAxis[0];
  vec[1]=coneAxis[1];
  vec[2]=coneAxis[2];
  angle=coneAngle;
  return true;
}



void get_plane(const std::vector<npoint3> &pts,npoint3 &orig, npoint3 &norm,std::vector<npoint3> &ptsout,bool outliers)
{
   std::vector<std::pair<double,int> > errors(pts.size());
   ApprOrthogonalPlane3<double> fitter;
   std::vector<Vector3<double> > positions;
   for (unsigned int i = 0; i < pts.size(); ++i)
    {
        Vector3<double> data;
        data[0]=pts[i].x();
        data[1]=pts[i].y();
        data[2]=pts[i].z();
        positions.push_back(data);
    }
    bool res;
    for (int iter=0;iter<(NITER);++iter)
    {
      unsigned int numVertices = static_cast<unsigned int>(positions.size());
      res = fitter.Fit(numVertices, positions.data());
      std::pair<Vector3<double>,Vector3<double> > plane=fitter.GetParameters();
      for (int i=0;i<pts.size();++i)
      {
        Vector3<double> v;
        v[0]=pts[i].x();v[1]=pts[i].y();v[2]=pts[i].z();
        Vector3<double> diff = v - plane.first;
        double err = fabs(Dot(diff, plane.second));
        errors[i]=std::make_pair(err,i);
      }
      std::sort(errors.begin(),errors.end());
      #ifdef _PRINTDEBUG_
      for (int i=0;i<pts.size();++i)
        std::cout << errors[i].first << " " << errors[i].second << std::endl;
      #endif
      positions.clear();
      if (pts.size()<4) break;
      double maxerr;
      if (iter<(NITER)-1) ptsout.clear();
      if (outliers)
      {
      for (int i=0;i<(pts.size()*(OUTLIERS))/10;++i)
      {
          Vector3<double> data;
          if (iter<(NITER)-1) ptsout.push_back(pts[errors[i].second]);
          data[0]=pts[errors[i].second].x();
          data[1]=pts[errors[i].second].y();
          data[2]=pts[errors[i].second].z();
          positions.push_back(data);
          maxerr=errors[i].first;
      }
      }
      else break;
      #ifdef _PRINTDEBUG_
      std::cout << maxerr << std::endl;
      if (res)
      {
          std::cout << "origin = "
          << plane.first[0] << " "
          << plane.first[1] << " "
          << plane.first[2] << std::endl;
          std::cout << "normal = "
          << plane.second[0] << " "
          << plane.second[1] << " "
          << plane.second[2] << std::endl;
      }
      #endif
    }

    std::pair<Vector3<double>,Vector3<double> > plane=fitter.GetParameters();
    
    if (!res) std::cout << "no fit" << std::endl;
    orig[0]=plane.first[0];
    orig[1]=plane.first[1];
    orig[2]=plane.first[2];
    norm[0]=plane.second[0];
    norm[1]=plane.second[1];
    norm[2]=plane.second[2];
}

void int_line_plane(const npoint3 &origl, const npoint3 &vecl, const npoint3 &origp, const npoint3 &normp, npoint3 &intersec)
{
  FIQuery<double, Line3<double>, Plane3<double> > intersect;
  Line3<double> l;
  l.origin[0]=origl[0];l.origin[1]=origl[1];l.origin[2]=origl[2];
  l.direction[0]=vecl[0];l.direction[1]=vecl[1];l.direction[2]=vecl[2];
  Vector3<double> nor;nor[0]=normp[0];nor[1]=normp[1];nor[2]=normp[2];
  Vector3<double> pt;pt[0]=origp[0];pt[1]=origp[1];pt[2]=origp[2];
  Plane3<double> p(nor,pt);
  auto result = intersect(l,p);
  #ifdef _PRINTDEBUG_
  std::cout << "center = "
        << result.point[0] << " "
        << result.point[1] << " "
        << result.point[2] << std::endl;
  #endif
  intersec[0]=result.point[0];
  intersec[1]=result.point[1];
  intersec[2]=result.point[2];
}


void adjustsb(std::vector<std::pair<npoint3,npoint3> > &sb,const std::vector<std::pair<npoint3,npoint3> > &sbr)
{
  int max;
  if (sb.size()<=sbr.size()) max=sb.size(); else max=sbr.size();
  Eigen::Matrix3d R,Us,Ut,Us2,R2;
  Eigen::Matrix3d swap;
  Eigen::Vector3d T,T2;
  std::vector<npoint3> sbr_;
  std::vector<npoint3> sb_;
  for (int i=0;i<sb.size();++i) sb_.push_back(sb[i].first);
  for (int i=0;i<sbr.size();++i) sbr_.push_back(sbr[i].first);

  Eigen::Vector3d p_t = getcentroid(sbr_);//centroid target point
//  p_t << sbr_[0].x(),sbr_[0].y(),sbr_[0].z();
  Eigen::Matrix3d cov_t = getcovariance(sbr_,p_t);//covariance matrix target point
  Eigen::Vector3d p_s = getcentroid(sb_);//centroid source point
//  p_s << sb_[0].x(),sb_[0].y(),sb_[0].z();
  Eigen::Matrix3d cov_s = getcovariance(sb_,p_s);//covariance matrix source point
  Eigen::JacobiSVD<Eigen::Matrix3d,Eigen::NoQRPreconditioner> svd_t(cov_t,Eigen::ComputeFullU);
  Eigen::JacobiSVD<Eigen::Matrix3d,Eigen::NoQRPreconditioner> svd_s(cov_s,Eigen::ComputeFullU);
  Ut=svd_t.matrixU();
  Us=svd_s.matrixU();
  #ifdef _PRINTDEBUG_
  std::cout << Ut << std::endl << std::endl;
  std::cout << Ut.determinant() << std::endl ;
  std::cout << svd_t.singularValues() << std::endl << std::endl;
  std::cout << Us << std::endl<< std::endl;
  std::cout << Us.determinant() << std::endl ;
  std::cout << svd_s.singularValues() << std::endl << std::endl;
  #endif
  R = Ut*Us.transpose();
  if (R.determinant()<0.0)
  {
    // la rotation inclut une réflexion vers un systeme d'axes non direct !
    swap << 1,0,0,0,1,0,0,0,-1;
    Us=Us*swap;
//    std::cout << Us << std::endl<< std::endl;
    R = Ut*Us.transpose();
  }
  double psca=0;
  Eigen::Vector3d tr;
  for (int i=0;i<max;++i)
  {
    tr << sb[i].second.x(),sb[i].second.y(),sb[i].second.z();
    tr = R*tr;
    psca+=sqrt(pow(tr[0]-sbr[i].second.x(),2)+pow(tr[1]-sbr[i].second.y(),2)+pow(tr[2]-sbr[i].second.z(),2));
  }
  if (psca>1.5*max)
  {
  // la rotation nous amène dans un sens inverse à ce qui est désiré 
    swap << -1,0,0,0,1,0,0,0,-1;
    Us=Us*swap;
    R = Ut*Us.transpose();
  }
  
  
  
  double err1=0;
  double err2=0;
  
  T = p_t - R*p_s;
    // rotation 180 z
  {
    swap << -1,0,0,0,-1,0,0,0,1;
    Us2=Us*swap;
    R2 = Ut*Us2.transpose();
  }
  T2 = p_t - R2*p_s;

  
  for (int i=0;i<max;++i)
  {
    tr << sb[i].first.x(),sb[i].first.y(),sb[i].first.z();
    tr = R*tr+T;
    err1+=sqrt(pow(tr[0]-sbr[i].first.x(),2)+pow(tr[1]-sbr[i].first.y(),2)+pow(tr[2]-sbr[i].first.z(),2));
  }
  
  for (int i=0;i<max;++i)
  {
    tr << sb[i].first.x(),sb[i].first.y(),sb[i].first.z();
    tr = R2*tr+T2;
    err2+=sqrt(pow(tr[0]-sbr[i].first.x(),2)+pow(tr[1]-sbr[i].first.y(),2)+pow(tr[2]-sbr[i].first.z(),2));
  }

  if (err2<err1)
  {
    Us=Us2;
    R=R2;
    T=T2;
  }
  
  std::cout << R << std::endl;
  std::cout << T << std::endl;

  for (int i=0;i<sb.size();++i)
  {
    tr << sb[i].first.x(),sb[i].first.y(),sb[i].first.z();
    tr = R*tr + T;
    sb[i].first.x()=tr(0);sb[i].first.y()=tr(1);sb[i].first.z()=tr(2);
    tr << sb[i].second.x(),sb[i].second.y(),sb[i].second.z();
    tr = R*tr;
    sb[i].second.x()=tr(0);sb[i].second.y()=tr(1);sb[i].second.z()=tr(2);
  }
}