void computePose(std::vector<vpPoint> &point, const std::vector<vpImagePoint> &corners,
                 const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo)
{
  vpPose pose;
  double x=0, y=0;
  for (unsigned int i=0; i < point.size(); i ++) {
    vpPixelMeterConversion::convertPoint(cam, corners[i], x, y);
    point[i].set_x(x);
    point[i].set_y(y);
    pose.addPoint(point[i]);
  }

  if (init == true) pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
  else              pose.computePose(vpPose::VIRTUAL_VS, cMo) ;

  vpTranslationVector t;
  cMo.extract(t);
  vpRotationMatrix R;
  cMo.extract(R);
  vpThetaUVector tu(R);

  std::cout << "cMo: ";
  for (unsigned int i=0; i < 3; i++)
    std::cout << t[i] << " ";
  for (unsigned int i=0; i < 3; i++)
    std::cout << vpMath::deg(tu[i]) << " ";
  std::cout << std::endl;
}

/*!
  Build a 6 dimension pose vector \f$ [\bf t, \Theta \bf u]^\top\f$ from
  an homogeneous matrix \f$ \bf M \f$.

  \param M : Homogeneous matrix \f$ \bf M \f$ from which translation \f$
  \bf t \f$ and \f$\Theta \bf u \f$ vectors are extracted to initialize
  the pose vector.

  \return The build pose vector.

*/
vpPoseVector
vpPoseVector::buildFrom(const vpHomogeneousMatrix& M)
{
  vpRotationMatrix R ;    M.extract(R) ;
  vpTranslationVector tv ; M.extract(tv) ;
  buildFrom(tv,R) ;
  return *this ;
}

/*!

  Initialize a velocity twist transformation matrix from an homogeneous matrix
  \f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
  \\ {\bf 0}_{1\times 3} & 1 \end{array} \right] \f]

  \param M : Homogeneous matrix \f$M\f$ used to initialize the twist
  transformation matrix.

*/
vpVelocityTwistMatrix
vpVelocityTwistMatrix::buildFrom(const vpHomogeneousMatrix &M)
{
  vpTranslationVector tv ;
  vpRotationMatrix R ;
  M.extract(R) ;
  M.extract(tv) ;

  buildFrom(tv, R) ;
  return (*this) ;
}

/*!
  Set the position and the orientation of a SceneNode.
  \param name : Name of the SceneNode to rotate.

  \param wMo : The homogeneous matrix representing the rotation and
  translation to apply.

*/
void vpAROgre::setPosition(const std::string &name,
         const vpHomogeneousMatrix &wMo)
{
  // Extract the position and orientation data
  vpRotationMatrix rotations;
  vpTranslationVector translation;
  wMo.extract(rotations);
  wMo.extract(translation);
  // Apply them to the node
  setPosition(name, translation);
  setRotation(name, rotations);
}

void printPose(const std::string &text, const vpHomogeneousMatrix &cMo)
{
  vpTranslationVector t;
  cMo.extract(t);
  vpRotationMatrix R;
  cMo.extract(R);
  vpThetaUVector tu(R);

  std::cout << text;
  for (unsigned int i=0; i < 3; i++)
    std::cout << t[i] << " ";
  for (unsigned int i=0; i < 3; i++)
    std::cout << vpMath::deg(tu[i]) << " ";
  std::cout << std::endl;
}

void
vpProjectionDisplay::displayCamera(vpImage<unsigned char> &I,
				   const vpHomogeneousMatrix &cextMo,
				   const vpHomogeneousMatrix &cMo,
				   const vpCameraParameters &cam)
{
  vpHomogeneousMatrix c1Mc ;
  c1Mc = cextMo*cMo.inverse() ;

  o.track(c1Mc) ;
  x.track(c1Mc) ;
  y.track(c1Mc) ;
  z.track(c1Mc) ;

  vpImagePoint ipo;
  vpImagePoint ipx;

  vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo) ;

  vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ipx) ;
  vpDisplay::displayArrow(I, ipo, ipx, vpColor::green) ;

  vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ipx) ;
  vpDisplay::displayArrow(I, ipo, ipx, vpColor::blue) ;

  vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ipx) ;
  vpDisplay::displayArrow(I, ipo, ipx, vpColor::red) ;
}

