Skip to content

Commit

Permalink
Fix since vpColvector constructors are explicit
Browse files Browse the repository at this point in the history
  • Loading branch information
@fspindle
fspindle committed May 13, 2024
1 parent 59e51bb commit 89f9f89
Showing 1 changed file with 28 additions and 20 deletions.
48 changes: 28 additions & 20 deletions modules/vision/src/calibration/vpHandEyeCalibration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,9 +71,9 @@ void vpHandEyeCalibration::calibrationVerifrMo(const std::vector<vpHomogeneousMa
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;
<< meanRot << std::endl;
std::cout << "theta U (deg): " << vpMath::deg(P[0]) << " " << vpMath::deg(P[1]) << " " << vpMath::deg(P[2])
<< std::endl;
<< std::endl;
}
#endif

Expand All @@ -94,7 +94,7 @@ void vpHandEyeCalibration::calibrationVerifrMo(const std::vector<vpHomogeneousMa
// standard deviation, translational part
double resTrans = 0.0;
for (unsigned int i = 0; i < nbPose; i++) {
vpColVector errTrans = ((vpColVector)rTo[i]) - meanTrans;
vpColVector errTrans = vpColVector(rTo[i] - meanTrans);
resTrans += errTrans.sumSquare();
std::cout << "Distance d between rMo(" << i << ") and mean (m) = " << sqrt(errTrans.sumSquare()) << std::endl;
}
Expand Down Expand Up @@ -146,16 +146,17 @@ int vpHandEyeCalibration::calibrationRotationProcrustes(const std::vector<vpHomo

vpRotationMatrix ejRei = rRej.t() * rRei;
vpThetaUVector ejPei(ejRei);
vpColVector xe = ejPei;
vpColVector xe = vpColVector(ejPei);

vpRotationMatrix cjRci = cjRo * ciRo.t();
vpThetaUVector cjPci(cjRci);
vpColVector xc = cjPci;
vpColVector xc = vpColVector(cjPci);

if (k == 0) {
Et = xe.t();
Ct = xc.t();
} else {
}
else {
Et.stack(xe.t());
Ct.stack(xc.t());
}
Expand All @@ -181,7 +182,7 @@ int vpHandEyeCalibration::calibrationRotationProcrustes(const std::vector<vpHomo
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;
<< std::endl;
// Residual
vpMatrix residual;
residual = A * Ct.t() - Et.t();
Expand Down Expand Up @@ -244,7 +245,8 @@ int vpHandEyeCalibration::calibrationRotationTsai(const std::vector<vpHomogeneou
if (k == 0) {
A = As;
B = b;
} else {
}
else {
A = vpMatrix::stack(A, As);
B = vpColVector::stack(B, b);
}
Expand Down Expand Up @@ -287,7 +289,7 @@ int vpHandEyeCalibration::calibrationRotationTsai(const std::vector<vpHomogeneou
{
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;
<< std::endl;
// Residual
for (unsigned int i = 0; i < 3; i++)
x[i] /= norm; /* original x */
Expand Down Expand Up @@ -371,7 +373,8 @@ int vpHandEyeCalibration::calibrationRotationTsaiOld(const std::vector<vpHomogen
if (k == 0) {
A = As;
B = b;
} else {
}
else {
A = vpMatrix::stack(A, As);
B = vpColVector::stack(B, b);
}
Expand Down Expand Up @@ -406,7 +409,8 @@ int vpHandEyeCalibration::calibrationRotationTsaiOld(const std::vector<vpHomogen
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
}
else
x = 0;

// Building of the rotation matrix eRc
Expand All @@ -417,7 +421,7 @@ int vpHandEyeCalibration::calibrationRotationTsaiOld(const std::vector<vpHomogen
{
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;
<< std::endl;
// Residual
vpColVector residual;
residual = A * x2 - B;
Expand Down Expand Up @@ -475,9 +479,10 @@ int vpHandEyeCalibration::calibrationTranslation(const std::vector<vpHomogeneous
if (k == 0) {
A = a;
B = b;
} else {
}
else {
A = vpMatrix::stack(A, a);
B = vpColVector::stack(B, b);
B = vpColVector::stack(B, vpColVector(b));
}
k++;
}
Expand Down Expand Up @@ -569,9 +574,10 @@ int vpHandEyeCalibration::calibrationTranslationOld(const std::vector<vpHomogene
if (k == 0) {
A = a;
B = b;
} else {
}
else {
A = vpMatrix::stack(A, a);
B = vpColVector::stack(B, b);
B = vpColVector::stack(B, vpColVector(b));
}
k++;
}
Expand Down Expand Up @@ -657,7 +663,8 @@ double vpHandEyeCalibration::calibrationErrVVS(const std::vector<vpHomogeneousMa
s = vpMatrix(eRc) * vpColVector(cjPci) - vpColVector(ejPei);
if (k == 0) {
errVVS = s;
} else {
}
else {
errVVS = vpColVector::stack(errVVS, s);
}
k++;
Expand Down Expand Up @@ -756,7 +763,8 @@ int vpHandEyeCalibration::calibrationVVS(const std::vector<vpHomogeneousMatrix>
}
if (k == 0) {
L = Ls;
} else {
}
else {
L = vpMatrix::stack(L, Ls);
}
k++;
Expand Down Expand Up @@ -793,7 +801,7 @@ int vpHandEyeCalibration::calibrationVVS(const std::vector<vpHomogeneousMatrix>
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::endl;
std::cout << "Translation: " << eTc[0] << " " << eTc[1] << " " << eTc[2] << std::endl;
// Residual
double res = err.sumSquare();
Expand Down Expand Up @@ -998,7 +1006,7 @@ int vpHandEyeCalibration::calibrate(const std::vector<vpHomogeneousMatrix> &cMo,
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::endl;
std::cout << "Translation: " << eMc[0][3] << " " << eMc[1][3] << " " << eMc[2][3] << std::endl;
}
#endif
Expand Down

0 comments on commit 89f9f89

Please sign in to comment.