vpHandEyeCalibration.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Hand-eye calibration.
 */
 
#include <cmath>  // std::fabs
#include <limits> // numeric_limits
 
#include <visp3/vision/vpHandEyeCalibration.h>
 
#define DEBUG_LEVEL1 0
#define DEBUG_LEVEL2 0
 
BEGIN_VISP_NAMESPACE
 
void vpHandEyeCalibration::calibrationVerifrMo(const std::vector<vpHomogeneousMatrix> &cMo,
                                               const std::vector<vpHomogeneousMatrix> &rMe,
                                               const vpHomogeneousMatrix &eMc)
{
  unsigned int nbPose = (unsigned int)cMo.size();
  std::vector<vpTranslationVector> rTo(nbPose);
  std::vector<vpRotationMatrix> rRo(nbPose);
 
  for (unsigned int i = 0; i < nbPose; i++) {
    vpHomogeneousMatrix rMo = rMe[i] * eMc * cMo[i];
    rRo[i] = rMo.getRotationMatrix();
    rTo[i] = rMo.getTranslationVector();
  }
  vpRotationMatrix meanRot = vpRotationMatrix::mean(rRo);
  vpTranslationVector meanTrans = vpTranslationVector::mean(rTo);
 
#if DEBUG_LEVEL2
  {
    std::cout << "Mean  " << std::endl;
    std::cout << "Translation: " << meanTrans.t() << std::endl;
    vpThetaUVector P(meanRot);
    std::cout << "Rotation : theta (deg) = " << vpMath::deg(sqrt(P.sumSquare())) << " Matrice : " << std::endl
      << meanRot << std::endl;
    std::cout << "theta U (deg): " << vpMath::deg(P[0]) << " " << vpMath::deg(P[1]) << " " << vpMath::deg(P[2])
      << std::endl;
  }
#endif
 
  // standard deviation, rotational part
  double resRot = 0.0;
  for (unsigned int i = 0; i < nbPose; i++) {
    vpRotationMatrix R = meanRot.t() * rRo[i]; // Rm^T  Ri
    vpThetaUVector P(R);
    // theta = Riemannian distance d(Rm,Ri)
    double theta = sqrt(P.sumSquare());
    std::cout << "Distance theta between rMo(" << i << ") and mean (deg) = " << vpMath::deg(theta) << std::endl;
    // Euclidean distance d(Rm,Ri) not used
    // theta = 2.0*sqrt(2.0)*sin(theta/2.0);
    resRot += theta * theta;
  }
  resRot = sqrt(resRot / nbPose);
  std::cout << "Mean residual rMo(" << nbPose << ") - rotation (deg) = " << vpMath::deg(resRot) << std::endl;
  // standard deviation, translational part
  double resTrans = 0.0;
  for (unsigned int i = 0; i < nbPose; i++) {
    vpColVector errTrans = vpColVector(rTo[i] - meanTrans);
    resTrans += errTrans.sumSquare();
    std::cout << "Distance d between rMo(" << i << ") and mean (m) = " << sqrt(errTrans.sumSquare()) << std::endl;
  }
  resTrans = sqrt(resTrans / nbPose);
  std::cout << "Mean residual rMo(" << nbPose << ") - translation (m) = " << resTrans << std::endl;
  double resPos = (resRot * resRot + resTrans * resTrans) * nbPose;
  resPos = sqrt(resPos / (2 * nbPose));
  std::cout << "Mean residual rMo(" << nbPose << ") - global = " << resPos << std::endl;
}
 