int HRP2ComputeControlLawProcess::loadcMh(vpHomogeneousMatrix& cMh)
{


  //TODO : this file name shouldn't be written here
  ifstream file;
  string name="./data/ViSP/hrp2CamParam/cMh.3";
  file.open (name.c_str());

  try
    {
      cMh.load(file);
    }
  catch(vpException a) // file doesn't exist
    {
      cout << "---- Open Exception Default---------------"<<endl;
      cout<<"----Wide camera extrinsic parameters ---- " <<endl;
      vpCTRACE << endl <<a;
      cout << "------------------ " <<endl;
      // fill a defaut matrix
      cMh[0][0]=0.016937505;   cMh[0][1]=-0.9997821632;  cMh[0][2]= -0.01217327283; cMh[0][3]=-0.05195444004;
      cMh[1][0]=-0.1698427574; cMh[1][1]=0.009121740254; cMh[1][2]= -0.9854286431;  cMh[1][3]= 0.1036017168 ;
      cMh[2][0]=0.9853256992 ; cMh[2][1]=0.0187590039;   cMh[2][2]= -0.1696511946;  cMh[2][3]=-0.04729590458 ;
      cMh[3][0]=0; cMh[3][1]=0;cMh[3][2]= 0;  cMh[3][3]=1 ;
      return -1;
    }

  file.close();
  return 0;
}

void
vpProjectionDisplay::displayCamera(vpImage<unsigned char> &I,
                                   const vpHomogeneousMatrix &cextMo,
                                   const vpHomogeneousMatrix &cMo,
                                   const vpCameraParameters &cam,
                                   const unsigned int thickness)
{
  vpHomogeneousMatrix c1Mc ;
  c1Mc = cextMo*cMo.inverse() ;

  o.track(c1Mc) ;

  if(o.get_Z() < 0)	// do not print camera if behind the external camera
	  return;

  x.track(c1Mc) ;
  y.track(c1Mc) ;
  z.track(c1Mc) ;

  vpImagePoint ipo;
  vpImagePoint ipx;

  vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo) ;

  vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ipx) ;
  vpDisplay::displayArrow(I, ipo, ipx, vpColor::green, 4+thickness, 2+thickness, thickness) ;

  vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ipx) ;
  vpDisplay::displayArrow(I, ipo, ipx, vpColor::blue, 4+thickness, 2+thickness, thickness) ;

  vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ipx) ;
  vpDisplay::displayArrow(I, ipo, ipx, vpColor::red, 4+thickness, 2+thickness, thickness) ;
}