int vpHandEyeCalibration::calibrationRotationProcrustes(const std::vector<vpHomogeneousMatrix> &cMo,
                                                        const std::vector<vpHomogeneousMatrix> &rMe,
                                                        vpRotationMatrix &eRc)
{
  // Method by solving the orthogonal Procrustes problem
  // [... (theta u)_e ...] = eRc [ ... (theta u)_c ...]
  // similar to E^T = eRc C^T below
 
  vpMatrix Et, Ct;
  vpMatrix A;
  unsigned int k = 0;
  unsigned int nbPose = (unsigned int)cMo.size();
 
  // for all couples ij
  for (unsigned int i = 0; i < nbPose; i++) {
    vpRotationMatrix rRei, ciRo;
    rMe[i].extract(rRei);
    cMo[i].extract(ciRo);
    // std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
 
    for (unsigned int j = 0; j < nbPose; j++) {
      if (j > i) // we don't use two times same couples...
      {
        vpRotationMatrix rRej, cjRo;
        rMe[j].extract(rRej);
        cMo[j].extract(cjRo);
        // std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
 
        vpRotationMatrix ejRei = rRej.t() * rRei;
        vpThetaUVector ejPei(ejRei);
        vpColVector xe = vpColVector(ejPei);
 
        vpRotationMatrix cjRci = cjRo * ciRo.t();
        vpThetaUVector cjPci(cjRci);
        vpColVector xc = vpColVector(cjPci);
 
        if (k == 0) {
          Et = xe.t();
          Ct = xc.t();
        }
        else {
          Et.stack(xe.t());
          Ct.stack(xc.t());
        }
        k++;
      }
    }
  }
  // std::cout << "Et "  << std::endl << Et << std::endl;
  // std::cout << "Ct "  << std::endl << Ct << std::endl;
 
  // R obtained from the SVD of (E C^T) with all singular values equal to 1
  A = Et.t() * Ct;
  vpMatrix M, U, V;
  vpColVector sv;
  int rank = A.pseudoInverse(M, sv, 1e-6, U, V);
  if (rank != 3)
    return -1;
  A = U * V.t();
  eRc = vpRotationMatrix(A);
 
#if DEBUG_LEVEL2
  {
    vpThetaUVector ePc(eRc);
    std::cout << "Rotation from Procrustes method " << std::endl;
    std::cout << "theta U (deg): " << vpMath::deg(ePc[0]) << " " << vpMath::deg(ePc[1]) << " " << vpMath::deg(ePc[2])
      << std::endl;
    // Residual
    vpMatrix residual;
    residual = A * Ct.t() - Et.t();
    //  std::cout << "Residual: " << std::endl << residual << std::endl;
    double res = sqrt(residual.sumSquare() / (residual.getRows() * residual.getCols()));
    printf("Mean residual (rotation) = %lf\n", res);
  }
#endif
  return 0;
}
 
int vpHandEyeCalibration::calibrationRotationTsai(const std::vector<vpHomogeneousMatrix> &cMo,
                                                  const std::vector<vpHomogeneousMatrix> &rMe, vpRotationMatrix &eRc)
{
  vpMatrix A;
  vpColVector B;
  unsigned int nbPose = (unsigned int)cMo.size();
  unsigned int k = 0;
  // for all couples ij
  for (unsigned int i = 0; i < nbPose; i++) {
    vpRotationMatrix rRei, ciRo;
    rMe[i].extract(rRei);
    cMo[i].extract(ciRo);
    // std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
 
    for (unsigned int j = 0; j < nbPose; j++) {
      if (j > i) // we don't use two times same couples...
      {
        vpRotationMatrix rRej, cjRo;
        rMe[j].extract(rRej);
        cMo[j].extract(cjRo);
        // std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
 
        vpRotationMatrix ejRei = rRej.t() * rRei;
        vpThetaUVector ejPei(ejRei);
 
        vpRotationMatrix cjRci = cjRo * ciRo.t();
        vpThetaUVector cjPci(cjRci);
        // std::cout << "theta U (camera) " << cjPci.t() << std::endl;
 
        vpMatrix As;
        vpColVector b(3);
 
        As = vpColVector::skew(vpColVector(ejPei) + vpColVector(cjPci));
 
        b = (vpColVector)cjPci - (vpColVector)ejPei; // A.40
 
        if (k == 0) {
          A = As;
          B = b;
        }
        else {
          A = vpMatrix::stack(A, As);
          B = vpColVector::stack(B, b);
        }
        k++;
      }
    }
  }
#if DEBUG_LEVEL2
  {
    std::cout << "Tsai method: system A X = B " << std::endl;
    std::cout << "A " << std::endl << A << std::endl;
    std::cout << "B " << std::endl << B << std::endl;
  }
#endif
  vpMatrix Ap;
  // the linear system A x = B is solved
  // using x = A^+ B
 
  int rank = A.pseudoInverse(Ap);
  if (rank != 3)
    return -1;
 
  vpColVector x = Ap * B;
 
  // extraction of theta U
 
  // x = tan(theta/2) U
  double norm = x.sumSquare();
  double c = 1 / sqrt(1 + norm);        // cos(theta/2)
  double alpha = acos(c);               // theta/2
  norm = 2.0 * c / vpMath::sinc(alpha); // theta / tan(theta/2)
  for (unsigned int i = 0; i < 3; i++)
    x[i] *= norm;
 
  // Building of the rotation matrix eRc
  vpThetaUVector xP(x[0], x[1], x[2]);
  eRc = vpRotationMatrix(xP);
 
#if DEBUG_LEVEL2
  {
    std::cout << "Rotation from Tsai method" << std::endl;
    std::cout << "theta U (deg): " << vpMath::deg(x[0]) << " " << vpMath::deg(x[1]) << " " << vpMath::deg(x[2])
      << std::endl;
    // Residual
    for (unsigned int i = 0; i < 3; i++)
      x[i] /= norm; /* original x */
    vpColVector residual;
    residual = A * x - B;
    // std::cout << "Residual: " << std::endl << residual << std::endl;
    double res = sqrt(residual.sumSquare() / residual.getRows());
    printf("Mean residual (rotation) = %lf\n", res);
  }
#endif
  return 0;
}
 