/*!

  Compute the pose \e cMo from the 3D coordinates of the points \e point and
  their corresponding 2D coordinates \e dot. The pose is computed using a Lowe
  non linear method.

  \param point : 3D coordinates of the points.

  \param dot : 2D coordinates of the points.

  \param ndot : Number of points or dots used for the pose estimation.

  \param cam : Intrinsic camera parameters.

  \param cMo : Homogeneous matrix in output describing the transformation
  between the camera and object frame.

  \param cto : Translation in ouput extracted from \e cMo.

  \param cro : Rotation in ouput extracted from \e cMo.

  \param init : Indicates if the we have to estimate an initial pose with
  Lagrange or Dementhon methods.

*/
void compute_pose(vpPoint point[], vpDot2 dot[], int ndot,
                  vpCameraParameters cam,
                  vpHomogeneousMatrix &cMo,
                  vpTranslationVector &cto,
                  vpRxyzVector &cro, bool init)
{
  vpHomogeneousMatrix cMo_dementhon;  // computed pose with dementhon
  vpHomogeneousMatrix cMo_lagrange;  // computed pose with dementhon
  vpRotationMatrix cRo;
  vpPose pose;
  vpImagePoint cog;
  for (int i=0; i < ndot; i ++) {

    double x=0, y=0;
    cog = dot[i].getCog();
    vpPixelMeterConversion::convertPoint(cam,
                                         cog,
                                         x, y) ; //pixel to meter conversion
    point[i].set_x(x) ;//projection perspective          p
    point[i].set_y(y) ;
    pose.addPoint(point[i]) ;
  }

  if (init == true) {
    pose.computePose(vpPose::DEMENTHON, cMo_dementhon) ;
    // Compute and return the residual expressed in meter for the pose matrix
    // 'cMo'
    double residual_dementhon = pose.computeResidual(cMo_dementhon);
    pose.computePose(vpPose::LAGRANGE, cMo_lagrange) ;
    double residual_lagrange = pose.computeResidual(cMo_lagrange);

    // Select the best pose to initialize the lowe pose computation
    if (residual_lagrange < residual_dementhon)  
      cMo = cMo_lagrange;
    else
      cMo = cMo_dementhon;

  }
  else { // init = false; use of the previous pose to initialise LOWE
    cRo.buildFrom(cro);
    cMo.buildFrom(cto, cRo);
  }
  pose.computePose(vpPose::LOWE, cMo) ;
  cMo.extract(cto);
  cMo.extract(cRo);
  cro.buildFrom(cRo);
}

/*!
  Build a 3D \f$ \theta u \f$ visual feature from an
  homogeneous matrix \f$ M \f$ that represent the displacement that
  the camera has to achieve.

  \param M [in] : Homogeneous transformation that describe the
  movement that the camera has to achieve. Only the rotational part of
  this homogeneous transformation is taken into consideration.
  Depending on the rotation representation type
  (vpFeatureThetaU::vpFeatureThetaURotationRepresentationType) used to
  construct this object, the parameter \e M represents either the
  rotation that the camera has to achieve to move from the desired
  camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
  that the camera has to achieve to move from the current camera frame
  to the desired one (\f$ ^{c}R_{c^*}\f$).


*/
void
vpFeatureThetaU::buildFrom(const vpHomogeneousMatrix &M)
{
    vpRotationMatrix R ;
    M.extract(R)  ;
    vpThetaUVector tu(R) ;
    buildFrom(tu) ;
}

void readConfigVar_PoseRxyz(const string &s, vpHomogeneousMatrix &M)
{
    vpColVector v(6);
    vpIoTools::readConfigVar(s, v);
    vpTranslationVector t(v[0], v[1], v[2]);
    vpRxyzVector r(v[3], v[4], v[5]);
    vpRotationMatrix R(r);
    M.buildFrom(t, R);
}

vpMatrix execute(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
                 vpMomentObject &src, vpMomentObject &dst)
{
  vpServo::vpServoIteractionMatrixType interaction_type = vpServo::CURRENT; ; //current or desired

  vpServo task;
  task.setServo(vpServo::EYEINHAND_CAMERA);
  //A,B,C parameters of source and destination plane
  double A; double B; double C;
  double Ad; double Bd; double Cd;
  //init main object: using moments up to order 6

  //Initializing values from regular plane (with ax+by+cz=d convention)
  vpPlane pl;
  pl.setABCD(0,0,1.0,0);
  pl.changeFrame(cMo);
  planeToABC(pl,A,B,C);

  pl.setABCD(0,0,1.0,0);
  pl.changeFrame(cdMo);
  planeToABC(pl,Ad,Bd,Cd);

  //extracting initial position (actually we only care about Zdst)
  vpTranslationVector vec;
  cdMo.extract(vec);

  ///////////////////////////// initializing moments and features /////////////////////////////////
  //don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
  vpMomentCommon moments (vpMomentCommon ::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst), vec[2]);
  vpMomentCommon momentsDes(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst),vec[2]);
  //same thing with common features
  vpFeatureMomentCommon featureMoments(moments);
  vpFeatureMomentCommon featureMomentsDes(momentsDes);

  moments.updateAll(src);
  momentsDes.updateAll(dst);

  featureMoments.updateAll(A,B,C);
  featureMomentsDes.updateAll(Ad,Bd,Cd);

  //setup the interaction type
  task.setInteractionMatrixType(interaction_type) ;
  //////////////////////////////////add useful features to task//////////////////////////////
  task.addFeature(featureMoments.getFeatureGravityNormalized(),featureMomentsDes.getFeatureGravityNormalized());
  task.addFeature(featureMoments.getFeatureAn(),featureMomentsDes.getFeatureAn());
  //the moments are different in case of a symmetric object
  task.addFeature(featureMoments.getFeatureCInvariant(),featureMomentsDes.getFeatureCInvariant(),(1 << 10) | (1 << 11));
  task.addFeature(featureMoments.getFeatureAlpha(),featureMomentsDes.getFeatureAlpha());

  task.setLambda(0.4) ;

  task.computeControlLaw();
  vpMatrix mat = task.computeInteractionMatrix();
  task.kill();
  return mat;
}