int vpHandEyeCalibration::calibrationRotationTsaiOld(const std::vector<vpHomogeneousMatrix> &cMo,
                                                     const std::vector<vpHomogeneousMatrix> &rMe, vpRotationMatrix &eRc)
{
  unsigned int nbPose = (unsigned int)cMo.size();
  vpMatrix A;
  vpColVector B;
  vpColVector x;
  unsigned int k = 0;
  // for all couples ij
  for (unsigned int i = 0; i < nbPose; i++) {
    vpRotationMatrix rRei, ciRo;
    rMe[i].extract(rRei);
    cMo[i].extract(ciRo);
    // std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
 
    for (unsigned int j = 0; j < nbPose; j++) {
      if (j > i) { // we don't use two times same couples...
        vpRotationMatrix rRej, cjRo;
        rMe[j].extract(rRej);
        cMo[j].extract(cjRo);
        // std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
 
        vpRotationMatrix rReij = rRej.t() * rRei;
 
        vpRotationMatrix cijRo = cjRo * ciRo.t();
 
        vpThetaUVector rPeij(rReij);
 
        double theta = sqrt(rPeij[0] * rPeij[0] + rPeij[1] * rPeij[1] + rPeij[2] * rPeij[2]);
 
        // std::cout << i << " " << j << " " << "ejRei: " << std::endl << rReij << std::endl;
        // std::cout << "theta (robot) " << theta << std::endl;
        // std::cout << "theta U (robot) " << rPeij << std::endl;
        // std::cout << "cjRci: " << std::endl << cijRo.t() << std::endl;
 
        for (unsigned int m = 0; m < 3; m++) {
          rPeij[m] = rPeij[m] * vpMath::sinc(theta / 2);
        }
 
        vpThetaUVector cijPo(cijRo);
        theta = sqrt(cijPo[0] * cijPo[0] + cijPo[1] * cijPo[1] + cijPo[2] * cijPo[2]);
        for (unsigned int m = 0; m < 3; m++) {
          cijPo[m] = cijPo[m] * vpMath::sinc(theta / 2);
        }
 
        // std::cout << "theta (camera) " << theta << std::endl;
        // std::cout << "theta U (camera) " << cijPo.t() << std::endl;
 
        vpMatrix As;
        vpColVector b(3);
 
        As = vpColVector::skew(vpColVector(rPeij) + vpColVector(cijPo));
 
        b = (vpColVector)cijPo - (vpColVector)rPeij; // A.40
 
        if (k == 0) {
          A = As;
          B = b;
        }
        else {
          A = vpMatrix::stack(A, As);
          B = vpColVector::stack(B, b);
        }
        k++;
      }
    }
  }
 
  // std::cout << "A "  << std::endl << A << std::endl;
  // std::cout << "B "  << std::endl << B << std::endl;
 
  // the linear system is defined
  // x = AtA^-1AtB is solved
  vpMatrix AtA = A.AtA();
 
  vpMatrix Ap;
  int rank = AtA.pseudoInverse(Ap, 1e-6);
  if (rank != 3)
    return -1;
 
  x = Ap * A.t() * B;
  vpColVector x2 = x; /* pour calcul residu */
 
  // extraction of theta and U
  double theta;
  double d = x.sumSquare();
  for (unsigned int i = 0; i < 3; i++)
    x[i] = 2 * x[i] / sqrt(1 + d);
  theta = sqrt(x.sumSquare()) / 2;
  theta = 2 * asin(theta);
  // if (theta !=0)
  if (std::fabs(theta) > std::numeric_limits<double>::epsilon()) {
    for (unsigned int i = 0; i < 3; i++)
      x[i] *= theta / (2 * sin(theta / 2));
  }
  else
    x = 0;
 
  // Building of the rotation matrix eRc
  vpThetaUVector xP(x[0], x[1], x[2]);
  eRc = vpRotationMatrix(xP);
 
#if DEBUG_LEVEL2
  {
    std::cout << "Rotation from Old Tsai method" << std::endl;
    std::cout << "theta U (deg): " << vpMath::deg(x[0]) << " " << vpMath::deg(x[1]) << " " << vpMath::deg(x[2])
      << std::endl;
    // Residual
    vpColVector residual;
    residual = A * x2 - B;
    // std::cout << "Residual: " << std::endl << residual << std::endl;
    double res = sqrt(residual.sumSquare() / residual.getRows());
    printf("Mean residual (rotation) = %lf\n", res);
  }
#endif
  return 0;
}
 
int vpHandEyeCalibration::calibrationTranslation(const std::vector<vpHomogeneousMatrix> &cMo,
                                                 const std::vector<vpHomogeneousMatrix> &rMe, vpRotationMatrix &eRc,
                                                 vpTranslationVector &eTc)
{
  vpMatrix I3(3, 3);
  I3.eye();
  unsigned int k = 0;
  unsigned int nbPose = (unsigned int)cMo.size();
  vpMatrix A(3 * nbPose, 3);
  vpColVector B(3 * nbPose);
  // Building of the system for the translation estimation
  // for all couples ij
  for (unsigned int i = 0; i < nbPose; i++) {
    for (unsigned int j = 0; j < nbPose; j++) {
      if (j > i) { // we don't use two times same couples...
        vpHomogeneousMatrix ejMei = rMe[j].inverse() * rMe[i];
        vpHomogeneousMatrix cjMci = cMo[j] * cMo[i].inverse();
 
        vpRotationMatrix ejRei, cjRci;
        vpTranslationVector ejTei, cjTci;
 
        ejMei.extract(ejRei);
        ejMei.extract(ejTei);
 
        cjMci.extract(cjRci);
        cjMci.extract(cjTci);
 
        vpMatrix a = vpMatrix(ejRei) - I3;
        vpTranslationVector b = eRc * cjTci - ejTei;
 
        if (k == 0) {
          A = a;
          B = b;
        }
        else {
          A = vpMatrix::stack(A, a);
          B = vpColVector::stack(B, vpColVector(b));
        }
        k++;
      }
    }
  }
 
  // the linear system A x = B is solved
  // using x = A^+ B
  vpMatrix Ap;
  int rank = A.pseudoInverse(Ap);
  if (rank != 3)
    return -1;
 
  vpColVector x = Ap * B;
  eTc = (vpTranslationVector)x;
 
#if DEBUG_LEVEL2
  {
    printf("New Hand-eye calibration : ");
    std::cout << "Translation: " << eTc[0] << " " << eTc[1] << " " << eTc[2] << std::endl;
    // residual
    vpColVector residual;
    residual = A * x - B;
    // std::cout << "Residual: " << std::endl << residual << std::endl;
    double res = sqrt(residual.sumSquare() / residual.getRows());
    printf("Mean residual (translation) = %lf\n", res);
  }
#endif
  return 0;
}
 