/*!
Converts an homogeneous matrix into a \f$\theta {\bf u}\f$ vector.
*/
vpThetaUVector
vpThetaUVector::buildFrom(const vpHomogeneousMatrix& M)
{
  vpRotationMatrix R;

  M.extract(R);
  buildFrom(R);

  return *this ;
}

/*!

  Initialize a force/torque twist transformation matrix from an homogeneous matrix
  \f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
  \\ {\bf 0}_{1\times 3} & 1 \end{array} \right] \f]

  \param M : Homogeneous matrix \f$M\f$ used to initialize the force/torque twist
  transformation matrix.

*/
vpForceTwistMatrix
vpForceTwistMatrix::buildFrom(const vpHomogeneousMatrix &M)
{
  vpTranslationVector t ;
  vpRotationMatrix R ;
  M.extract(R) ;
  M.extract(t) ;
  
  buildFrom(t, R) ;
  return (*this) ;
}

/*!
  compute the homography using a displacement matrix.

  the homography is given by:

  \f$ {}^cH_{c_0} = {}^cR_{c_0} + \frac{{}^cT_{c_0} . {}^tN}{d_0} \f$

  Several internal variables are computed (dt, cRc0_0n)

  \param _cTc0 : the displacement matrix of the camera between the initial position of the camera and the current camera position
  \param _cHc0 : the homography of the plane
*/
void
vpMbtDistanceKltPoints::computeHomography(const vpHomogeneousMatrix& _cTc0, vpHomography& _cHc0)
{
  vpRotationMatrix cRc0;
  vpTranslationVector ctransc0;

  _cTc0.extract(cRc0);
  _cTc0.extract(ctransc0);
  vpMatrix cHc0 = _cHc0.convert();

//   vpGEMM(cRc0, 1.0, invd0, cRc0, -1.0, _cHc0, VP_GEMM_A_T);
  vpGEMM(ctransc0, N, -invd0, cRc0, 1.0, cHc0, VP_GEMM_B_T);
  cHc0 /= cHc0[2][2];

  H = cHc0;

//   vpQuaternionVector NQuat(N[0], N[1], N[2], 0.0);
//   vpQuaternionVector RotQuat(cRc0);
//   vpQuaternionVector RotQuatConj(-RotQuat.x(), -RotQuat.y(), -RotQuat.z(), RotQuat.w());
//   vpQuaternionVector partial = RotQuat * NQuat;
//   vpQuaternionVector resQuat = (partial * RotQuatConj);
//
//   cRc0_0n = vpColVector(3);
//   cRc0_0n[0] = resQuat.x();
//   cRc0_0n[1] = resQuat.y();
//   cRc0_0n[2] = resQuat.z();

  cRc0_0n = cRc0*N;

//   vpPlane p(corners[0], corners[1], corners[2]);
//   vpColVector Ncur = p.getNormal();
//   Ncur.normalize();
  N_cur = cRc0_0n;
  dt = 0.0;
  for (unsigned int i = 0; i < 3; i += 1){
    dt += ctransc0[i] * (N_cur[i]);
  }
}