int vpHandEyeCalibration::calibrationTranslationOld(const std::vector<vpHomogeneousMatrix> &cMo,
                                                    const std::vector<vpHomogeneousMatrix> &rMe, vpRotationMatrix &eRc,
                                                    vpTranslationVector &eTc)
{
  vpMatrix A;
  vpColVector B;
  // Building of the system for the translation estimation
  // for all couples ij
  vpRotationMatrix I3;
  I3.eye();
  int k = 0;
  unsigned int nbPose = (unsigned int)cMo.size();
 
  for (unsigned int i = 0; i < nbPose; i++) {
    vpRotationMatrix rRei, ciRo;
    vpTranslationVector rTei, ciTo;
    rMe[i].extract(rRei);
    cMo[i].extract(ciRo);
    rMe[i].extract(rTei);
    cMo[i].extract(ciTo);
 
    for (unsigned int j = 0; j < nbPose; j++) {
      if (j > i) // we don't use two times same couples...
      {
 
        vpRotationMatrix rRej, cjRo;
        rMe[j].extract(rRej);
        cMo[j].extract(cjRo);
 
        vpTranslationVector rTej, cjTo;
        rMe[j].extract(rTej);
        cMo[j].extract(cjTo);
 
        vpRotationMatrix rReij = rRej.t() * rRei;
 
        vpTranslationVector rTeij = rTej + (-rTei);
 
        rTeij = rRej.t() * rTeij;
 
        vpMatrix a = vpMatrix(rReij) - vpMatrix(I3);
 
        vpTranslationVector b;
        b = eRc * cjTo - rReij * eRc * ciTo + rTeij;
 
        if (k == 0) {
          A = a;
          B = b;
        }
        else {
          A = vpMatrix::stack(A, a);
          B = vpColVector::stack(B, vpColVector(b));
        }
        k++;
      }
    }
  }
 
  // the linear system is solved
  // x = AtA^-1AtB is solved
  vpMatrix AtA = A.AtA();
  vpMatrix Ap;
  vpColVector AeTc;
  int rank = AtA.pseudoInverse(Ap, 1e-6);
  if (rank != 3)
    return -1;
 
  AeTc = Ap * A.t() * B;
  eTc = (vpTranslationVector)AeTc;
 
#if DEBUG_LEVEL2
  {
    printf("Old Hand-eye calibration : ");
    std::cout << "Translation: " << eTc[0] << " " << eTc[1] << " " << eTc[2] << std::endl;
 
    // residual
    vpColVector residual;
    residual = A * AeTc - B;
    // std::cout << "Residual: " << std::endl << residual << std::endl;
    double res = 0;
    for (unsigned int i = 0; i < residual.getRows(); i++)
      res += residual[i] * residual[i];
    res = sqrt(res / residual.getRows());
    printf("Mean residual (translation) = %lf\n", res);
  }
#endif
  return 0;
}
 
double vpHandEyeCalibration::calibrationErrVVS(const std::vector<vpHomogeneousMatrix> &cMo,
                                               const std::vector<vpHomogeneousMatrix> &rMe,
                                               const vpHomogeneousMatrix &eMc, vpColVector &errVVS)
{
  unsigned int nbPose = (unsigned int)cMo.size();
  vpMatrix I3(3, 3);
  I3.eye();
  vpRotationMatrix eRc;
  vpTranslationVector eTc;
  eMc.extract(eRc);
  eMc.extract(eTc);
 
  unsigned int k = 0;
  for (unsigned int i = 0; i < nbPose; i++) {
    for (unsigned int j = 0; j < nbPose; j++) {
      if (j > i) // we don't use two times same couples...
      {
        vpColVector s(3);
 
        vpHomogeneousMatrix ejMei = rMe[j].inverse() * rMe[i];
        vpHomogeneousMatrix cjMci = cMo[j] * cMo[i].inverse();
 
        vpRotationMatrix ejRei, cjRci;
        vpTranslationVector ejTei, cjTci;
 
        ejMei.extract(ejRei);
        vpThetaUVector ejPei(ejRei);
        ejMei.extract(ejTei);
 
        cjMci.extract(cjRci);
        vpThetaUVector cjPci(cjRci);
        cjMci.extract(cjTci);
        // terms due to rotation
        s = vpMatrix(eRc) * vpColVector(cjPci) - vpColVector(ejPei);
        if (k == 0) {
          errVVS = s;
        }
        else {
          errVVS = vpColVector::stack(errVVS, s);
        }
        k++;
        // terms due to translation
        s = (vpMatrix(ejRei) - I3) * eTc - eRc * cjTci + ejTei;
        errVVS = vpColVector::stack(errVVS, s);
      } // enf if i > j
    }   // end for j
  }     // end for i
 
  double resRot, resTrans, resPos;
  resRot = resTrans = resPos = 0.0;
  for (unsigned int i = 0; i < (unsigned int)errVVS.size(); i += 6) {
    resRot += errVVS[i] * errVVS[i];
    resRot += errVVS[i + 1] * errVVS[i + 1];
    resRot += errVVS[i + 2] * errVVS[i + 2];
    resTrans += errVVS[i + 3] * errVVS[i + 3];
    resTrans += errVVS[i + 4] * errVVS[i + 4];
    resTrans += errVVS[i + 5] * errVVS[i + 5];
  }
  resPos = resRot + resTrans;
  resRot = sqrt(resRot * 2 / errVVS.size());
  resTrans = sqrt(resTrans * 2 / errVVS.size());
  resPos = sqrt(resPos / errVVS.size());
#if DEBUG_LEVEL1
  {
    printf("Mean VVS residual - rotation (deg) = %lf\n", vpMath::deg(resRot));
    printf("Mean VVS residual - translation = %lf\n", resTrans);
    printf("Mean VVS residual - global = %lf\n", resPos);
  }
#endif
  return resPos;
}
 
#define NB_ITER_MAX 30
 
int vpHandEyeCalibration::calibrationVVS(const std::vector<vpHomogeneousMatrix> &cMo,
                                         const std::vector<vpHomogeneousMatrix> &rMe, vpHomogeneousMatrix &eMc)
{
  unsigned int it = 0;
  double res = 1.0;
  unsigned int nbPose = (unsigned int)cMo.size();
  vpColVector err;
  vpMatrix L;
  vpMatrix I3(3, 3);
  I3.eye();
  vpRotationMatrix eRc;
  vpTranslationVector eTc;
  eMc.extract(eRc);
  eMc.extract(eTc);
 
  /* FC : on recalcule 2 fois tous les ejMei et cjMci a chaque iteration
    alors qu'ils sont constants. Ce serait sans doute mieux de les
    calculer une seule fois et de les stocker. Pourraient alors servir
    dans les autres fonctions HandEye. A voir si vraiment interessant vu la
    combinatoire. Idem pour les theta u */
  while ((res > 1e-7) && (it < NB_ITER_MAX)) {
    /* compute s - s^* */
    vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, err);
    /* compute L_s */
    unsigned int k = 0;
    for (unsigned int i = 0; i < nbPose; i++) {
      for (unsigned int j = 0; j < nbPose; j++) {
        if (j > i) // we don't use two times same couples...
        {
          vpMatrix Ls(3, 6), Lv(3, 3), Lw(3, 3);
 
          vpHomogeneousMatrix ejMei = rMe[j].inverse() * rMe[i];
          vpHomogeneousMatrix cjMci = cMo[j] * cMo[i].inverse();
 
          vpRotationMatrix ejRei;
          ejMei.extract(ejRei);
          vpThetaUVector cjPci(cjMci);
 
          vpTranslationVector cjTci;
 
          cjMci.extract(cjTci);
          // terms due to rotation
          // Lv.diag(0.0); //
          Lv = 0.0;
          Lw = -vpMatrix(eRc) * vpColVector::skew(vpColVector(cjPci));
          for (unsigned int m = 0; m < 3; m++)
            for (unsigned int n = 0; n < 3; n++) {
              Ls[m][n] = Lv[m][n];
              Ls[m][n + 3] = Lw[m][n];
            }
          if (k == 0) {
            L = Ls;
          }
          else {
            L = vpMatrix::stack(L, Ls);
          }
          k++;
          // terms due to translation
          Lv = (vpMatrix(ejRei) - I3) * vpMatrix(eRc);
          Lw = vpMatrix(eRc) * vpColVector::skew(vpColVector(cjTci));
          for (unsigned int m = 0; m < 3; m++)
            for (unsigned int n = 0; n < 3; n++) {
              Ls[m][n] = Lv[m][n];
              Ls[m][n + 3] = Lw[m][n];
            }
          L = vpMatrix::stack(L, Ls);
 
        } // enf if i > j
      }   // end for j
    }     // end for i
    double lambda = 0.9;
    vpMatrix Lp;
    int rank = L.pseudoInverse(Lp);
    if (rank != 6)
      return -1;
 
    vpColVector e = Lp * err;
    vpColVector v = -e * lambda;
    //  std::cout << "e: "  << e.t() << std::endl;
    eMc = eMc * vpExponentialMap::direct(v);
    eMc.extract(eRc);
    eMc.extract(eTc);
    res = sqrt(v.sumSquare() / v.getRows());
    it++;
  } // end while
#if DEBUG_LEVEL2
  {
    printf(" Iteration number for NL hand-eye minimization : %d\n", it);
    vpThetaUVector ePc(eRc);
    std::cout << "theta U (deg): " << vpMath::deg(ePc[0]) << " " << vpMath::deg(ePc[1]) << " " << vpMath::deg(ePc[2])
      << std::endl;
    std::cout << "Translation: " << eTc[0] << " " << eTc[1] << " " << eTc[2] << std::endl;
    // Residual
    double res = err.sumSquare();
    res = sqrt(res / err.getRows());
    printf("Mean residual (rotation+translation) = %lf\n", res);
  }