/*!
  Allow homogeneous matrix multiplication.

  \code
#include <visp/vpHomogeneousMatrix.h>

int main()
{
  vpHomogeneousMatrix aMb, bMc;
  // Initialize aMb and bMc...

  // Compute aMc * bMc
  vpHomogeneousMatrix aMc = aMb * bMc;  
}
  \endcode

*/
vpHomogeneousMatrix
vpHomogeneousMatrix::operator*(const vpHomogeneousMatrix &M) const
{
  vpHomogeneousMatrix p,p1 ;

  vpRotationMatrix R1, R2, R ;
  vpTranslationVector T1, T2 , T;


  extract(T1) ;
  M.extract(T2) ;

  extract (R1) ;
  M.extract (R2) ;

  R = R1*R2 ;

  T = R1*T2 + T1 ;

  p.insert(T) ;
  p.insert(R) ;

  return p;
}

  geometry_msgs::Transform toGeometryMsgsTransform(const vpHomogeneousMatrix& mat){
    geometry_msgs::Transform trans;
    vpQuaternionVector q;
    mat.extract(q);
    trans.rotation.x = q.x();
    trans.rotation.y = q.y();
    trans.rotation.z = q.z();
    trans.rotation.w = q.w();


    trans.translation.x = mat[0][3];
    trans.translation.y = mat[1][3];
    trans.translation.z = mat[2][3];

    return trans;
  }

/*!
\brief  Compute the pose using the Lowe non linear approach
it consider the minimization of a residual using
the levenberg marquartd approach.

The approach has been proposed by D.G Lowe in 1992 paper \cite Lowe92a.

*/
void
vpPose::poseLowe(vpHomogeneousMatrix & cMo)
{
#if (DEBUG_LEVEL1)
  std::cout << "begin CCalcuvpPose::PoseLowe(...) " << std::endl;
#endif
  int	n, m;	/* nombre d'elements dans la matrice jac */
  int	lwa;	/* taille du vecteur wa */
  int	ldfjac;	/* taille maximum d'une ligne de jac */
  int   info, ipvt[NBR_PAR];
  int	tst_lmder;
  double f[2 * NBPTMAX], sol[NBR_PAR];
  double	tol, jac[NBR_PAR][2 * NBPTMAX], wa[2 * NBPTMAX + 50];
  //  double	u[3];	/* vecteur de rotation */
  //  double	rd[3][3]; /* matrice de rotation */

  n = NBR_PAR;		/* nombres d'inconnues	*/
  m = (int)(2 * npt);		/* nombres d'equations	*/
  lwa = 2 * NBPTMAX + 50;  /* taille du vecteur de travail	*/
  ldfjac = 2 * NBPTMAX;	/* nombre d'elements max sur une ligne	*/
  tol = std::numeric_limits<double>::epsilon();		/* critere d'arret	*/

  //  c = cam ;
  // for (i=0;i<3;i++)
  //   for (j=0;j<3;j++) rd[i][j] = cMo[i][j];
  //  mat_rot(rd,u);
  vpRotationMatrix cRo ;
  cMo.extract(cRo) ;
  vpThetaUVector u(cRo) ;
  for (unsigned int i=0;i<3;i++)
  {
    sol[i] = cMo[i][3];
    sol[i+3] = u[i];
  }

  vpPoint P ;
  unsigned int i_=0;
  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
  {
    P = *it;
    XI[i_] = P.get_x();//*cam.px + cam.xc ;
    YI[i_] = P.get_y() ;//;*cam.py + cam.yc ;
    XO[i_] = P.get_oX();
    YO[i_] = P.get_oY();
    ZO[i_] = P.get_oZ();
    ++i_;
  }
  tst_lmder = lmder1 (&fcn, m, n, sol, f, &jac[0][0], ldfjac, tol, &info, ipvt, lwa, wa);
  if (tst_lmder == -1)
  {
    std::cout <<  " in CCalculPose::PoseLowe(...) : " ;
    std::cout << "pb de minimisation,  returns FATAL_ERROR";
    // return FATAL_ERROR ;
  }

  for (unsigned int i = 0; i < 3; i++)
    u[i] = sol[i + 3];

  for (unsigned int i=0;i<3;i++)
  {
    cMo[i][3] = sol[i];
    u[i] = sol[i+3];
  }

  vpRotationMatrix rd(u) ;
  cMo.insert(rd) ;
  //  rot_mat(u,rd);
  //  for (i=0;i<3;i++) for (j=0;j<3;j++) cMo[i][j] = rd[i][j];

#if (DEBUG_LEVEL1)
  std::cout << "end CCalculPose::PoseLowe(...) " << std::endl;
#endif
  //  return OK ;
}

double
vpHomography::computeDisplacement(unsigned int nbpoint,
                                  vpPoint *c1P,
                                  vpPoint *c2P,
                                  vpPlane *oN,
                                  vpHomogeneousMatrix &c2Mc1,
                                  vpHomogeneousMatrix &c1Mo,
                                  int userobust
                                 )
{


  vpColVector e(2) ;
  double r_1 = -1 ;



  vpColVector p2(3) ;
  vpColVector p1(3) ;
  vpColVector Hp2(3) ;
  vpColVector Hp1(3) ;

  vpMatrix H2(2,6) ;
  vpColVector e2(2) ;
  vpMatrix H1(2,6) ;
  vpColVector e1(2) ;


  int only_1 = 1 ;
  int only_2 = 0 ;
  int iter = 0 ;
  unsigned int i ;
  unsigned int n=0 ;
  n = nbpoint ;
  if ((only_1==1) || (only_2==1))  ; else n *=2 ;

  vpRobust robust(n);
  vpColVector res(n) ;
  vpColVector w(n) ;
  w =1 ;
  robust.setThreshold(0.0000) ;
  vpMatrix W(2*n,2*n)  ;
  W = 0 ;

  vpColVector N1(3), N2(3) ;
  double d1, d2 ;

  double r =1e10 ;
  iter =0 ;
  while (vpMath::equal(r_1,r,threshold_displacement) == false )
  {

    r_1 =r ;
    // compute current position


    //Change frame (current)
    vpHomogeneousMatrix c1Mc2, c2Mo ;
    vpRotationMatrix c1Rc2, c2Rc1  ;
    vpTranslationVector c1Tc2, c2Tc1 ;
    c1Mc2 = c2Mc1.inverse() ;
    c2Mc1.extract(c2Rc1) ;
    c2Mc1.extract(c2Tc1) ;
    c2Mc1.extract(c1Rc2) ;
    c1Mc2.extract(c1Tc2) ;

    c2Mo = c2Mc1*c1Mo ;



    vpMatrix L(2,3), Lp ;
    int k =0 ;
    for (i=0 ; i < nbpoint ; i++)
    {
      getPlaneInfo(oN[i], c1Mo, N1, d1) ;
      getPlaneInfo(oN[i], c2Mo, N2, d2) ;
      p2[0] = c2P[i].get_x() ;
      p2[1] = c2P[i].get_y() ;
      p2[2] = 1.0 ;
      p1[0] = c1P[i].get_x() ;
      p1[1] = c1P[i].get_y() ;
      p1[2] = 1.0 ;

      vpMatrix H(3,3) ;

      Hp2 = ((vpMatrix)c1Rc2 + ((vpMatrix)c1Tc2*N2.t())/d2)*p2 ;  // p2 = Hp1
      Hp1 = ((vpMatrix)c2Rc1 + ((vpMatrix)c2Tc1*N1.t())/d1)*p1 ;  // p1 = Hp2

      Hp2 /= Hp2[2] ;  // normalisation
      Hp1 /= Hp1[2] ;


      // set up the interaction matrix
      double x = Hp2[0] ;
      double y = Hp2[1] ;
      double Z1  ;

      Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ;        // 1/z


//.........这里部分代码省略.........

/*!

  Compute an instantaneous velocity from an homogeneous matrix. The inverse
  function is the exponential map, see direct().

  \param M : An homogeneous matrix corresponding to a displacement.

  \param delta_t : Sampling time \f$ \Delta t \f$. Time during which the
  displacement is applied.

  \return Instantaneous velocity \f$ \bf v \f$ represented by a 6 dimension
  vector \f$ [{\bf t}, {\bf \theta u}]^t \f$ where \f$ {\bf \theta u} \f$ is a
  rotation vector (see vpThetaUVector) and \f$ \bf t \f$ is a translation
  vector:  \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y, {\theta
  u}_z] \f$ (see vpTranslationVector and vpThetaUVector).

  \sa direct(const vpColVector &, const float &)
*/
vpColVector
vpExponentialMap::inverse(const vpHomogeneousMatrix &M, const double &delta_t)
{
  vpColVector v(6);
  unsigned int i;
  double theta,si,co,sinc,mcosc,msinc,det;
  vpThetaUVector u ;
  vpRotationMatrix Rd,a;
  vpTranslationVector dt ;

  M.extract(Rd);
  u.buildFrom(Rd);
  for (i=0;i<3;i++) v[3+i] = u[i];

  theta = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
  si = sin(theta);
  co = cos(theta);
  sinc  = vpMath::sinc(si,theta);
  mcosc = vpMath::mcosc(co,theta);
  msinc = vpMath::msinc(si,theta);

  // a below is not a pure rotation matrix, even if not so far from
  // the Rodrigues formula : sinc I + (1-sinc)/t^2 VV^T + (1-cos)/t^2 [V]_X
  // with V = t.U

  a[0][0] = sinc + u[0]*u[0]*msinc;
  a[0][1] = u[0]*u[1]*msinc - u[2]*mcosc;
  a[0][2] = u[0]*u[2]*msinc + u[1]*mcosc;

  a[1][0] = u[0]*u[1]*msinc + u[2]*mcosc;
  a[1][1] = sinc + u[1]*u[1]*msinc;
  a[1][2] = u[1]*u[2]*msinc - u[0]*mcosc;

  a[2][0] = u[0]*u[2]*msinc - u[1]*mcosc;
  a[2][1] = u[1]*u[2]*msinc + u[0]*mcosc;
  a[2][2] = sinc + u[2]*u[2]*msinc;

  det = a[0][0]*a[1][1]*a[2][2] + a[1][0]*a[2][1]*a[0][2]
      + a[0][1]*a[1][2]*a[2][0] - a[2][0]*a[1][1]*a[0][2]
      - a[1][0]*a[0][1]*a[2][2] - a[0][0]*a[2][1]*a[1][2];

  if (fabs(det) > 1.e-5)
  {
     v[0] =  (M[0][3]*a[1][1]*a[2][2]
           +   M[1][3]*a[2][1]*a[0][2]
           +   M[2][3]*a[0][1]*a[1][2]
           -   M[2][3]*a[1][1]*a[0][2]
           -   M[1][3]*a[0][1]*a[2][2]
           -   M[0][3]*a[2][1]*a[1][2])/det;
     v[1] =  (a[0][0]*M[1][3]*a[2][2]
           +   a[1][0]*M[2][3]*a[0][2]
           +   M[0][3]*a[1][2]*a[2][0]
           -   a[2][0]*M[1][3]*a[0][2]
           -   a[1][0]*M[0][3]*a[2][2]
           -   a[0][0]*M[2][3]*a[1][2])/det;
     v[2] =  (a[0][0]*a[1][1]*M[2][3]
           +   a[1][0]*a[2][1]*M[0][3]
           +   a[0][1]*M[1][3]*a[2][0]
           -   a[2][0]*a[1][1]*M[0][3]
           -   a[1][0]*a[0][1]*M[2][3]
           -   a[0][0]*a[2][1]*M[1][3])/det;
  }
  else
  {
     v[0] = M[0][3];
     v[1] = M[1][3];
     v[2] = M[2][3];
  }

  // Apply the sampling time to the computed velocity
  v /= delta_t;

  return(v);
}