#endif
  if (it == NB_ITER_MAX)
    return 1; // VVS has not converged before NB_ITER_MAX
  else
    return 0;
}
 
#undef NB_ITER_MAX
 
#define HE_I 0
#define HE_TSAI_OROT 1
#define HE_TSAI_ORNT 2
#define HE_TSAI_NROT 3
#define HE_TSAI_NRNT 4
#define HE_PROCRUSTES_OT 5
#define HE_PROCRUSTES_NT 6
 
int vpHandEyeCalibration::calibrate(const std::vector<vpHomogeneousMatrix> &cMo,
                                    const std::vector<vpHomogeneousMatrix> &rMe, vpHomogeneousMatrix &eMc)
{
  if (cMo.size() != rMe.size())
    throw vpException(vpException::dimensionError, "cMo and rMe have different sizes");
 
  vpRotationMatrix eRc;
  vpTranslationVector eTc;
  vpColVector errVVS;
  double resPos;
 
  /* initialisation of eMc to I in case all other methods fail */
  eMc.eye();
  resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
  double vmin = resPos; // will serve to determine the best method
#if DEBUG_LEVEL1
  int He_method = HE_I; // will serve to know which is the best method
#endif
  vpHomogeneousMatrix eMcMin = eMc; // best initial estimation for VSS
  // Method using Old Tsai implementation
  int err = vpHandEyeCalibration::calibrationRotationTsaiOld(cMo, rMe, eRc);
  if (err != 0)
    printf("\n Problem in solving Hand-Eye Rotation by Old Tsai method \n");
  else {
    eMc.insert(eRc);
    err = vpHandEyeCalibration::calibrationTranslationOld(cMo, rMe, eRc, eTc);
    if (err != 0)
      printf("\n Problem in solving Hand-Eye Translation by Old Tsai method after Old Tsai method for Rotation\n");
    else {
      eMc.insert(eTc);
#if DEBUG_LEVEL1
      {
        printf("\nRotation by (old) Tsai, old implementation for translation\n");
        vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
      }
#endif
      resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
      if (resPos < vmin) {
        vmin = resPos;
        eMcMin = eMc;
#if DEBUG_LEVEL1
        He_method = HE_TSAI_OROT;
#endif
      }
    }
    err = vpHandEyeCalibration::calibrationTranslation(cMo, rMe, eRc, eTc);
    if (err != 0)
      printf("\n Problem in solving Hand-Eye Translation after Old Tsai method for Rotation\n");
    else {
      eMc.insert(eTc);
#if DEBUG_LEVEL1
      {
        printf("\nRotation by (old) Tsai, new implementation for translation\n");
        vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
      }
#endif
      resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
      if (resPos < vmin) {
        vmin = resPos;
        eMcMin = eMc;
#if DEBUG_LEVEL1
        He_method = HE_TSAI_ORNT;
#endif
      }
    }
  }
  // First method using Tsai formulation
  err = vpHandEyeCalibration::calibrationRotationTsai(cMo, rMe, eRc);
  if (err != 0)
    printf("\n Problem in solving Hand-Eye Rotation by Tsai method \n");
  else {
    eMc.insert(eRc);
    err = vpHandEyeCalibration::calibrationTranslationOld(cMo, rMe, eRc, eTc);
    if (err != 0)
      printf("\n Problem in solving Hand-Eye Translation by Old Tsai method after Tsai method for Rotation\n");
    else {
      eMc.insert(eTc);
#if DEBUG_LEVEL1
      {
        printf("\nRotation by Tsai, old implementation for translation\n");
        vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
      }
#endif
      resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
      if (resPos < vmin) {
        vmin = resPos;
        eMcMin = eMc;
#if DEBUG_LEVEL1
        He_method = HE_TSAI_NROT;
#endif
      }
    }
    err = vpHandEyeCalibration::calibrationTranslation(cMo, rMe, eRc, eTc);
    if (err != 0)
      printf("\n Problem in solving Hand-Eye Translation after Tsai method for Rotation \n");
    else {
      eMc.insert(eTc);
#if DEBUG_LEVEL1
      {
        printf("\nRotation by Tsai, new implementation for translation\n");
        vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
      }
#endif
      resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
      if (resPos < vmin) {
        vmin = resPos;
        eMcMin = eMc;
#if DEBUG_LEVEL1
        He_method = HE_TSAI_NRNT;
#endif
      }
    }
  }
  // Second method by solving the orthogonal Procrustes problem
  err = vpHandEyeCalibration::calibrationRotationProcrustes(cMo, rMe, eRc);
  if (err != 0)
    printf("\n Problem in solving Hand-Eye Rotation by Procrustes method \n");
  else {
    eMc.insert(eRc);
    err = vpHandEyeCalibration::calibrationTranslationOld(cMo, rMe, eRc, eTc);
    if (err != 0)
      printf("\n Problem in solving Hand-Eye Translation by Old Tsai method after Procrustes method for Rotation\n");
    else {
      eMc.insert(eTc);
#if DEBUG_LEVEL1
      {
        printf("\nRotation by Procrustes, old implementation for translation\n");
        vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
      }
#endif
      resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
      if (resPos < vmin) {
        vmin = resPos;
        eMcMin = eMc;
#if DEBUG_LEVEL1
        He_method = HE_PROCRUSTES_OT;
#endif
      }
    }
    err = vpHandEyeCalibration::calibrationTranslation(cMo, rMe, eRc, eTc);
    if (err != 0)
      printf("\n Problem in solving Hand-Eye Translation after Procrustes method for Rotation\n");
    else {
      eMc.insert(eTc);
#if DEBUG_LEVEL1
      {
        printf("\nRotation by Procrustes, new implementation for translation\n");
        vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
      }
#endif
      resPos = vpHandEyeCalibration::calibrationErrVVS(cMo, rMe, eMc, errVVS);
      if (resPos < vmin) {
        eMcMin = eMc;
#if DEBUG_LEVEL1
        vmin = resPos;
        He_method = HE_PROCRUSTES_NT;
#endif
      }
    }
  }
 
  /* determination of the best method in case at least one succeeds */
  eMc = eMcMin;
#if DEBUG_LEVEL1
  {
    if (He_method == HE_I)
      printf("Best method : I !!!, vmin = %lf\n", vmin);
    if (He_method == HE_TSAI_OROT)
      printf("Best method : TSAI_OROT, vmin = %lf\n", vmin);
    if (He_method == HE_TSAI_ORNT)
      printf("Best method : TSAI_ORNT, vmin = %lf\n", vmin);
    if (He_method == HE_TSAI_NROT)
      printf("Best method : TSAI_NROT, vmin = %lf\n", vmin);
    if (He_method == HE_TSAI_NRNT)
      printf("Best method : TSAI_NRNT, vmin = %lf\n", vmin);
    if (He_method == HE_PROCRUSTES_OT)
      printf("Best method : PROCRUSTES_OT, vmin = %lf\n", vmin);
    if (He_method == HE_PROCRUSTES_NT)
      printf("Best method : PROCRUSTES_NT, vmin = %lf\n", vmin);
    vpThetaUVector ePc(eMc);
    std::cout << "theta U (deg): " << vpMath::deg(ePc[0]) << " " << vpMath::deg(ePc[1]) << " " << vpMath::deg(ePc[2])
      << std::endl;
    std::cout << "Translation: " << eMc[0][3] << " " << eMc[1][3] << " " << eMc[2][3] << std::endl;
  }
#endif
 
  // Non linear iterative minimization to estimate simultaneouslty eRc and eTc
  err = vpHandEyeCalibration::calibrationVVS(cMo, rMe, eMc);
  // FC : err : 0 si tout OK, -1 si pb de rang, 1 si pas convergence
  if (err != 0)
    printf("\n Problem in solving Hand-Eye Calibration by VVS \n");
  else {
    printf("\nRotation and translation after VVS\n");
    vpHandEyeCalibration::calibrationVerifrMo(cMo, rMe, eMc);
  }
  return err;
}
 
END_VISP_NAMESPACE
 
#undef HE_I
#undef HE_TSAI_OROT
#undef HE_TSAI_ORNT
#undef HE_TSAI_NROT
#undef HE_TSAI_NRNT
#undef HE_PROCRUSTES_OT
#undef HE_PROCRUSTES_NT
 
#undef DEBUG_LEVEL1
#undef DEBUG_LEVEL2