diff --git a/modules/core/src/math/matrix/vpMatrix.cpp b/modules/core/src/math/matrix/vpMatrix.cpp index 74a8f69791..87b7cfbca6 100644 --- a/modules/core/src/math/matrix/vpMatrix.cpp +++ b/modules/core/src/math/matrix/vpMatrix.cpp @@ -30,8 +30,8 @@ * * Description: * Matrix manipulation. - * -*****************************************************************************/ + */ + /*! \file vpMatrix.cpp \brief Definition of the vpMatrix class @@ -126,7 +126,7 @@ void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMa rank_out = 0; - for (unsigned int i = 0; i < ncols; i++) { + for (unsigned int i = 0; i < sv.size(); i++) { if (sv[i] > maxsv * svThreshold) { rank_out++; } @@ -862,7 +862,7 @@ void vpMatrix::diag(const vpColVector &A) /*! Set the matrix as a diagonal matrix where each element on the diagonal is -set to \e val. Elements that are not on the diagonal are set to 0. + set to \e val. Elements that are not on the diagonal are set to 0. \param val : Value to set. @@ -899,7 +899,6 @@ void vpMatrix::diag(const double &val) } /*! - Create a diagonal matrix with the element of a vector \f$ DA_{ii} = A_i \f$. \param A : Vector which element will be put in the diagonal. @@ -1067,7 +1066,6 @@ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) (speed gain if used many times with the same result matrix size). \exception vpException::dimensionError If matrices are not 3-by-3 dimension. - */ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpRotationMatrix &C) { @@ -1104,7 +1102,6 @@ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpRotationMat (speed gain if used many times with the same result matrix size). \exception vpException::dimensionError If matrices are not 4-by-4 dimension. - */ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpHomogeneousMatrix &C) { @@ -2410,18 +2407,7 @@ vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const vpMatrix U, V, Ap; vpColVector sv; - Ap.resize(ncols, nrows, false, false); - - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); + U = *this; U.svdLapack(sv, V); compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr); @@ -2478,18 +2464,7 @@ unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) con vpMatrix U, V; vpColVector sv; - Ap.resize(ncols, nrows, false, false); - - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); + U = *this; U.svdLapack(sv, V); compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr); @@ -2550,27 +2525,13 @@ unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double int rank_out; vpMatrix U, V; - vpColVector sv_; Ap.resize(ncols, nrows, false, false); - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); - U.svdLapack(sv_, V); - - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr); + U = *this; + U.svdLapack(sv, V); - // Remove singular values equal to the one that corresponds to the lines of 0 - // introduced when m < n - memcpy(sv.data, sv_.data, sv.size() * sizeof(double)); + compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr); return static_cast(rank_out); } @@ -2688,25 +2649,18 @@ unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double unsigned int ncols = getCols(); int rank_out; vpMatrix U, V; - vpColVector sv_; if (nrows < ncols) { U.resize(ncols, ncols, true); - sv.resize(nrows, false); + U.insert(*this, 0, 0); } else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); + U = *this; } - U.insert(*this, 0, 0); - U.svdLapack(sv_, V); - - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt); + U.svdLapack(sv, V); - // Remove singular values equal to the one that corresponds to the lines of 0 - // introduced when m < n - memcpy(sv.data, sv_.data, sv.size() * sizeof(double)); + compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt); return static_cast(rank_out); } @@ -2757,18 +2711,7 @@ vpMatrix vpMatrix::pseudoInverseLapack(int rank_in) const vpMatrix U, V, Ap; vpColVector sv; - Ap.resize(ncols, nrows, false, false); - - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); + U = *this; U.svdLapack(sv, V); compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr); @@ -2832,18 +2775,7 @@ int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, int rank_in) const vpMatrix U, V; vpColVector sv; - Ap.resize(ncols, nrows, false, false); - - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); + U = *this; U.svdLapack(sv, V); compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr); @@ -2910,29 +2842,14 @@ int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in) co unsigned int ncols = getCols(); int rank_out; double svThreshold = 1e-26; - vpMatrix U, V; - vpColVector sv_; Ap.resize(ncols, nrows, false, false); - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); - U.svdLapack(sv_, V); - - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr); + U = *this; + U.svdLapack(sv, V); - // Remove singular values equal to the one that corresponds to the lines of 0 - // introduced when m < n - memcpy(sv.data, sv_.data, sv.size() * sizeof(double)); + compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr); return rank_out; } @@ -3056,25 +2973,18 @@ int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in, vp int rank_out; double svThreshold = 1e-26; vpMatrix U, V; - vpColVector sv_; if (nrows < ncols) { U.resize(ncols, ncols, true); - sv.resize(nrows, false); + U.insert(*this, 0, 0); } else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); + U = *this; } - U.insert(*this, 0, 0); - U.svdLapack(sv_, V); - - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt); + U.svdLapack(sv, V); - // Remove singular values equal to the one that corresponds to the lines of 0 - // introduced when m < n - memcpy(sv.data, sv_.data, sv.size() * sizeof(double)); + compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt); return rank_out; } @@ -3122,22 +3032,10 @@ vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const unsigned int nrows = getRows(); unsigned int ncols = getCols(); int rank_out; - vpMatrix U, V, Ap; vpColVector sv; - Ap.resize(ncols, nrows, false, false); - - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); + U = *this; U.svdEigen3(sv, V); compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr); @@ -3190,28 +3088,17 @@ unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) con unsigned int nrows = getRows(); unsigned int ncols = getCols(); int rank_out; - vpMatrix U, V; vpColVector sv; - Ap.resize(ncols, nrows, false, false); - - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); + U = *this; U.svdEigen3(sv, V); compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr); return static_cast(rank_out); } + /*! Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf A\f$ along with singular values and return the rank of the matrix using @@ -3264,29 +3151,14 @@ unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double unsigned int nrows = getRows(); unsigned int ncols = getCols(); int rank_out; - vpMatrix U, V; - vpColVector sv_; Ap.resize(ncols, nrows, false, false); - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); - U.svdEigen3(sv_, V); - - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr); + U = *this; + U.svdEigen3(sv, V); - // Remove singular values equal to the one that corresponds to the lines of 0 - // introduced when m < n - memcpy(sv.data, sv_.data, sv.size() * sizeof(double)); + compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr); return static_cast(rank_out); } @@ -3404,25 +3276,18 @@ unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double unsigned int ncols = getCols(); int rank_out; vpMatrix U, V; - vpColVector sv_; if (nrows < ncols) { U.resize(ncols, ncols, true); - sv.resize(nrows, false); + U.insert(*this, 0, 0); } else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); + U = *this; } - U.insert(*this, 0, 0); - U.svdEigen3(sv_, V); - - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt); + U.svdEigen3(sv, V); - // Remove singular values equal to the one that corresponds to the lines of 0 - // introduced when m < n - memcpy(sv.data, sv_.data, sv.size() * sizeof(double)); + compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt); return static_cast(rank_out); } @@ -3469,22 +3334,10 @@ vpMatrix vpMatrix::pseudoInverseEigen3(int rank_in) const unsigned int ncols = getCols(); int rank_out; double svThreshold = 1e-26; - vpMatrix U, V, Ap; vpColVector sv; - Ap.resize(ncols, nrows, false, false); - - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); + U = *this; U.svdEigen3(sv, V); compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr); @@ -3544,22 +3397,12 @@ int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, int rank_in) const unsigned int ncols = getCols(); int rank_out; double svThreshold = 1e-26; - vpMatrix U, V; vpColVector sv; Ap.resize(ncols, nrows, false, false); - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); + U = *this; U.svdEigen3(sv, V); compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr); @@ -3626,29 +3469,14 @@ int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in) co unsigned int ncols = getCols(); int rank_out; double svThreshold = 1e-26; - vpMatrix U, V; - vpColVector sv_; Ap.resize(ncols, nrows, false, false); - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); - U.svdEigen3(sv_, V); - - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr); + U = *this; + U.svdEigen3(sv, V); - // Remove singular values equal to the one that corresponds to the lines of 0 - // introduced when m < n - memcpy(sv.data, sv_.data, sv.size() * sizeof(double)); + compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr); return rank_out; } @@ -3772,25 +3600,17 @@ int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in, vp int rank_out; double svThreshold = 1e-26; vpMatrix U, V; - vpColVector sv_; if (nrows < ncols) { U.resize(ncols, ncols, true); - sv.resize(nrows, false); + U.insert(*this, 0, 0); } else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); + U = *this; } + U.svdEigen3(sv, V); - U.insert(*this, 0, 0); - U.svdEigen3(sv_, V); - - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt); - - // Remove singular values equal to the one that corresponds to the lines of 0 - // introduced when m < n - memcpy(sv.data, sv_.data, sv.size() * sizeof(double)); + compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt); return rank_out; } @@ -3838,22 +3658,10 @@ vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const unsigned int nrows = getRows(); unsigned int ncols = getCols(); int rank_out; - vpMatrix U, V, Ap; vpColVector sv; - Ap.resize(ncols, nrows, false, false); - - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); + U = *this; U.svdOpenCV(sv, V); compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr); @@ -3906,22 +3714,12 @@ unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) con unsigned int nrows = getRows(); unsigned int ncols = getCols(); int rank_out; - vpMatrix U, V; vpColVector sv; Ap.resize(ncols, nrows, false, false); - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); + U = *this; U.svdOpenCV(sv, V); compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr); @@ -3980,29 +3778,14 @@ unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double unsigned int nrows = getRows(); unsigned int ncols = getCols(); int rank_out; - vpMatrix U, V; - vpColVector sv_; Ap.resize(ncols, nrows, false, false); - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); - U.svdOpenCV(sv_, V); - - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr); + U = *this; + U.svdOpenCV(sv, V); - // Remove singular values equal to the one that corresponds to the lines of 0 - // introduced when m < n - memcpy(sv.data, sv_.data, sv.size() * sizeof(double)); + compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr); return static_cast(rank_out); } @@ -4120,25 +3903,18 @@ unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double unsigned int ncols = getCols(); int rank_out; vpMatrix U, V; - vpColVector sv_; if (nrows < ncols) { U.resize(ncols, ncols, true); - sv.resize(nrows, false); + U.insert(*this, 0, 0); } else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); + U = *this; } - U.insert(*this, 0, 0); - U.svdOpenCV(sv_, V); - - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt); + U.svdOpenCV(sv, V); - // Remove singular values equal to the one that corresponds to the lines of 0 - // introduced when m < n - memcpy(sv.data, sv_.data, sv.size() * sizeof(double)); + compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt); return static_cast(rank_out); } @@ -4185,22 +3961,10 @@ vpMatrix vpMatrix::pseudoInverseOpenCV(int rank_in) const unsigned int ncols = getCols(); int rank_out; double svThreshold = 1e-26; - vpMatrix U, V, Ap; vpColVector sv; - Ap.resize(ncols, nrows, false, false); - - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); + U = *this; U.svdOpenCV(sv, V); compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr); @@ -4260,22 +4024,12 @@ int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, int rank_in) const unsigned int ncols = getCols(); int rank_out; double svThreshold = 1e-26; - vpMatrix U, V; vpColVector sv; Ap.resize(ncols, nrows, false, false); - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); + U = *this; U.svdOpenCV(sv, V); compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr); @@ -4342,29 +4096,14 @@ int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in) co unsigned int ncols = getCols(); int rank_out; double svThreshold = 1e-26; - vpMatrix U, V; - vpColVector sv_; Ap.resize(ncols, nrows, false, false); - if (nrows < ncols) { - U.resize(ncols, ncols, true); - sv.resize(nrows, false); - } - else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); - } - - U.insert(*this, 0, 0); - U.svdOpenCV(sv_, V); - - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr); + U = *this; + U.svdOpenCV(sv, V); - // Remove singular values equal to the one that corresponds to the lines of 0 - // introduced when m < n - memcpy(sv.data, sv_.data, sv.size() * sizeof(double)); + compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr); return rank_out; } @@ -4488,25 +4227,18 @@ int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in, vp int rank_out; double svThreshold = 1e-26; vpMatrix U, V; - vpColVector sv_; if (nrows < ncols) { U.resize(ncols, ncols, true); - sv.resize(nrows, false); + U.insert(*this, 0, 0); } else { - U.resize(nrows, ncols, false); - sv.resize(ncols, false); + U = *this; } - U.insert(*this, 0, 0); - U.svdOpenCV(sv_, V); - - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt); + U.svdOpenCV(sv, V); - // Remove singular values equal to the one that corresponds to the lines of 0 - // introduced when m < n - memcpy(sv.data, sv_.data, sv.size() * sizeof(double)); + compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt); return rank_out; } @@ -4902,22 +4634,22 @@ int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix // Reconstruct matrix A from ImA, ImAt, KerAt vpMatrix S(rank, A.getCols()); - for(unsigned int i = 0; i< rank; i++) + for (unsigned int i = 0; i< rank; i++) S[i][i] = sv[i]; vpMatrix Vt(A.getCols(), A.getCols()); Vt.insert(imAt.t(), 0, 0); Vt.insert(kerAt, rank, 0); - (imA * S * Vt).print(std::cout, 10, "Im(A) * S * [Im(A^T) | Ker(A)]^T:"); + (imA *S *Vt).print(std::cout, 10, "Im(A) * S * [Im(A^T) | Ker(A)]^T:"); } \endcode - Once build, the previous example produces the following output: + Once build, the previous example produces the following output : \code{.sh} - A: [2,3]= + A: [2,3] = 2 3 5 -4 2 3 - A^+ (pseudo-inverse): [3,2]= + A^+(pseudo-inverse): [3,2]= 0.117899 -0.190782 0.065380 0.039657 0.113612 0.052518 @@ -4926,7 +4658,7 @@ int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix Im(A): [2,2]= 0.81458 -0.58003 0.58003 0.81458 - Im(A^T): [3,2]= + Im(A^T): [3,2] = -0.100515 -0.994397 0.524244 -0.024967 0.845615 -0.102722 @@ -4934,7 +4666,7 @@ int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix -0.032738 -0.851202 0.523816 - Im(A) * S * [Im(A^T) | Ker(A)]^T:[2,3]= + Im(A) * S * [Im(A^T) | Ker(A)]^T: [2,3]= 2 3 5 -4 2 3 \endcode @@ -4999,1790 +4731,1790 @@ unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThr { vpMatrix A(2, 3); -A[0][0] = 2; A[0][1] = 3; A[0][2] = 5; -A[1][0] = -4; A[1][1] = 2; A[1][2] = 3; + A[0][0] = 2; A[0][1] = 3; A[0][2] = 5; + A[1][0] = -4; A[1][1] = 2; A[1][2] = 3; -A.print(std::cout, 10, "A: "); + A.print(std::cout, 10, "A: "); -vpColVector sv; -vpMatrix A_p, imA, imAt, kerAt; -int rank_in = 2; -int rank_out = A.pseudoInverse(A_p, sv, rank_in, imA, imAt, kerAt); -if (rank_out != rank_in) { - std::cout << "There is a possibility that the pseudo-inverse in wrong." << std::endl; - std::cout << "Are you sure that the matrix rank is " << rank_in << std::endl; -} + vpColVector sv; + vpMatrix A_p, imA, imAt, kerAt; + int rank_in = 2; + int rank_out = A.pseudoInverse(A_p, sv, rank_in, imA, imAt, kerAt); + if (rank_out != rank_in) { + std::cout << "There is a possibility that the pseudo-inverse in wrong." << std::endl; + std::cout << "Are you sure that the matrix rank is " << rank_in << std::endl; + } -A_p.print(std::cout, 10, "A^+ (pseudo-inverse): "); -std::cout << "Rank in : " << rank_in << std::endl; -std::cout << "Rank out: " << rank_out << std::endl; -std::cout << "Singular values: " << sv.t() << std::endl; -imA.print(std::cout, 10, "Im(A): "); -imAt.print(std::cout, 10, "Im(A^T): "); + A_p.print(std::cout, 10, "A^+ (pseudo-inverse): "); + std::cout << "Rank in : " << rank_in << std::endl; + std::cout << "Rank out: " << rank_out << std::endl; + std::cout << "Singular values: " << sv.t() << std::endl; + imA.print(std::cout, 10, "Im(A): "); + imAt.print(std::cout, 10, "Im(A^T): "); -if (kerAt.size()) { - kerAt.t().print(std::cout, 10, "Ker(A): "); -} -else { - std::cout << "Ker(A) empty " << std::endl; -} + if (kerAt.size()) { + kerAt.t().print(std::cout, 10, "Ker(A): "); + } + else { + std::cout << "Ker(A) empty " << std::endl; + } -// Reconstruct matrix A from ImA, ImAt, KerAt -vpMatrix S(rank, A.getCols()); -for (unsigned int i = 0; i < rank_in; i++) - S[i][i] = sv[i]; -vpMatrix Vt(A.getCols(), A.getCols()); -Vt.insert(imAt.t(), 0, 0); -Vt.insert(kerAt, rank, 0); -(imA * S * Vt).print(std::cout, 10, "Im(A) * S * [Im(A^T) | Ker(A)]^T:"); + // Reconstruct matrix A from ImA, ImAt, KerAt + vpMatrix S(rank, A.getCols()); + for (unsigned int i = 0; i < rank_in; i++) + S[i][i] = sv[i]; + vpMatrix Vt(A.getCols(), A.getCols()); + Vt.insert(imAt.t(), 0, 0); + Vt.insert(kerAt, rank, 0); + (imA * S * Vt).print(std::cout, 10, "Im(A) * S * [Im(A^T) | Ker(A)]^T:"); } \endcode - Once build, the previous example produces the following output : + Once build, the previous example produces the following output : \code - A : [2, 3] = - 2 3 5 - - 4 2 3 - A ^ +(pseudo - inverse) : [3, 2] = - 0.117899 - 0.190782 - 0.065380 0.039657 - 0.113612 0.052518 - Rank in : 2 - Rank out : 2 - Singular values : 6.874359351 4.443330227 - Im(A) : [2, 2] = - 0.81458 - 0.58003 - 0.58003 0.81458 - Im(A ^ T) : [3, 2] = - -0.100515 - 0.994397 - 0.524244 - 0.024967 - 0.845615 - 0.102722 - Ker(A) : [3, 1] = - -0.032738 - - 0.851202 - 0.523816 - Im(A) * S *[Im(A ^ T) | Ker(A)] ^ T : [2, 3] = - 2 3 5 - - 4 2 3 - \endcode - */ - int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt, - vpMatrix &kerAt) const - { + A : [2, 3] = + 2 3 5 + - 4 2 3 + A ^ +(pseudo - inverse) : [3, 2] = + 0.117899 - 0.190782 + 0.065380 0.039657 + 0.113612 0.052518 + Rank in : 2 + Rank out : 2 + Singular values : 6.874359351 4.443330227 + Im(A) : [2, 2] = + 0.81458 - 0.58003 + 0.58003 0.81458 + Im(A ^ T) : [3, 2] = + -0.100515 - 0.994397 + 0.524244 - 0.024967 + 0.845615 - 0.102722 + Ker(A) : [3, 1] = + -0.032738 + - 0.851202 + 0.523816 + Im(A) * S *[Im(A ^ T) | Ker(A)] ^ T : [2, 3] = + 2 3 5 + - 4 2 3 + \endcode +*/ +int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt, + vpMatrix &kerAt) const +{ #if defined(VISP_HAVE_LAPACK) - return pseudoInverseLapack(Ap, sv, rank_in, imA, imAt, kerAt); + return pseudoInverseLapack(Ap, sv, rank_in, imA, imAt, kerAt); #elif defined(VISP_HAVE_EIGEN3) - return pseudoInverseEigen3(Ap, sv, rank_in, imA, imAt, kerAt); + return pseudoInverseEigen3(Ap, sv, rank_in, imA, imAt, kerAt); #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1 - return pseudoInverseOpenCV(Ap, sv, rank_in, imA, imAt, kerAt); + return pseudoInverseOpenCV(Ap, sv, rank_in, imA, imAt, kerAt); #else - (void)Ap; - (void)sv; - (void)rank_in; - (void)imA; - (void)imAt; - (void)kerAt; - throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. " - "Install Lapack, Eigen3 or OpenCV 3rd party")); + (void)Ap; + (void)sv; + (void)rank_in; + (void)imA; + (void)imAt; + (void)kerAt; + throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. " + "Install Lapack, Eigen3 or OpenCV 3rd party")); #endif - } +} - /*! - Extract a column vector from a matrix. - \warning All the indexes start from 0 in this function. - \param j : Index of the column to extract. If col=0, the first column is extracted. - \param i_begin : Index of the row that gives the location of the first element - of the column vector to extract. - \param column_size : Size of the column vector to extract. - \return The extracted column vector. +/*! + Extract a column vector from a matrix. + \warning All the indexes start from 0 in this function. + \param j : Index of the column to extract. If col=0, the first column is extracted. + \param i_begin : Index of the row that gives the location of the first element + of the column vector to extract. + \param column_size : Size of the column vector to extract. + \return The extracted column vector. + + The following example shows how to use this function: + \code + #include + #include - The following example shows how to use this function: - \code - #include - #include + int main() + { + vpMatrix A(4,4); - int main() - { - vpMatrix A(4,4); + for(unsigned int i=0; i < A.getRows(); i++) + for(unsigned int j=0; j < A.getCols(); j++) + A[i][j] = i*A.getCols()+j; + + A.print(std::cout, 4); + + vpColVector cv = A.getCol(1, 1, 3); + std::cout << "Column vector: \n" << cv << std::endl; + } + \endcode + It produces the following output : + \code + [4, 4] = + 0 1 2 3 + 4 5 6 7 + 8 9 10 11 + 12 13 14 15 + column vector : + 5 + 9 + 13 + \endcode +*/ +vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, unsigned int column_size) const +{ + if (i_begin + column_size > getRows() || j >= getCols()) + throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(), + getCols())); + vpColVector c(column_size); + for (unsigned int i = 0; i < column_size; i++) + c[i] = (*this)[i_begin + i][j]; + return c; +} - for(unsigned int i=0; i < A.getRows(); i++) - for(unsigned int j=0; j < A.getCols(); j++) - A[i][j] = i*A.getCols()+j; +/*! + Extract a column vector from a matrix. + \warning All the indexes start from 0 in this function. + \param j : Index of the column to extract. If j=0, the first column is extracted. + \return The extracted column vector. - A.print(std::cout, 4); + The following example shows how to use this function: + \code + #include + #include - vpColVector cv = A.getCol(1, 1, 3); - std::cout << "Column vector: \n" << cv << std::endl; - } - \endcode - It produces the following output: - \code - [4,4]= - 0 1 2 3 - 4 5 6 7 - 8 9 10 11 - 12 13 14 15 - column vector: - 5 - 9 - 13 - \endcode - */ - vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, unsigned int column_size) const + int main() { - if (i_begin + column_size > getRows() || j >= getCols()) - throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(), - getCols())); - vpColVector c(column_size); - for (unsigned int i = 0; i < column_size; i++) - c[i] = (*this)[i_begin + i][j]; - return c; - } - - /*! - Extract a column vector from a matrix. - \warning All the indexes start from 0 in this function. - \param j : Index of the column to extract. If j=0, the first column is extracted. - \return The extracted column vector. - - The following example shows how to use this function: - \code - #include - #include + vpMatrix A(4,4); - int main() - { - vpMatrix A(4,4); + for (unsigned int i = 0; i < A.getRows(); i++) + for (unsigned int j = 0; j < A.getCols(); j++) + A[i][j] = i*A.getCols()+j; - for(unsigned int i=0; i < A.getRows(); i++) - for(unsigned int j=0; j < A.getCols(); j++) - A[i][j] = i*A.getCols()+j; + A.print(std::cout, 4); + + vpColVector cv = A.getCol(1); + std::cout << "Column vector: \n" << cv << std::endl; + } + \endcode + It produces the following output : + \code + [4, 4] = + 0 1 2 3 + 4 5 6 7 + 8 9 10 11 + 12 13 14 15 + column vector : + 1 + 5 + 9 + 13 + \endcode + */ +vpColVector vpMatrix::getCol(unsigned int j) const { return getCol(j, 0, rowNum); } - A.print(std::cout, 4); +/*! + Extract a row vector from a matrix. + \warning All the indexes start from 0 in this function. + \param i : Index of the row to extract. If i=0, the first row is extracted. + \return The extracted row vector. - vpColVector cv = A.getCol(1); - std::cout << "Column vector: \n" << cv << std::endl; - } - \endcode - It produces the following output: - \code - [4,4]= - 0 1 2 3 - 4 5 6 7 - 8 9 10 11 - 12 13 14 15 - column vector: - 1 - 5 - 9 - 13 - \endcode - */ - vpColVector vpMatrix::getCol(unsigned int j) const { return getCol(j, 0, rowNum); } - - /*! - Extract a row vector from a matrix. - \warning All the indexes start from 0 in this function. - \param i : Index of the row to extract. If i=0, the first row is extracted. - \return The extracted row vector. - - The following example shows how to use this function: - \code - #include - #include - - int main() - { - vpMatrix A(4,4); - - for(unsigned int i=0; i < A.getRows(); i++) - for(unsigned int j=0; j < A.getCols(); j++) - A[i][j] = i*A.getCols()+j; - - A.print(std::cout, 4); - - vpRowVector rv = A.getRow(1); - std::cout << "Row vector: \n" << rv << std::endl; - } - \endcode - It produces the following output: + The following example shows how to use this function: \code - [4,4]= - 0 1 2 3 - 4 5 6 7 - 8 9 10 11 - 12 13 14 15 - Row vector: - 4 5 6 7 - \endcode - */ - vpRowVector vpMatrix::getRow(unsigned int i) const { return getRow(i, 0, colNum); } - - /*! - Extract a row vector from a matrix. - \warning All the indexes start from 0 in this function. - \param i : Index of the row to extract. If i=0, the first row is extracted. - \param j_begin : Index of the column that gives the location of the first - element of the row vector to extract. - \param row_size : Size of the row vector to extract. - \return The extracted row vector. - - The following example shows how to use this function: - \code - #include - #include + #include + #include - int main() - { - vpMatrix A(4,4); + int main() + { + vpMatrix A(4,4); - for(unsigned int i=0; i < A.getRows(); i++) - for(unsigned int j=0; j < A.getCols(); j++) - A[i][j] = i*A.getCols()+j; + for(unsigned int i=0; i < A.getRows(); i++) + for(unsigned int j=0; j < A.getCols(); j++) + A[i][j] = i*A.getCols()+j; - A.print(std::cout, 4); + A.print(std::cout, 4); - vpRowVector rv = A.getRow(1, 1, 3); - std::cout << "Row vector: \n" << rv << std::endl; - } - \endcode - It produces the following output: - \code - [4,4]= - 0 1 2 3 - 4 5 6 7 - 8 9 10 11 - 12 13 14 15 - Row vector: - 5 6 7 - \endcode - */ - vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const + vpRowVector rv = A.getRow(1); + std::cout << "Row vector: \n" << rv << std::endl; + } + \endcode + It produces the following output: + \code + [4,4]= + 0 1 2 3 + 4 5 6 7 + 8 9 10 11 + 12 13 14 15 + Row vector: + 4 5 6 7 + \endcode + */ +vpRowVector vpMatrix::getRow(unsigned int i) const { return getRow(i, 0, colNum); } + +/*! + Extract a row vector from a matrix. + \warning All the indexes start from 0 in this function. + \param i : Index of the row to extract. If i=0, the first row is extracted. + \param j_begin : Index of the column that gives the location of the first + element of the row vector to extract. + \param row_size : Size of the row vector to extract. + \return The extracted row vector. + + The following example shows how to use this function: + \code + #include + #include + + int main() { - if (j_begin + row_size > colNum || i >= rowNum) - throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix")); + vpMatrix A(4,4); - vpRowVector r(row_size); - if (r.data != nullptr && data != nullptr) { - memcpy(r.data, (*this)[i] + j_begin, row_size * sizeof(double)); - } + for(unsigned int i=0; i < A.getRows(); i++) + for(unsigned int j=0; j < A.getCols(); j++) + A[i][j] = i*A.getCols()+j; - return r; - } + A.print(std::cout, 4); - /*! - Extract a diagonal vector from a matrix. + vpRowVector rv = A.getRow(1, 1, 3); + std::cout << "Row vector: \n" << rv << std::endl; + } + \endcode + It produces the following output : + \code + [4, 4] = + 0 1 2 3 + 4 5 6 7 + 8 9 10 11 + 12 13 14 15 + Row vector : + 5 6 7 + \endcode +*/ +vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const +{ + if (j_begin + row_size > colNum || i >= rowNum) + throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix")); - \return The diagonal of the matrix. + vpRowVector r(row_size); + if (r.data != nullptr && data != nullptr) { + memcpy(r.data, (*this)[i] + j_begin, row_size * sizeof(double)); + } - \warning An empty vector is returned if the matrix is empty. + return r; +} - The following example shows how to use this function: - \code - #include +/*! + Extract a diagonal vector from a matrix. - int main() - { - vpMatrix A(3,4); + \return The diagonal of the matrix. - for(unsigned int i=0; i < A.getRows(); i++) - for(unsigned int j=0; j < A.getCols(); j++) - A[i][j] = i*A.getCols()+j; + \warning An empty vector is returned if the matrix is empty. - A.print(std::cout, 4); + The following example shows how to use this function: + \code + #include - vpColVector diag = A.getDiag(); - std::cout << "Diag vector: \n" << diag.t() << std::endl; - } - \endcode - It produces the following output: - \code - [3,4]= - 0 1 2 3 - 4 5 6 7 - 8 9 10 11 - Diag vector: - 0 5 10 - \endcode - */ - vpColVector vpMatrix::getDiag() const + int main() { - unsigned int min_size = std::min(rowNum, colNum); - vpColVector diag; + vpMatrix A(3, 4); - if (min_size > 0) { - diag.resize(min_size, false); + for (unsigned int i = 0; i < A.getRows(); i++) + for (unsigned int j = 0; j < A.getCols(); j++) + A[i][j] = i*A.getCols()+j; - for (unsigned int i = 0; i < min_size; i++) { - diag[i] = (*this)[i][i]; - } - } + A.print(std::cout, 4); - return diag; + vpColVector diag = A.getDiag(); + std::cout << "Diag vector: \n" << diag.t() << std::endl; } + \endcode + It produces the following output : + \code + [3, 4] = + 0 1 2 3 + 4 5 6 7 + 8 9 10 11 + Diag vector : + 0 5 10 + \endcode +*/ +vpColVector vpMatrix::getDiag() const +{ + unsigned int min_size = std::min(rowNum, colNum); + vpColVector diag; - /*! - Stack matrix \e B to the end of matrix \e A and return the resulting matrix - [ A B ]^T - - \param A : Upper matrix. - \param B : Lower matrix. - \return Stacked matrix [ A B ]^T + if (min_size > 0) { + diag.resize(min_size, false); - \warning A and B must have the same number of columns. - */ - vpMatrix vpMatrix::stack(const vpMatrix &A, const vpMatrix &B) - { - vpMatrix C; + for (unsigned int i = 0; i < min_size; i++) { + diag[i] = (*this)[i][i]; + } + } - vpMatrix::stack(A, B, C); + return diag; +} - return C; - } +/*! + Stack matrix \e B to the end of matrix \e A and return the resulting matrix + [ A B ]^T - /*! - Stack matrix \e B to the end of matrix \e A and return the resulting matrix - in \e C. + \param A : Upper matrix. + \param B : Lower matrix. + \return Stacked matrix [ A B ]^T - \param A : Upper matrix. - \param B : Lower matrix. - \param C : Stacked matrix C = [ A B ]^T + \warning A and B must have the same number of columns. +*/ +vpMatrix vpMatrix::stack(const vpMatrix &A, const vpMatrix &B) +{ + vpMatrix C; - \warning A and B must have the same number of columns. A and C, B and C must - be two different objects. - */ - void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) - { - unsigned int nra = A.getRows(); - unsigned int nrb = B.getRows(); + vpMatrix::stack(A, B, C); - if (nra != 0) { - if (A.getCols() != B.getCols()) { - throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(), - A.getCols(), B.getRows(), B.getCols())); - } - } + return C; +} - if (A.data != nullptr && A.data == C.data) { - std::cerr << "A and C must be two different objects!" << std::endl; - return; - } +/*! + Stack matrix \e B to the end of matrix \e A and return the resulting matrix + in \e C. - if (B.data != nullptr && B.data == C.data) { - std::cerr << "B and C must be two different objects!" << std::endl; - return; - } + \param A : Upper matrix. + \param B : Lower matrix. + \param C : Stacked matrix C = [ A B ]^T - C.resize(nra + nrb, B.getCols(), false, false); + \warning A and B must have the same number of columns. A and C, B and C must + be two different objects. +*/ +void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) +{ + unsigned int nra = A.getRows(); + unsigned int nrb = B.getRows(); - if (C.data != nullptr && A.data != nullptr && A.size() > 0) { - // Copy A in C - memcpy(C.data, A.data, sizeof(double) * A.size()); + if (nra != 0) { + if (A.getCols() != B.getCols()) { + throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(), + A.getCols(), B.getRows(), B.getCols())); } + } - if (C.data != nullptr && B.data != nullptr && B.size() > 0) { - // Copy B in C - memcpy(C.data + A.size(), B.data, sizeof(double) * B.size()); - } + if (A.data != nullptr && A.data == C.data) { + std::cerr << "A and C must be two different objects!" << std::endl; + return; } - /*! - Stack row vector \e r to matrix \e A and return the resulting matrix [ A r ]^T + if (B.data != nullptr && B.data == C.data) { + std::cerr << "B and C must be two different objects!" << std::endl; + return; + } - \param A : Upper matrix. - \param r : Lower row vector. - \return Stacked matrix [ A r ]^T + C.resize(nra + nrb, B.getCols(), false, false); - \warning \e A and \e r must have the same number of columns. - */ - vpMatrix vpMatrix::stack(const vpMatrix &A, const vpRowVector &r) - { - vpMatrix C; - vpMatrix::stack(A, r, C); + if (C.data != nullptr && A.data != nullptr && A.size() > 0) { + // Copy A in C + memcpy(C.data, A.data, sizeof(double) * A.size()); + } - return C; + if (C.data != nullptr && B.data != nullptr && B.size() > 0) { + // Copy B in C + memcpy(C.data + A.size(), B.data, sizeof(double) * B.size()); } +} - /*! - Stack row vector \e r to the end of matrix \e A and return the resulting - matrix in \e C. +/*! + Stack row vector \e r to matrix \e A and return the resulting matrix [ A r ]^T - \param A : Upper matrix. - \param r : Lower row vector. - \param C : Stacked matrix C = [ A r ]^T + \param A : Upper matrix. + \param r : Lower row vector. + \return Stacked matrix [ A r ]^T - \warning A and r must have the same number of columns. A and C must be two - different objects. - */ - void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C) - { - if (A.data != nullptr && A.data == C.data) { - std::cerr << "A and C must be two different objects!" << std::endl; - return; - } + \warning \e A and \e r must have the same number of columns. +*/ +vpMatrix vpMatrix::stack(const vpMatrix &A, const vpRowVector &r) +{ + vpMatrix C; + vpMatrix::stack(A, r, C); + + return C; +} + +/*! + Stack row vector \e r to the end of matrix \e A and return the resulting + matrix in \e C. - C = A; - C.stack(r); + \param A : Upper matrix. + \param r : Lower row vector. + \param C : Stacked matrix C = [ A r ]^T + + \warning A and r must have the same number of columns. A and C must be two + different objects. +*/ +void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C) +{ + if (A.data != nullptr && A.data == C.data) { + std::cerr << "A and C must be two different objects!" << std::endl; + return; } - /*! - Stack column vector \e c to matrix \e A and return the resulting matrix [ A c ] + C = A; + C.stack(r); +} - \param A : Left matrix. - \param c : Right column vector. - \return Stacked matrix [ A c ] +/*! + Stack column vector \e c to matrix \e A and return the resulting matrix [ A c ] - \warning \e A and \e c must have the same number of rows. - */ - vpMatrix vpMatrix::stack(const vpMatrix &A, const vpColVector &c) - { - vpMatrix C; - vpMatrix::stack(A, c, C); + \param A : Left matrix. + \param c : Right column vector. + \return Stacked matrix [ A c ] - return C; - } + \warning \e A and \e c must have the same number of rows. +*/ +vpMatrix vpMatrix::stack(const vpMatrix &A, const vpColVector &c) +{ + vpMatrix C; + vpMatrix::stack(A, c, C); - /*! - Stack column vector \e c to the end of matrix \e A and return the resulting - matrix in \e C. + return C; +} - \param A : Left matrix. - \param c : Right column vector. - \param C : Stacked matrix C = [ A c ] +/*! + Stack column vector \e c to the end of matrix \e A and return the resulting + matrix in \e C. - \warning A and c must have the same number of rows. A and C must be two - different objects. - */ - void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C) - { - if (A.data != nullptr && A.data == C.data) { - std::cerr << "A and C must be two different objects!" << std::endl; - return; - } + \param A : Left matrix. + \param c : Right column vector. + \param C : Stacked matrix C = [ A c ] - C = A; - C.stack(c); + \warning A and c must have the same number of rows. A and C must be two + different objects. +*/ +void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C) +{ + if (A.data != nullptr && A.data == C.data) { + std::cerr << "A and C must be two different objects!" << std::endl; + return; } - /*! - Insert matrix B in matrix A at the given position. + C = A; + C.stack(c); +} - \param A : Main matrix. - \param B : Matrix to insert. - \param r : Index of the row where to add the matrix. - \param c : Index of the column where to add the matrix. - \return Matrix with B insert in A. +/*! + Insert matrix B in matrix A at the given position. - \warning Throw exception if the sizes of the matrices do not allow the - insertion. - */ - vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, unsigned int r, unsigned int c) - { - vpArray2D C; + \param A : Main matrix. + \param B : Matrix to insert. + \param r : Index of the row where to add the matrix. + \param c : Index of the column where to add the matrix. + \return Matrix with B insert in A. - vpArray2D::insert(A, B, C, r, c); + \warning Throw exception if the sizes of the matrices do not allow the + insertion. +*/ +vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, unsigned int r, unsigned int c) +{ + vpArray2D C; - return vpMatrix(C); - } + vpArray2D::insert(A, B, C, r, c); - /*! - \relates vpMatrix - Insert matrix B in matrix A at the given position. + return vpMatrix(C); +} - \param A : Main matrix. - \param B : Matrix to insert. - \param C : Result matrix. - \param r : Index of the row where to insert matrix B. - \param c : Index of the column where to insert matrix B. +/*! + \relates vpMatrix + Insert matrix B in matrix A at the given position. - \warning Throw exception if the sizes of the matrices do not - allow the insertion. - */ - void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, unsigned int c) - { - vpArray2D C_array; + \param A : Main matrix. + \param B : Matrix to insert. + \param C : Result matrix. + \param r : Index of the row where to insert matrix B. + \param c : Index of the column where to insert matrix B. - vpArray2D::insert(A, B, C_array, r, c); + \warning Throw exception if the sizes of the matrices do not + allow the insertion. +*/ +void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, unsigned int c) +{ + vpArray2D C_array; - C = C_array; - } + vpArray2D::insert(A, B, C_array, r, c); - /*! - Juxtapose to matrices C = [ A B ]. + C = C_array; +} - \f$ C = \left( \begin{array}{cc} A & B \end{array}\right) \f$ +/*! + Juxtapose to matrices C = [ A B ]. - \param A : Left matrix. - \param B : Right matrix. - \return Juxtaposed matrix C = [ A B ] + \f$ C = \left( \begin{array}{cc} A & B \end{array}\right) \f$ - \warning A and B must have the same number of rows. - */ - vpMatrix vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B) - { - vpMatrix C; + \param A : Left matrix. + \param B : Right matrix. + \return Juxtaposed matrix C = [ A B ] - juxtaposeMatrices(A, B, C); + \warning A and B must have the same number of rows. +*/ +vpMatrix vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B) +{ + vpMatrix C; - return C; - } + juxtaposeMatrices(A, B, C); - /*! - \relates vpMatrix - Juxtapose to matrices C = [ A B ]. + return C; +} - \f$ C = \left( \begin{array}{cc} A & B \end{array}\right) \f$ +/*! + \relates vpMatrix + Juxtapose to matrices C = [ A B ]. - \param A : Left matrix. - \param B : Right matrix. - \param C : Juxtaposed matrix C = [ A B ] + \f$ C = \left( \begin{array}{cc} A & B \end{array}\right) \f$ - \warning A and B must have the same number of rows. - */ - void vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) - { - unsigned int nca = A.getCols(); - unsigned int ncb = B.getCols(); + \param A : Left matrix. + \param B : Right matrix. + \param C : Juxtaposed matrix C = [ A B ] - if (nca != 0) { - if (A.getRows() != B.getRows()) { - throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(), - A.getCols(), B.getRows(), B.getCols())); - } - } + \warning A and B must have the same number of rows. +*/ +void vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) +{ + unsigned int nca = A.getCols(); + unsigned int ncb = B.getCols(); - if (B.getRows() == 0 || nca + ncb == 0) { - std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl; - return; + if (nca != 0) { + if (A.getRows() != B.getRows()) { + throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(), + A.getCols(), B.getRows(), B.getCols())); } + } - C.resize(B.getRows(), nca + ncb, false, false); - - C.insert(A, 0, 0); - C.insert(B, 0, nca); + if (B.getRows() == 0 || nca + ncb == 0) { + std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl; + return; } - //-------------------------------------------------------------------- - // Output - //-------------------------------------------------------------------- + C.resize(B.getRows(), nca + ncb, false, false); - /*! + C.insert(A, 0, 0); + C.insert(B, 0, nca); +} - Pretty print a matrix. The data are tabulated. - The common widths before and after the decimal point - are set with respect to the parameter `length`. +//-------------------------------------------------------------------- +// Output +//-------------------------------------------------------------------- - \param s : Stream used for the printing. +/*! - \param length : The suggested width of each matrix element. - If needed, the used `length` grows in order to accommodate the whole integral part, - and shrinks the decimal part to print only `length` digits. - \param intro : The introduction which is printed before the matrix. - Can be set to zero (or omitted), in which case - the introduction is not printed. + Pretty print a matrix. The data are tabulated. + The common widths before and after the decimal point + are set with respect to the parameter `length`. - \return Returns the common total width for all matrix elements. + \param s : Stream used for the printing. - \sa std::ostream &operator<<(std::ostream &s, const vpArray2D &A) - */ - int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const - { - typedef std::string::size_type size_type; - - unsigned int m = getRows(); - unsigned int n = getCols(); - - std::vector values(m * n); - std::ostringstream oss; - std::ostringstream ossFixed; - std::ios_base::fmtflags original_flags = oss.flags(); - - ossFixed.setf(std::ios::fixed, std::ios::floatfield); - - size_type maxBefore = 0; // the length of the integral part - size_type maxAfter = 0; // number of decimals plus - // one place for the decimal point - for (unsigned int i = 0; i < m; ++i) { - for (unsigned int j = 0; j < n; ++j) { - oss.str(""); - oss << (*this)[i][j]; - if (oss.str().find("e") != std::string::npos) { - ossFixed.str(""); - ossFixed << (*this)[i][j]; - oss.str(ossFixed.str()); - } + \param length : The suggested width of each matrix element. + If needed, the used `length` grows in order to accommodate the whole integral part, + and shrinks the decimal part to print only `length` digits. + \param intro : The introduction which is printed before the matrix. + Can be set to zero (or omitted), in which case + the introduction is not printed. - values[i * n + j] = oss.str(); - size_type thislen = values[i * n + j].size(); - size_type p = values[i * n + j].find('.'); + \return Returns the common total width for all matrix elements. - if (p == std::string::npos) { - maxBefore = vpMath::maximum(maxBefore, thislen); - // maxAfter remains the same - } - else { - maxBefore = vpMath::maximum(maxBefore, p); - maxAfter = vpMath::maximum(maxAfter, thislen - p); - } + \sa std::ostream &operator<<(std::ostream &s, const vpArray2D &A) +*/ +int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const +{ + typedef std::string::size_type size_type; + + unsigned int m = getRows(); + unsigned int n = getCols(); + + std::vector values(m * n); + std::ostringstream oss; + std::ostringstream ossFixed; + std::ios_base::fmtflags original_flags = oss.flags(); + + ossFixed.setf(std::ios::fixed, std::ios::floatfield); + + size_type maxBefore = 0; // the length of the integral part + size_type maxAfter = 0; // number of decimals plus + // one place for the decimal point + for (unsigned int i = 0; i < m; ++i) { + for (unsigned int j = 0; j < n; ++j) { + oss.str(""); + oss << (*this)[i][j]; + if (oss.str().find("e") != std::string::npos) { + ossFixed.str(""); + ossFixed << (*this)[i][j]; + oss.str(ossFixed.str()); } - } - size_type totalLength = length; - // increase totalLength according to maxBefore - totalLength = vpMath::maximum(totalLength, maxBefore); - // decrease maxAfter according to totalLength - maxAfter = std::min(maxAfter, totalLength - maxBefore); - - if (!intro.empty()) - s << intro; - s << "[" << m << "," << n << "]=\n"; - - for (unsigned int i = 0; i < m; i++) { - s << " "; - for (unsigned int j = 0; j < n; j++) { - size_type p = values[i * n + j].find('.'); - s.setf(std::ios::right, std::ios::adjustfield); - s.width((std::streamsize)maxBefore); - s << values[i * n + j].substr(0, p).c_str(); - - if (maxAfter > 0) { - s.setf(std::ios::left, std::ios::adjustfield); - if (p != std::string::npos) { - s.width((std::streamsize)maxAfter); - s << values[i * n + j].substr(p, maxAfter).c_str(); - } - else { - s.width((std::streamsize)maxAfter); - s << ".0"; - } - } + values[i * n + j] = oss.str(); + size_type thislen = values[i * n + j].size(); + size_type p = values[i * n + j].find('.'); - s << ' '; + if (p == std::string::npos) { + maxBefore = vpMath::maximum(maxBefore, thislen); + // maxAfter remains the same + } + else { + maxBefore = vpMath::maximum(maxBefore, p); + maxAfter = vpMath::maximum(maxAfter, thislen - p); } - s << std::endl; } - - s.flags(original_flags); // restore s to standard state - - return (int)(maxBefore + maxAfter); } - /*! - Print using Matlab syntax, to copy/paste in Matlab later. + size_type totalLength = length; + // increase totalLength according to maxBefore + totalLength = vpMath::maximum(totalLength, maxBefore); + // decrease maxAfter according to totalLength + maxAfter = std::min(maxAfter, totalLength - maxBefore); - The following code - \code - #include + if (!intro.empty()) + s << intro; + s << "[" << m << "," << n << "]=\n"; - int main() - { - vpMatrix M(2,3); - int cpt = 0; - for (unsigned int i=0; i> M = [ 0, 1, 2, ; - 3, 4, 5, ] + s << std::endl; + } - M = + s.flags(original_flags); // restore s to standard state - 0 1 2 - 3 4 5 + return (int)(maxBefore + maxAfter); +} - >> - \endcode - */ - std::ostream &vpMatrix::matlabPrint(std::ostream &os) const +/*! + Print using Matlab syntax, to copy/paste in Matlab later. + + The following code + \code + #include + + int main() { - os << "[ "; - for (unsigned int i = 0; i < this->getRows(); ++i) { - for (unsigned int j = 0; j < this->getCols(); ++j) { - os << (*this)[i][j] << ", "; - } - if (this->getRows() != i + 1) { - os << ";" << std::endl; - } - else { - os << "]" << std::endl; - } - } - return os; - } + vpMatrix M(2,3); + int cpt = 0; + for (unsigned int i=0; igetRows(); ++i) { + for (unsigned int j = 0; j < this->getCols(); ++j) { + os << (*this)[i][j] << ", "; } - \endcode - produces this output: - \code - M = ([ - [0, 1, 2, ], - [3, 4, 5, ], - ]) - \endcode - that could be copy/paste in Maple. - - */ - std::ostream &vpMatrix::maplePrint(std::ostream &os) const - { - os << "([ " << std::endl; - for (unsigned int i = 0; i < this->getRows(); ++i) { - os << "["; - for (unsigned int j = 0; j < this->getCols(); ++j) { - os << (*this)[i][j] << ", "; - } - os << "]," << std::endl; + if (this->getRows() != i + 1) { + os << ";" << std::endl; + } + else { + os << "]" << std::endl; } - os << "])" << std::endl; - return os; } + return os; +} - /*! - Print/save a matrix in csv format. +/*! + Print using Maple syntax, to copy/paste in Maple later. - The following code - \code - #include + The following code + \code + #include - int main() - { - std::ofstream ofs("log.csv", std::ofstream::out); - vpMatrix M(2,3); - int cpt = 0; - for (unsigned int i=0; igetRows(); ++i) { + os << "["; + for (unsigned int j = 0; j < this->getCols(); ++j) { + os << (*this)[i][j] << ", "; } - return os; + os << "]," << std::endl; } + os << "])" << std::endl; + return os; +} - /*! - Print to be used as part of a C++ code later. +/*! + Print/save a matrix in csv format. - \param os : the stream to be printed in. - \param matrixName : name of the matrix, "A" by default. - \param octet : if false, print using double, if true, print byte per byte - each bytes of the double array. + The following code + \code + #include - The following code shows how to use this function: - \code - #include + int main() + { + std::ofstream ofs("log.csv", std::ofstream::out); + vpMatrix M(2,3); + int cpt = 0; + for (unsigned int i=0; igetRows(); ++i) { + for (unsigned int j = 0; j < this->getCols(); ++j) { + os << (*this)[i][j]; + if (!(j == (this->getCols() - 1))) + os << ", "; } - \endcode - It produces the following output that could be copy/paste in a C++ code: - \code - vpMatrix M (2, 3); - M[0][0] = 0; - M[0][1] = 1; - M[0][2] = 2; - - M[1][0] = 3; - M[1][1] = 4; - M[1][2] = 5; - \endcode - */ - std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const + os << std::endl; + } + return os; +} + +/*! + Print to be used as part of a C++ code later. + + \param os : the stream to be printed in. + \param matrixName : name of the matrix, "A" by default. + \param octet : if false, print using double, if true, print byte per byte + each bytes of the double array. + + The following code shows how to use this function: + \code + #include + + int main() { - os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl; + vpMatrix M(2,3); + int cpt = 0; + for (unsigned int i=0; igetRows(); ++i) { - for (unsigned int j = 0; j < this->getCols(); ++j) { - if (!octet) { - os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl; - } - else { - for (unsigned int k = 0; k < sizeof(double); ++k) { - os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex - << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl; - } + M.cppPrint(std::cout, "M"); + } + \endcode + It produces the following output that could be copy/paste in a C++ code: + \code + vpMatrix M (2, 3); + M[0][0] = 0; + M[0][1] = 1; + M[0][2] = 2; + + M[1][0] = 3; + M[1][1] = 4; + M[1][2] = 5; + \endcode +*/ +std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const +{ + os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl; + + for (unsigned int i = 0; i < this->getRows(); ++i) { + for (unsigned int j = 0; j < this->getCols(); ++j) { + if (!octet) { + os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl; + } + else { + for (unsigned int k = 0; k < sizeof(double); ++k) { + os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex + << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl; } } - os << std::endl; } - return os; + os << std::endl; } + return os; +} - /*! - Stack A at the end of the current matrix, or copy if the matrix has no - dimensions : this = [ this A ]^T. - */ - void vpMatrix::stack(const vpMatrix &A) - { - if (rowNum == 0) { - *this = A; +/*! + Stack A at the end of the current matrix, or copy if the matrix has no + dimensions : this = [ this A ]^T. +*/ +void vpMatrix::stack(const vpMatrix &A) +{ + if (rowNum == 0) { + *this = A; + } + else if (A.getRows() > 0) { + if (colNum != A.getCols()) { + throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum, + A.getRows(), A.getCols())); } - else if (A.getRows() > 0) { - if (colNum != A.getCols()) { - throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum, - A.getRows(), A.getCols())); - } - unsigned int rowNumOld = rowNum; - resize(rowNum + A.getRows(), colNum, false, false); - insert(A, rowNumOld, 0); - } + unsigned int rowNumOld = rowNum; + resize(rowNum + A.getRows(), colNum, false, false); + insert(A, rowNumOld, 0); } +} - /*! - Stack row vector \e r at the end of the current matrix, or copy if the - matrix has no dimensions: this = [ this r ]^T. +/*! + Stack row vector \e r at the end of the current matrix, or copy if the + matrix has no dimensions: this = [ this r ]^T. - Here an example for a robot velocity log : - \code - vpMatrix A; - vpColVector v(6); - for(unsigned int i = 0;i<100;i++) - { - robot.getVelocity(vpRobot::ARTICULAR_FRAME, v); - Velocities.stack(v.t()); - } - \endcode - */ - void vpMatrix::stack(const vpRowVector &r) + Here an example for a robot velocity log : + \code + vpMatrix A; + vpColVector v(6); + for(unsigned int i = 0;i<100;i++) { - if (rowNum == 0) { - *this = r; + robot.getVelocity(vpRobot::ARTICULAR_FRAME, v); + Velocities.stack(v.t()); + } + \endcode +*/ +void vpMatrix::stack(const vpRowVector &r) +{ + if (rowNum == 0) { + *this = r; + } + else { + if (colNum != r.getCols()) { + throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum, + colNum, r.getCols())); } - else { - if (colNum != r.getCols()) { - throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum, - colNum, r.getCols())); - } - if (r.size() == 0) { - return; - } + if (r.size() == 0) { + return; + } - unsigned int oldSize = size(); - resize(rowNum + 1, colNum, false, false); + unsigned int oldSize = size(); + resize(rowNum + 1, colNum, false, false); - if (data != nullptr && r.data != nullptr && data != r.data) { - // Copy r in data - memcpy(data + oldSize, r.data, sizeof(double) * r.size()); - } + if (data != nullptr && r.data != nullptr && data != r.data) { + // Copy r in data + memcpy(data + oldSize, r.data, sizeof(double) * r.size()); } } +} - /*! - Stack column vector \e c at the right of the current matrix, or copy if the - matrix has no dimensions: this = [ this c ]. +/*! + Stack column vector \e c at the right of the current matrix, or copy if the + matrix has no dimensions: this = [ this c ]. - Here an example for a robot velocity log matrix: - \code - vpMatrix log; - vpColVector v(6); - for(unsigned int i = 0; i<100;i++) - { - robot.getVelocity(vpRobot::ARTICULAR_FRAME, v); - log.stack(v); - } - \endcode - Here the log matrix has size 6 rows by 100 columns. - */ - void vpMatrix::stack(const vpColVector &c) + Here an example for a robot velocity log matrix: + \code + vpMatrix log; + vpColVector v(6); + for(unsigned int i = 0; i<100;i++) { - if (colNum == 0) { - *this = c; + robot.getVelocity(vpRobot::ARTICULAR_FRAME, v); + log.stack(v); + } + \endcode + Here the log matrix has size 6 rows by 100 columns. +*/ +void vpMatrix::stack(const vpColVector &c) +{ + if (colNum == 0) { + *this = c; + } + else { + if (rowNum != c.getRows()) { + throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum, + colNum, c.getRows())); } - else { - if (rowNum != c.getRows()) { - throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum, - colNum, c.getRows())); - } - if (c.size() == 0) { - return; - } + if (c.size() == 0) { + return; + } - vpMatrix tmp = *this; - unsigned int oldColNum = colNum; - resize(rowNum, colNum + 1, false, false); + vpMatrix tmp = *this; + unsigned int oldColNum = colNum; + resize(rowNum, colNum + 1, false, false); - if (data != nullptr && tmp.data != nullptr && data != tmp.data) { - // Copy c in data - for (unsigned int i = 0; i < rowNum; i++) { - memcpy(data + i * colNum, tmp.data + i * oldColNum, sizeof(double) * oldColNum); - rowPtrs[i][oldColNum] = c[i]; - } + if (data != nullptr && tmp.data != nullptr && data != tmp.data) { + // Copy c in data + for (unsigned int i = 0; i < rowNum; i++) { + memcpy(data + i * colNum, tmp.data + i * oldColNum, sizeof(double) * oldColNum); + rowPtrs[i][oldColNum] = c[i]; } } } +} - /*! - Insert matrix A at the given position in the current matrix. +/*! + Insert matrix A at the given position in the current matrix. - \warning Throw vpException::dimensionError if the - dimensions of the matrices do not allow the operation. + \warning Throw vpException::dimensionError if the + dimensions of the matrices do not allow the operation. - \param A : The matrix to insert. - \param r : The index of the row to begin to insert data. - \param c : The index of the column to begin to insert data. - */ - void vpMatrix::insert(const vpMatrix &A, unsigned int r, unsigned int c) - { - if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) { - if (A.colNum == colNum && data != nullptr && A.data != nullptr && A.data != data) { - memcpy(data + r * colNum, A.data, sizeof(double) * A.size()); - } - else if (data != nullptr && A.data != nullptr && A.data != data) { - for (unsigned int i = r; i < (r + A.getRows()); i++) { - memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum); - } - } + \param A : The matrix to insert. + \param r : The index of the row to begin to insert data. + \param c : The index of the column to begin to insert data. +*/ +void vpMatrix::insert(const vpMatrix &A, unsigned int r, unsigned int c) +{ + if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) { + if (A.colNum == colNum && data != nullptr && A.data != nullptr && A.data != data) { + memcpy(data + r * colNum, A.data, sizeof(double) * A.size()); } - else { - throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)", - A.getRows(), A.getCols(), rowNum, colNum, r, c); + else if (data != nullptr && A.data != nullptr && A.data != data) { + for (unsigned int i = r; i < (r + A.getRows()); i++) { + memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum); + } } } + else { + throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)", + A.getRows(), A.getCols(), rowNum, colNum, r, c); + } +} - /*! - Compute the eigenvalues of a n-by-n real symmetric matrix using - Lapack 3rd party. +/*! + Compute the eigenvalues of a n-by-n real symmetric matrix using + Lapack 3rd party. - \return The eigenvalues of a n-by-n real symmetric matrix, sorted in ascending order. + \return The eigenvalues of a n-by-n real symmetric matrix, sorted in ascending order. - \exception vpException::dimensionError If the matrix is not square. - \exception vpException::fatalError If the matrix is not symmetric. - \exception vpException::functionNotImplementedError If the Lapack 3rd party - is not detected. + \exception vpException::dimensionError If the matrix is not square. + \exception vpException::fatalError If the matrix is not symmetric. + \exception vpException::functionNotImplementedError If the Lapack 3rd party + is not detected. - Here an example: - \code - #include - - #include - #include - - int main() - { - vpMatrix A(3,3); // A is a symmetric matrix - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; - A[1][0] = 1/2.; A[1][1] = 1/3.; A[1][2] = 1/4.; - A[2][0] = 1/3.; A[2][1] = 1/4.; A[2][2] = 1/5.; - std::cout << "Initial symmetric matrix: \n" << A << std::endl; - - // Compute the eigen values - vpColVector evalue; // Eigenvalues - evalue = A.eigenValues(); - std::cout << "Eigen values: \n" << evalue << std::endl; - } - \endcode + Here an example: + \code + #include - \sa eigenValues(vpColVector &, vpMatrix &) + #include + #include - */ - vpColVector vpMatrix::eigenValues() const + int main() { - vpColVector evalue(rowNum); // Eigen values + vpMatrix A(3,3); // A is a symmetric matrix + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; + A[1][0] = 1/2.; A[1][1] = 1/3.; A[1][2] = 1/4.; + A[2][0] = 1/3.; A[2][1] = 1/4.; A[2][2] = 1/5.; + std::cout << "Initial symmetric matrix: \n" << A << std::endl; - if (rowNum != colNum) { - throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum, - colNum)); - } + // Compute the eigen values + vpColVector evalue; // Eigenvalues + evalue = A.eigenValues(); + std::cout << "Eigen values: \n" << evalue << std::endl; + } + \endcode - // Check if the matrix is symmetric: At - A = 0 - vpMatrix At_A = (*this).t() - (*this); - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < rowNum; j++) { - // if (At_A[i][j] != 0) { - if (std::fabs(At_A[i][j]) > std::numeric_limits::epsilon()) { - throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix")); - } + \sa eigenValues(vpColVector &, vpMatrix &) + +*/ +vpColVector vpMatrix::eigenValues() const +{ + vpColVector evalue(rowNum); // Eigen values + + if (rowNum != colNum) { + throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum, + colNum)); + } + + // Check if the matrix is symmetric: At - A = 0 + vpMatrix At_A = (*this).t() - (*this); + for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int j = 0; j < rowNum; j++) { + // if (At_A[i][j] != 0) { + if (std::fabs(At_A[i][j]) > std::numeric_limits::epsilon()) { + throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix")); } } + } #if defined(VISP_HAVE_LAPACK) #if defined(VISP_HAVE_GSL) /* be careful of the copy below */ - { - gsl_vector *eval = gsl_vector_alloc(rowNum); - gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum); - - gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum); - gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum); + { + gsl_vector *eval = gsl_vector_alloc(rowNum); + gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum); - unsigned int Atda = (unsigned int)m->tda; - for (unsigned int i = 0; i < rowNum; i++) { - unsigned int k = i * Atda; - for (unsigned int j = 0; j < colNum; j++) - m->data[k + j] = (*this)[i][j]; - } - gsl_eigen_symmv(m, eval, evec, w); + gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum); + gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum); - gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC); + unsigned int Atda = (unsigned int)m->tda; + for (unsigned int i = 0; i < rowNum; i++) { + unsigned int k = i * Atda; + for (unsigned int j = 0; j < colNum; j++) + m->data[k + j] = (*this)[i][j]; + } + gsl_eigen_symmv(m, eval, evec, w); - for (unsigned int i = 0; i < rowNum; i++) { - evalue[i] = gsl_vector_get(eval, i); - } + gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC); - gsl_eigen_symmv_free(w); - gsl_vector_free(eval); - gsl_matrix_free(m); - gsl_matrix_free(evec); + for (unsigned int i = 0; i < rowNum; i++) { + evalue[i] = gsl_vector_get(eval, i); } + + gsl_eigen_symmv_free(w); + gsl_vector_free(eval); + gsl_matrix_free(m); + gsl_matrix_free(evec); + } #else - { - const char jobz = 'N'; - const char uplo = 'U'; - vpMatrix A = (*this); - vpColVector WORK; - int lwork = -1; - int info = 0; - double wkopt; - vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info); - lwork = static_cast(wkopt); - WORK.resize(lwork); - vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info); - } + { + const char jobz = 'N'; + const char uplo = 'U'; + vpMatrix A = (*this); + vpColVector WORK; + int lwork = -1; + int info = 0; + double wkopt; + vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info); + lwork = static_cast(wkopt); + WORK.resize(lwork); + vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info); + } #endif #else - { - throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. " - "You should install Lapack 3rd party")); - } -#endif - return evalue; + { + throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. " + "You should install Lapack 3rd party")); } +#endif + return evalue; +} - /*! - Compute the eigenvalues of a n-by-n real symmetric matrix using - Lapack 3rd party. +/*! + Compute the eigenvalues of a n-by-n real symmetric matrix using + Lapack 3rd party. - \param evalue : Eigenvalues of the matrix, sorted in ascending order. + \param evalue : Eigenvalues of the matrix, sorted in ascending order. - \param evector : Corresponding eigenvectors of the matrix. + \param evector : Corresponding eigenvectors of the matrix. - \exception vpException::dimensionError If the matrix is not square. - \exception vpException::fatalError If the matrix is not symmetric. - \exception vpException::functionNotImplementedError If Lapack 3rd party is - not detected. + \exception vpException::dimensionError If the matrix is not square. + \exception vpException::fatalError If the matrix is not symmetric. + \exception vpException::functionNotImplementedError If Lapack 3rd party is + not detected. - Here an example: - \code - #include + Here an example: + \code + #include - #include - #include + #include + #include - int main() - { - vpMatrix A(4,4); // A is a symmetric matrix - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; - A[1][0] = 1/2.; A[1][1] = 1/3.; A[1][2] = 1/4.; A[1][3] = 1/5.; - A[2][0] = 1/3.; A[2][1] = 1/4.; A[2][2] = 1/5.; A[2][3] = 1/6.; - A[3][0] = 1/4.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; - std::cout << "Initial symmetric matrix: \n" << A << std::endl; + int main() + { + vpMatrix A(4,4); // A is a symmetric matrix + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; + A[1][0] = 1/2.; A[1][1] = 1/3.; A[1][2] = 1/4.; A[1][3] = 1/5.; + A[2][0] = 1/3.; A[2][1] = 1/4.; A[2][2] = 1/5.; A[2][3] = 1/6.; + A[3][0] = 1/4.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; + std::cout << "Initial symmetric matrix: \n" << A << std::endl; - vpColVector d; // Eigenvalues - vpMatrix V; // Eigenvectors + vpColVector d; // Eigenvalues + vpMatrix V; // Eigenvectors - // Compute the eigenvalues and eigenvectors - A.eigenValues(d, V); - std::cout << "Eigen values: \n" << d << std::endl; - std::cout << "Eigen vectors: \n" << V << std::endl; + // Compute the eigenvalues and eigenvectors + A.eigenValues(d, V); + std::cout << "Eigen values: \n" << d << std::endl; + std::cout << "Eigen vectors: \n" << V << std::endl; - vpMatrix D; - D.diag(d); // Eigenvalues are on the diagonal + vpMatrix D; + D.diag(d); // Eigenvalues are on the diagonal - std::cout << "D: " << D << std::endl; + std::cout << "D: " << D << std::endl; - // Verification: A * V = V * D - std::cout << "AV-VD = 0 ? \n" << (A*V) - (V*D) << std::endl; - } - \endcode + // Verification: A * V = V * D + std::cout << "AV-VD = 0 ? \n" << (A*V) - (V*D) << std::endl; + } + \endcode - \sa eigenValues() - */ - void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const - { - if (rowNum != colNum) { - throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum, - colNum)); - } + \sa eigenValues() +*/ +void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const +{ + if (rowNum != colNum) { + throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum, + colNum)); + } - // Check if the matrix is symmetric: At - A = 0 - vpMatrix At_A = (*this).t() - (*this); - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < rowNum; j++) { - // if (At_A[i][j] != 0) { - if (std::fabs(At_A[i][j]) > std::numeric_limits::epsilon()) { - throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix")); - } + // Check if the matrix is symmetric: At - A = 0 + vpMatrix At_A = (*this).t() - (*this); + for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int j = 0; j < rowNum; j++) { + // if (At_A[i][j] != 0) { + if (std::fabs(At_A[i][j]) > std::numeric_limits::epsilon()) { + throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix")); } } + } - // Resize the output matrices - evalue.resize(rowNum); - evector.resize(rowNum, colNum); + // Resize the output matrices + evalue.resize(rowNum); + evector.resize(rowNum, colNum); #if defined(VISP_HAVE_LAPACK) #if defined(VISP_HAVE_GSL) /* be careful of the copy below */ - { - gsl_vector *eval = gsl_vector_alloc(rowNum); - gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum); + { + gsl_vector *eval = gsl_vector_alloc(rowNum); + gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum); - gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum); - gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum); + gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum); + gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum); - unsigned int Atda = (unsigned int)m->tda; - for (unsigned int i = 0; i < rowNum; i++) { - unsigned int k = i * Atda; - for (unsigned int j = 0; j < colNum; j++) - m->data[k + j] = (*this)[i][j]; - } - gsl_eigen_symmv(m, eval, evec, w); + unsigned int Atda = (unsigned int)m->tda; + for (unsigned int i = 0; i < rowNum; i++) { + unsigned int k = i * Atda; + for (unsigned int j = 0; j < colNum; j++) + m->data[k + j] = (*this)[i][j]; + } + gsl_eigen_symmv(m, eval, evec, w); - gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC); + gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC); - for (unsigned int i = 0; i < rowNum; i++) { - evalue[i] = gsl_vector_get(eval, i); - } - Atda = (unsigned int)evec->tda; - for (unsigned int i = 0; i < rowNum; i++) { - unsigned int k = i * Atda; - for (unsigned int j = 0; j < rowNum; j++) { - evector[i][j] = evec->data[k + j]; - } + for (unsigned int i = 0; i < rowNum; i++) { + evalue[i] = gsl_vector_get(eval, i); + } + Atda = (unsigned int)evec->tda; + for (unsigned int i = 0; i < rowNum; i++) { + unsigned int k = i * Atda; + for (unsigned int j = 0; j < rowNum; j++) { + evector[i][j] = evec->data[k + j]; } - - gsl_eigen_symmv_free(w); - gsl_vector_free(eval); - gsl_matrix_free(m); - gsl_matrix_free(evec); } + + gsl_eigen_symmv_free(w); + gsl_vector_free(eval); + gsl_matrix_free(m); + gsl_matrix_free(evec); + } #else // defined(VISP_HAVE_GSL) - { - const char jobz = 'V'; - const char uplo = 'U'; - vpMatrix A = (*this); - vpColVector WORK; - int lwork = -1; - int info = 0; - double wkopt; - vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info); - lwork = static_cast(wkopt); - WORK.resize(lwork); - vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info); - evector = A.t(); - } + { + const char jobz = 'V'; + const char uplo = 'U'; + vpMatrix A = (*this); + vpColVector WORK; + int lwork = -1; + int info = 0; + double wkopt; + vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info); + lwork = static_cast(wkopt); + WORK.resize(lwork); + vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info); + evector = A.t(); + } #endif // defined(VISP_HAVE_GSL) #else - { - throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. " - "You should install Lapack 3rd party")); - } -#endif + { + throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. " + "You should install Lapack 3rd party")); } +#endif +} - /*! - Function to compute the null space (the kernel) of a m-by-n matrix \f$\bf - A\f$. +/*! + Function to compute the null space (the kernel) of a m-by-n matrix \f$\bf + A\f$. - The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A}) - = { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$. + The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A}) + = { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$. - \param kerAt: The matrix that contains the null space (kernel) of \f$\bf - A\f$ defined by the matrix \f${\bf X}^T\f$. If matrix \f$\bf A\f$ is full - rank, the dimension of \c kerAt is (0, n), otherwise the dimension is (n-r, - n). This matrix is thus the transpose of \f$\mbox{Ker}({\bf A})\f$. + \param kerAt: The matrix that contains the null space (kernel) of \f$\bf + A\f$ defined by the matrix \f${\bf X}^T\f$. If matrix \f$\bf A\f$ is full + rank, the dimension of \c kerAt is (0, n), otherwise the dimension is (n-r, + n). This matrix is thus the transpose of \f$\mbox{Ker}({\bf A})\f$. - \param svThreshold: Threshold used to test the singular values. If - a singular value is lower than this threshold we consider that the - matrix is not full rank. + \param svThreshold: Threshold used to test the singular values. If + a singular value is lower than this threshold we consider that the + matrix is not full rank. - \return The rank of the matrix. - */ - unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const - { - unsigned int nbline = getRows(); - unsigned int nbcol = getCols(); + \return The rank of the matrix. +*/ +unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const +{ + unsigned int nbline = getRows(); + unsigned int nbcol = getCols(); - vpMatrix U, V; // Copy of the matrix, SVD function is destructive - vpColVector sv; - sv.resize(nbcol, false); // singular values - V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition + vpMatrix U, V; // Copy of the matrix, SVD function is destructive + vpColVector sv; + sv.resize(nbcol, false); // singular values + V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition - // Copy and resize matrix to have at least as many rows as columns - // kernel is computed in svd method only if the matrix has more rows than - // columns + // Copy and resize matrix to have at least as many rows as columns + // kernel is computed in svd method only if the matrix has more rows than + // columns - if (nbline < nbcol) - U.resize(nbcol, nbcol, true); - else - U.resize(nbline, nbcol, false); + if (nbline < nbcol) + U.resize(nbcol, nbcol, true); + else + U.resize(nbline, nbcol, false); - U.insert(*this, 0, 0); + U.insert(*this, 0, 0); - U.svd(sv, V); + U.svd(sv, V); - // Compute the highest singular value and rank of the matrix - double maxsv = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv) { - maxsv = sv[i]; - } + // Compute the highest singular value and rank of the matrix + double maxsv = 0; + for (unsigned int i = 0; i < nbcol; i++) { + if (sv[i] > maxsv) { + maxsv = sv[i]; } + } - unsigned int rank = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv * svThreshold) { - rank++; - } + unsigned int rank = 0; + for (unsigned int i = 0; i < nbcol; i++) { + if (sv[i] > maxsv * svThreshold) { + rank++; } + } - kerAt.resize(nbcol - rank, nbcol); - if (rank != nbcol) { - for (unsigned int j = 0, k = 0; j < nbcol; j++) { - // if( v.col(j) in kernel and non zero ) - if ((sv[j] <= maxsv * svThreshold) && - (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits::epsilon())) { - for (unsigned int i = 0; i < V.getRows(); i++) { - kerAt[k][i] = V[i][j]; - } - k++; + kerAt.resize(nbcol - rank, nbcol); + if (rank != nbcol) { + for (unsigned int j = 0, k = 0; j < nbcol; j++) { + // if( v.col(j) in kernel and non zero ) + if ((sv[j] <= maxsv * svThreshold) && + (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits::epsilon())) { + for (unsigned int i = 0; i < V.getRows(); i++) { + kerAt[k][i] = V[i][j]; } + k++; } } - - return rank; } - /*! - Function to compute the null space (the kernel) of a m-by-n matrix \f$\bf - A\f$. + return rank; +} + +/*! + Function to compute the null space (the kernel) of a m-by-n matrix \f$\bf + A\f$. - The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A}) - = { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$. + The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A}) + = { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$. - \param kerA: The matrix that contains the null space (kernel) of \f$\bf - A\f$. If matrix \f$\bf A\f$ is full rank, the dimension of \c kerA is (n, 0), - otherwise its dimension is (n, n-r). + \param kerA: The matrix that contains the null space (kernel) of \f$\bf + A\f$. If matrix \f$\bf A\f$ is full rank, the dimension of \c kerA is (n, 0), + otherwise its dimension is (n, n-r). - \param svThreshold: Threshold used to test the singular values. The dimension - of kerA corresponds to the number of singular values lower than this threshold + \param svThreshold: Threshold used to test the singular values. The dimension + of kerA corresponds to the number of singular values lower than this threshold - \return The dimension of the nullspace, that is \f$ n - r \f$. - */ - unsigned int vpMatrix::nullSpace(vpMatrix &kerA, double svThreshold) const - { - unsigned int nbrow = getRows(); - unsigned int nbcol = getCols(); + \return The dimension of the nullspace, that is \f$ n - r \f$. +*/ +unsigned int vpMatrix::nullSpace(vpMatrix &kerA, double svThreshold) const +{ + unsigned int nbrow = getRows(); + unsigned int nbcol = getCols(); - vpMatrix U, V; // Copy of the matrix, SVD function is destructive - vpColVector sv; - sv.resize(nbcol, false); // singular values - V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition + vpMatrix U, V; // Copy of the matrix, SVD function is destructive + vpColVector sv; + sv.resize(nbcol, false); // singular values + V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition - // Copy and resize matrix to have at least as many rows as columns - // kernel is computed in svd method only if the matrix has more rows than - // columns + // Copy and resize matrix to have at least as many rows as columns + // kernel is computed in svd method only if the matrix has more rows than + // columns - if (nbrow < nbcol) - U.resize(nbcol, nbcol, true); - else - U.resize(nbrow, nbcol, false); + if (nbrow < nbcol) + U.resize(nbcol, nbcol, true); + else + U.resize(nbrow, nbcol, false); - U.insert(*this, 0, 0); + U.insert(*this, 0, 0); - U.svd(sv, V); + U.svd(sv, V); - // Compute the highest singular value and rank of the matrix - double maxsv = sv[0]; + // Compute the highest singular value and rank of the matrix + double maxsv = sv[0]; - unsigned int rank = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv * svThreshold) { - rank++; - } + unsigned int rank = 0; + for (unsigned int i = 0; i < nbcol; i++) { + if (sv[i] > maxsv * svThreshold) { + rank++; } + } - kerA.resize(nbcol, nbcol - rank); - if (rank != nbcol) { - for (unsigned int j = 0, k = 0; j < nbcol; j++) { - // if( v.col(j) in kernel and non zero ) - if (sv[j] <= maxsv * svThreshold) { - for (unsigned int i = 0; i < nbcol; i++) { - kerA[i][k] = V[i][j]; - } - k++; + kerA.resize(nbcol, nbcol - rank); + if (rank != nbcol) { + for (unsigned int j = 0, k = 0; j < nbcol; j++) { + // if( v.col(j) in kernel and non zero ) + if (sv[j] <= maxsv * svThreshold) { + for (unsigned int i = 0; i < nbcol; i++) { + kerA[i][k] = V[i][j]; } + k++; } } - - return (nbcol - rank); } - /*! - Function to compute the null space (the kernel) of a m-by-n matrix \f$\bf - A\f$. + return (nbcol - rank); +} - The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A}) - = { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$. +/*! + Function to compute the null space (the kernel) of a m-by-n matrix \f$\bf + A\f$. - \param kerA: The matrix that contains the null space (kernel) of \f$\bf - A\f$. If matrix \f$\bf A\f$ is full rank, the dimension of \c kerA is (n, 0), - otherwise its dimension is (n, n-r). + The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A}) + = { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$. - \param dim: the dimension of the null space when it is known a priori + \param kerA: The matrix that contains the null space (kernel) of \f$\bf + A\f$. If matrix \f$\bf A\f$ is full rank, the dimension of \c kerA is (n, 0), + otherwise its dimension is (n, n-r). - \return The estimated dimension of the nullspace, that is \f$ n - r \f$, by - using 1e-6 as threshold for the sigular values. - */ - unsigned int vpMatrix::nullSpace(vpMatrix &kerA, int dim) const - { - unsigned int nbrow = getRows(); - unsigned int nbcol = getCols(); - unsigned int dim_ = static_cast(dim); + \param dim: the dimension of the null space when it is known a priori - vpMatrix U, V; // Copy of the matrix, SVD function is destructive - vpColVector sv; - sv.resize(nbcol, false); // singular values - V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition + \return The estimated dimension of the nullspace, that is \f$ n - r \f$, by + using 1e-6 as threshold for the sigular values. +*/ +unsigned int vpMatrix::nullSpace(vpMatrix &kerA, int dim) const +{ + unsigned int nbrow = getRows(); + unsigned int nbcol = getCols(); + unsigned int dim_ = static_cast(dim); - // Copy and resize matrix to have at least as many rows as columns - // kernel is computed in svd method only if the matrix has more rows than - // columns + vpMatrix U, V; // Copy of the matrix, SVD function is destructive + vpColVector sv; + sv.resize(nbcol, false); // singular values + V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition - if (nbrow < nbcol) - U.resize(nbcol, nbcol, true); - else - U.resize(nbrow, nbcol, false); + // Copy and resize matrix to have at least as many rows as columns + // kernel is computed in svd method only if the matrix has more rows than + // columns - U.insert(*this, 0, 0); + if (nbrow < nbcol) + U.resize(nbcol, nbcol, true); + else + U.resize(nbrow, nbcol, false); - U.svd(sv, V); + U.insert(*this, 0, 0); - kerA.resize(nbcol, dim_); - if (dim_ != 0) { - unsigned int rank = nbcol - dim_; - for (unsigned int k = 0; k < dim_; k++) { - unsigned int j = k + rank; - for (unsigned int i = 0; i < nbcol; i++) { - kerA[i][k] = V[i][j]; - } + U.svd(sv, V); + + kerA.resize(nbcol, dim_); + if (dim_ != 0) { + unsigned int rank = nbcol - dim_; + for (unsigned int k = 0; k < dim_; k++) { + unsigned int j = k + rank; + for (unsigned int i = 0; i < nbcol; i++) { + kerA[i][k] = V[i][j]; } } + } - double maxsv = sv[0]; - unsigned int rank = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv * 1e-6) { - rank++; - } + double maxsv = sv[0]; + unsigned int rank = 0; + for (unsigned int i = 0; i < nbcol; i++) { + if (sv[i] > maxsv * 1e-6) { + rank++; } - return (nbcol - rank); } + return (nbcol - rank); +} - /*! - Compute the determinant of a n-by-n matrix. +/*! + Compute the determinant of a n-by-n matrix. - \param method : Method used to compute the determinant. Default LU - decomposition method is faster than the method based on Gaussian - elimination. + \param method : Method used to compute the determinant. Default LU + decomposition method is faster than the method based on Gaussian + elimination. - \return Determinant of the matrix. + \return Determinant of the matrix. - \code - #include - - #include - - int main() - { - vpMatrix A(3,3); - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; - A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.; - A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.; - std::cout << "Initial matrix: \n" << A << std::endl; - - // Compute the determinant - std:: cout << "Determinant by default method : " << A.det() << std::endl; - std:: cout << "Determinant by LU decomposition : " << A.detByLU() << std::endl; - std:: cout << "Determinant by LU decomposition (Lapack): " << A.detByLULapack() << std::endl; - std:: cout << "Determinant by LU decomposition (OpenCV): " << A.detByLUOpenCV() << std::endl; - std:: cout << "Determinant by LU decomposition (Eigen3): " << A.detByLUEigen3() << std::endl; - } - \endcode - */ - double vpMatrix::det(vpDetMethod method) const - { - double det = 0.; + \code + #include - if (method == LU_DECOMPOSITION) { - det = this->detByLU(); - } + #include - return (det); + int main() + { + vpMatrix A(3,3); + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; + A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.; + A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.; + std::cout << "Initial matrix: \n" << A << std::endl; + + // Compute the determinant + std:: cout << "Determinant by default method : " << A.det() << std::endl; + std:: cout << "Determinant by LU decomposition : " << A.detByLU() << std::endl; + std:: cout << "Determinant by LU decomposition (Lapack): " << A.detByLULapack() << std::endl; + std:: cout << "Determinant by LU decomposition (OpenCV): " << A.detByLUOpenCV() << std::endl; + std:: cout << "Determinant by LU decomposition (Eigen3): " << A.detByLUEigen3() << std::endl; } + \endcode +*/ +double vpMatrix::det(vpDetMethod method) const +{ + double det = 0.; - /*! + if (method == LU_DECOMPOSITION) { + det = this->detByLU(); + } - Compute the exponential matrix of a square matrix. + return (det); +} - \return Return the exponential matrix. +/*! - */ - vpMatrix vpMatrix::expm() const - { - if (colNum != rowNum) { - throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix", - rowNum, colNum)); - } - else { + Compute the exponential matrix of a square matrix. + + \return Return the exponential matrix. + +*/ +vpMatrix vpMatrix::expm() const +{ + if (colNum != rowNum) { + throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix", + rowNum, colNum)); + } + else { #ifdef VISP_HAVE_GSL - size_t size_ = rowNum * colNum; - double *b = new double[size_]; - for (size_t i = 0; i < size_; i++) - b[i] = 0.; - gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum); - gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum); - gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0); - // gsl_matrix_fprintf(stdout, &em.matrix, "%g"); - vpMatrix expA; - expA.resize(rowNum, colNum, false); - memcpy(expA.data, b, size_ * sizeof(double)); - - delete[] b; - return expA; + size_t size_ = rowNum * colNum; + double *b = new double[size_]; + for (size_t i = 0; i < size_; i++) + b[i] = 0.; + gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum); + gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum); + gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0); + // gsl_matrix_fprintf(stdout, &em.matrix, "%g"); + vpMatrix expA; + expA.resize(rowNum, colNum, false); + memcpy(expA.data, b, size_ * sizeof(double)); + + delete[] b; + return expA; #else - vpMatrix _expE(rowNum, colNum, false); - vpMatrix _expD(rowNum, colNum, false); - vpMatrix _expX(rowNum, colNum, false); - vpMatrix _expcX(rowNum, colNum, false); - vpMatrix _eye(rowNum, colNum, false); - - _eye.eye(); - vpMatrix exp(*this); - - // double f; - int e; - double c = 0.5; - int q = 6; - int p = 1; - - double nA = 0; - for (unsigned int i = 0; i < rowNum; i++) { - double sum = 0; - for (unsigned int j = 0; j < colNum; j++) { - sum += fabs((*this)[i][j]); - } - if (sum > nA || i == 0) { - nA = sum; - } - } - - /* f = */ frexp(nA, &e); - // double s = (0 > e+1)?0:e+1; - double s = e + 1; - - double sca = 1.0 / pow(2.0, s); - exp = sca * exp; - _expX = *this; - _expE = c * exp + _eye; - _expD = -c * exp + _eye; - for (int k = 2; k <= q; k++) { - c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1))); - _expcX = exp * _expX; - _expX = _expcX; - _expcX = c * _expX; - _expE = _expE + _expcX; - if (p) - _expD = _expD + _expcX; - else - _expD = _expD - _expcX; - p = !p; + vpMatrix _expE(rowNum, colNum, false); + vpMatrix _expD(rowNum, colNum, false); + vpMatrix _expX(rowNum, colNum, false); + vpMatrix _expcX(rowNum, colNum, false); + vpMatrix _eye(rowNum, colNum, false); + + _eye.eye(); + vpMatrix exp(*this); + + // double f; + int e; + double c = 0.5; + int q = 6; + int p = 1; + + double nA = 0; + for (unsigned int i = 0; i < rowNum; i++) { + double sum = 0; + for (unsigned int j = 0; j < colNum; j++) { + sum += fabs((*this)[i][j]); } - _expX = _expD.inverseByLU(); - exp = _expX * _expE; - for (int k = 1; k <= s; k++) { - _expE = exp * exp; - exp = _expE; + if (sum > nA || i == 0) { + nA = sum; } - return exp; -#endif } - } - /**************************************************************************************************************/ - /**************************************************************************************************************/ + /* f = */ frexp(nA, &e); + // double s = (0 > e+1)?0:e+1; + double s = e + 1; + + double sca = 1.0 / pow(2.0, s); + exp = sca * exp; + _expX = *this; + _expE = c * exp + _eye; + _expD = -c * exp + _eye; + for (int k = 2; k <= q; k++) { + c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1))); + _expcX = exp * _expX; + _expX = _expcX; + _expcX = c * _expX; + _expE = _expE + _expcX; + if (p) + _expD = _expD + _expcX; + else + _expD = _expD - _expcX; + p = !p; + } + _expX = _expD.inverseByLU(); + exp = _expX * _expE; + for (int k = 1; k <= s; k++) { + _expE = exp * exp; + exp = _expE; + } + return exp; +#endif + } +} - // Specific functions +/**************************************************************************************************************/ +/**************************************************************************************************************/ - /* - input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows. +// Specific functions - output:: the complement matrix of the element (rowNo,colNo). - This is the matrix obtained from M after elimenating the row rowNo and column - colNo +/* + input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows. - example: - 1 2 3 - M = 4 5 6 - 7 8 9 - 1 3 - subblock(M, 1, 1) give the matrix 7 9 - */ - vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row) - { - vpMatrix M_comp; - M_comp.resize(M.getRows() - 1, M.getCols() - 1, false); - - for (unsigned int i = 0; i < col; i++) { - for (unsigned int j = 0; j < row; j++) - M_comp[i][j] = M[i][j]; - for (unsigned int j = row + 1; j < M.getRows(); j++) - M_comp[i][j - 1] = M[i][j]; - } - for (unsigned int i = col + 1; i < M.getCols(); i++) { - for (unsigned int j = 0; j < row; j++) - M_comp[i - 1][j] = M[i][j]; - for (unsigned int j = row + 1; j < M.getRows(); j++) - M_comp[i - 1][j - 1] = M[i][j]; - } - return M_comp; - } + output:: the complement matrix of the element (rowNo,colNo). + This is the matrix obtained from M after elimenating the row rowNo and column + colNo - /*! - \return The condition number, the ratio of the largest singular value of - the matrix to the smallest. + example: + 1 2 3 + M = 4 5 6 + 7 8 9 + 1 3 + subblock(M, 1, 1) give the matrix 7 9 +*/ +vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row) +{ + vpMatrix M_comp; + M_comp.resize(M.getRows() - 1, M.getCols() - 1, false); + + for (unsigned int i = 0; i < col; i++) { + for (unsigned int j = 0; j < row; j++) + M_comp[i][j] = M[i][j]; + for (unsigned int j = row + 1; j < M.getRows(); j++) + M_comp[i][j - 1] = M[i][j]; + } + for (unsigned int i = col + 1; i < M.getCols(); i++) { + for (unsigned int j = 0; j < row; j++) + M_comp[i - 1][j] = M[i][j]; + for (unsigned int j = row + 1; j < M.getRows(); j++) + M_comp[i - 1][j - 1] = M[i][j]; + } + return M_comp; +} - \param svThreshold: Threshold used to test the singular values. If - a singular value is lower than this threshold we consider that the - matrix is not full rank. +/*! + \return The condition number, the ratio of the largest singular value of + the matrix to the smallest. - */ - double vpMatrix::cond(double svThreshold) const - { - unsigned int nbline = getRows(); - unsigned int nbcol = getCols(); + \param svThreshold: Threshold used to test the singular values. If + a singular value is lower than this threshold we consider that the + matrix is not full rank. - vpMatrix U, V; // Copy of the matrix, SVD function is destructive - vpColVector sv; - sv.resize(nbcol); // singular values - V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition + */ +double vpMatrix::cond(double svThreshold) const +{ + unsigned int nbline = getRows(); + unsigned int nbcol = getCols(); - // Copy and resize matrix to have at least as many rows as columns - // kernel is computed in svd method only if the matrix has more rows than - // columns + vpMatrix U, V; // Copy of the matrix, SVD function is destructive + vpColVector sv; + sv.resize(nbcol); // singular values + V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition - if (nbline < nbcol) - U.resize(nbcol, nbcol, true); - else - U.resize(nbline, nbcol, false); + // Copy and resize matrix to have at least as many rows as columns + // kernel is computed in svd method only if the matrix has more rows than + // columns - U.insert(*this, 0, 0); + if (nbline < nbcol) + U.resize(nbcol, nbcol, true); + else + U.resize(nbline, nbcol, false); - U.svd(sv, V); + U.insert(*this, 0, 0); - // Compute the highest singular value - double maxsv = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv) { - maxsv = sv[i]; - } - } + U.svd(sv, V); - // Compute the rank of the matrix - unsigned int rank = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv * svThreshold) { - rank++; - } + // Compute the highest singular value + double maxsv = 0; + for (unsigned int i = 0; i < nbcol; i++) { + if (sv[i] > maxsv) { + maxsv = sv[i]; } + } - // Compute the lowest singular value - double minsv = maxsv; - for (unsigned int i = 0; i < rank; i++) { - if (sv[i] < minsv) { - minsv = sv[i]; - } + // Compute the rank of the matrix + unsigned int rank = 0; + for (unsigned int i = 0; i < nbcol; i++) { + if (sv[i] > maxsv * svThreshold) { + rank++; } + } - if (std::fabs(minsv) > std::numeric_limits::epsilon()) { - return maxsv / minsv; - } - else { - return std::numeric_limits::infinity(); + // Compute the lowest singular value + double minsv = maxsv; + for (unsigned int i = 0; i < rank; i++) { + if (sv[i] < minsv) { + minsv = sv[i]; } } - /*! - Compute \f${\bf H} + \alpha * diag({\bf H})\f$ - \param H : input Matrix \f${\bf H}\f$. This matrix should be square. - \param alpha : Scalar \f$\alpha\f$ - \param HLM : Resulting operation. - */ - void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM) - { - if (H.getCols() != H.getRows()) { - throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(), - H.getCols())); - } + if (std::fabs(minsv) > std::numeric_limits::epsilon()) { + return maxsv / minsv; + } + else { + return std::numeric_limits::infinity(); + } +} - HLM = H; - for (unsigned int i = 0; i < H.getCols(); i++) { - HLM[i][i] += alpha * H[i][i]; - } +/*! + Compute \f${\bf H} + \alpha * diag({\bf H})\f$ + \param H : input Matrix \f${\bf H}\f$. This matrix should be square. + \param alpha : Scalar \f$\alpha\f$ + \param HLM : Resulting operation. + */ +void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM) +{ + if (H.getCols() != H.getRows()) { + throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(), + H.getCols())); } - /*! - Compute and return the Frobenius norm (also called Euclidean norm) \f$||A|| = \sqrt{ \sum{A_{ij}^2}}\f$. + HLM = H; + for (unsigned int i = 0; i < H.getCols(); i++) { + HLM[i][i] += alpha * H[i][i]; + } +} - \return The Frobenius norm (also called Euclidean norm) if the matrix is initialized, 0 otherwise. +/*! + Compute and return the Frobenius norm (also called Euclidean norm) \f$||A|| = \sqrt{ \sum{A_{ij}^2}}\f$. - \sa infinityNorm(), inducedL2Norm() - */ - double vpMatrix::frobeniusNorm() const - { - double norm = 0.0; - for (unsigned int i = 0; i < dsize; i++) { - double x = *(data + i); - norm += x * x; - } + \return The Frobenius norm (also called Euclidean norm) if the matrix is initialized, 0 otherwise. - return sqrt(norm); + \sa infinityNorm(), inducedL2Norm() +*/ +double vpMatrix::frobeniusNorm() const +{ + double norm = 0.0; + for (unsigned int i = 0; i < dsize; i++) { + double x = *(data + i); + norm += x * x; } - /*! - Compute and return the induced L2 norm \f$||A|| = \Sigma_{max}(A)\f$ which is equal to - the maximum singular value of the matrix. + return sqrt(norm); +} - \return The induced L2 norm if the matrix is initialized, 0 otherwise. +/*! + Compute and return the induced L2 norm \f$||A|| = \Sigma_{max}(A)\f$ which is equal to + the maximum singular value of the matrix. - \sa infinityNorm(), frobeniusNorm() - */ - double vpMatrix::inducedL2Norm() const - { - if (this->dsize != 0) { - vpMatrix v; - vpColVector w; - - vpMatrix M = *this; - - M.svd(w, v); - - double max = w[0]; - unsigned int maxRank = std::min(this->getCols(), this->getRows()); - // The maximum reachable rank is either the number of columns or the number of rows - // of the matrix. - unsigned int boundary = std::min(maxRank, w.size()); - // boundary is here to ensure that the number of singular values used for the com- - // putation of the euclidean norm of the matrix is not greater than the maximum - // reachable rank. Indeed, some svd library pad the singular values vector with 0s - // if the input matrix is non-square. - for (unsigned int i = 0; i < boundary; i++) { - if (max < w[i]) { - max = w[i]; - } + \return The induced L2 norm if the matrix is initialized, 0 otherwise. + + \sa infinityNorm(), frobeniusNorm() +*/ +double vpMatrix::inducedL2Norm() const +{ + if (this->dsize != 0) { + vpMatrix v; + vpColVector w; + + vpMatrix M = *this; + + M.svd(w, v); + + double max = w[0]; + unsigned int maxRank = std::min(this->getCols(), this->getRows()); + // The maximum reachable rank is either the number of columns or the number of rows + // of the matrix. + unsigned int boundary = std::min(maxRank, w.size()); + // boundary is here to ensure that the number of singular values used for the com- + // putation of the euclidean norm of the matrix is not greater than the maximum + // reachable rank. Indeed, some svd library pad the singular values vector with 0s + // if the input matrix is non-square. + for (unsigned int i = 0; i < boundary; i++) { + if (max < w[i]) { + max = w[i]; } - return max; - } - else { - return 0.; } + return max; } + else { + return 0.; + } +} - /*! +/*! - Compute and return the infinity norm \f$ {||A||}_{\infty} = - max\left(\sum_{j=0}^{n}{\mid A_{ij} \mid}\right) \f$ with \f$i \in - \{0, ..., m\}\f$ where \f$(m,n)\f$ is the matrix size. + Compute and return the infinity norm \f$ {||A||}_{\infty} = + max\left(\sum_{j=0}^{n}{\mid A_{ij} \mid}\right) \f$ with \f$i \in + \{0, ..., m\}\f$ where \f$(m,n)\f$ is the matrix size. - \return The infinity norm if the matrix is initialized, 0 otherwise. + \return The infinity norm if the matrix is initialized, 0 otherwise. - \sa frobeniusNorm(), inducedL2Norm() - */ - double vpMatrix::infinityNorm() const - { - double norm = 0.0; - for (unsigned int i = 0; i < rowNum; i++) { - double x = 0; - for (unsigned int j = 0; j < colNum; j++) { - x += fabs(*(*(rowPtrs + i) + j)); - } - if (x > norm) { - norm = x; - } + \sa frobeniusNorm(), inducedL2Norm() +*/ +double vpMatrix::infinityNorm() const +{ + double norm = 0.0; + for (unsigned int i = 0; i < rowNum; i++) { + double x = 0; + for (unsigned int j = 0; j < colNum; j++) { + x += fabs(*(*(rowPtrs + i) + j)); + } + if (x > norm) { + norm = x; } - return norm; } + return norm; +} - /*! - Return the sum square of all the \f$A_{ij}\f$ elements of the matrix \f$A(m, - n)\f$. +/*! + Return the sum square of all the \f$A_{ij}\f$ elements of the matrix \f$A(m, + n)\f$. - \return The value \f$\sum A_{ij}^{2}\f$. - */ - double vpMatrix::sumSquare() const - { - double sum_square = 0.0; - double x; + \return The value \f$\sum A_{ij}^{2}\f$. +*/ +double vpMatrix::sumSquare() const +{ + double sum_square = 0.0; + double x; - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < colNum; j++) { - x = rowPtrs[i][j]; - sum_square += x * x; - } + for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int j = 0; j < colNum; j++) { + x = rowPtrs[i][j]; + sum_square += x * x; } - - return sum_square; } + + return sum_square; +} #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) /*! \deprecated This function is deprecated. You should rather use frobeniusNorm(). @@ -6793,89 +6525,89 @@ Vt.insert(kerAt, rank, 0); \sa frobeniusNorm(), infinityNorm(), inducedL2Norm() */ - double vpMatrix::euclideanNorm() const { return frobeniusNorm(); } +double vpMatrix::euclideanNorm() const { return frobeniusNorm(); } - vpMatrix vpMatrix::stackMatrices(const vpColVector &A, const vpColVector &B) - { - return (vpMatrix)(vpColVector::stack(A, B)); - } +vpMatrix vpMatrix::stackMatrices(const vpColVector &A, const vpColVector &B) +{ + return (vpMatrix)(vpColVector::stack(A, B)); +} - void vpMatrix::stackMatrices(const vpColVector &A, const vpColVector &B, vpColVector &C) - { - vpColVector::stack(A, B, C); - } +void vpMatrix::stackMatrices(const vpColVector &A, const vpColVector &B, vpColVector &C) +{ + vpColVector::stack(A, B, C); +} - vpMatrix vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B) { return vpMatrix::stack(A, B); } +vpMatrix vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B) { return vpMatrix::stack(A, B); } - void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); } +void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); } - /*! - \deprecated This method is deprecated. You should rather use getRow(). - More precisely, the following code: - \code - vpMatrix L; - unsigned int row_index = ...; - ... = L.row(row_index); - \endcode - should be replaced with: - \code - ... = L.getRow(row_index - 1); - \endcode +/*! + \deprecated This method is deprecated. You should rather use getRow(). + More precisely, the following code: + \code + vpMatrix L; + unsigned int row_index = ...; + ... = L.row(row_index); + \endcode + should be replaced with: + \code + ... = L.getRow(row_index - 1); + \endcode - \warning Notice row(1) is the 0th row. - This function returns the i-th row of the matrix. - \param i : Index of the row to extract noting that row index start at 1 to get the first row. + \warning Notice row(1) is the 0th row. + This function returns the i-th row of the matrix. + \param i : Index of the row to extract noting that row index start at 1 to get the first row. - */ - vpRowVector vpMatrix::row(unsigned int i) - { - vpRowVector c(getCols()); +*/ +vpRowVector vpMatrix::row(unsigned int i) +{ + vpRowVector c(getCols()); - for (unsigned int j = 0; j < getCols(); j++) - c[j] = (*this)[i - 1][j]; - return c; - } + for (unsigned int j = 0; j < getCols(); j++) + c[j] = (*this)[i - 1][j]; + return c; +} - /*! - \deprecated This method is deprecated. You should rather use getCol(). - More precisely, the following code: - \code - vpMatrix L; - unsigned int column_index = ...; - ... = L.column(column_index); - \endcode - should be replaced with: - \code - ... = L.getCol(column_index - 1); - \endcode +/*! + \deprecated This method is deprecated. You should rather use getCol(). + More precisely, the following code: + \code + vpMatrix L; + unsigned int column_index = ...; + ... = L.column(column_index); + \endcode + should be replaced with: + \code + ... = L.getCol(column_index - 1); + \endcode - \warning Notice column(1) is the 0-th column. - This function returns the j-th columns of the matrix. - \param j : Index of the column to extract noting that column index start at 1 to get the first column. - */ - vpColVector vpMatrix::column(unsigned int j) - { - vpColVector c(getRows()); + \warning Notice column(1) is the 0-th column. + This function returns the j-th columns of the matrix. + \param j : Index of the column to extract noting that column index start at 1 to get the first column. +*/ +vpColVector vpMatrix::column(unsigned int j) +{ + vpColVector c(getRows()); - for (unsigned int i = 0; i < getRows(); i++) - c[i] = (*this)[i][j - 1]; - return c; - } + for (unsigned int i = 0; i < getRows(); i++) + c[i] = (*this)[i][j - 1]; + return c; +} - /*! - \deprecated You should rather use diag(const double &) +/*! + \deprecated You should rather use diag(const double &) - Set the matrix diagonal elements to \e val. - More generally set M[i][i] = val. - */ - void vpMatrix::setIdentity(const double &val) - { - for (unsigned int i = 0; i < rowNum; i++) - for (unsigned int j = 0; j < colNum; j++) - if (i == j) - (*this)[i][j] = val; - else - (*this)[i][j] = 0; - } + Set the matrix diagonal elements to \e val. + More generally set M[i][i] = val. +*/ +void vpMatrix::setIdentity(const double &val) +{ + for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int j = 0; j < colNum; j++) + if (i == j) + (*this)[i][j] = val; + else + (*this)[i][j] = 0; +} #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) diff --git a/modules/core/src/math/matrix/vpMatrix_svd.cpp b/modules/core/src/math/matrix/vpMatrix_svd.cpp index 094ee588e3..f45551e674 100644 --- a/modules/core/src/math/matrix/vpMatrix_svd.cpp +++ b/modules/core/src/math/matrix/vpMatrix_svd.cpp @@ -85,8 +85,7 @@ SVD related functions Singular value decomposition (SVD) using OpenCV 3rd party. - Given matrix \f$M\f$, this function computes it singular value decomposition -such as + Given matrix \f$M\f$, this function computes it singular value decomposition such as \f[ M = U \Sigma V^{\top} \f] @@ -104,45 +103,45 @@ such as Here an example of SVD decomposition of a non square Matrix M. -\code -#include -#include - -int main() -{ - vpMatrix M(3,2); - M[0][0] = 1; - M[1][0] = 2; - M[2][0] = 0.5; - - M[0][1] = 6; - M[1][1] = 8 ; - M[2][1] = 9 ; - - vpMatrix V; - vpColVector w; - vpMatrix Mrec; - vpMatrix Sigma; - - M.svdOpenCV(w, V); - // Here M is modified and is now equal to U + \code + #include + #include - // Construct the diagonal matrix from the singular values - Sigma.diag(w); - - // Reconstruct the initial matrix M using the decomposition - Mrec = M * Sigma * V.t(); - - // Here, Mrec is obtained equal to the initial value of M - // Mrec[0][0] = 1; - // Mrec[1][0] = 2; - // Mrec[2][0] = 0.5; - // Mrec[0][1] = 6; - // Mrec[1][1] = 8 ; - // Mrec[2][1] = 9 ; - - std::cout << "Reconstructed M matrix: \n" << Mrec << std::endl; -} + int main() + { + vpMatrix M(3,2); + M[0][0] = 1; + M[1][0] = 2; + M[2][0] = 0.5; + + M[0][1] = 6; + M[1][1] = 8 ; + M[2][1] = 9 ; + + vpMatrix V; + vpColVector w; + vpMatrix Mrec; + vpMatrix Sigma; + + M.svdOpenCV(w, V); + // Here M is modified and is now equal to U + + // Construct the diagonal matrix from the singular values + Sigma.diag(w); + + // Reconstruct the initial matrix M using the decomposition + Mrec = M * Sigma * V.t(); + + // Here, Mrec is obtained equal to the initial value of M + // Mrec[0][0] = 1; + // Mrec[1][0] = 2; + // Mrec[2][0] = 0.5; + // Mrec[0][1] = 6; + // Mrec[1][1] = 8 ; + // Mrec[2][1] = 9 ; + + std::cout << "Reconstructed M matrix: \n" << Mrec << std::endl; + } \endcode \sa svd(), svdEigen3(), svdLapack() @@ -191,45 +190,45 @@ void vpMatrix::svdOpenCV(vpColVector &w, vpMatrix &V) Here an example of SVD decomposition of a non square Matrix M. -\code -#include -#include - -int main() -{ - vpMatrix M(3,2); - M[0][0] = 1; - M[1][0] = 2; - M[2][0] = 0.5; - - M[0][1] = 6; - M[1][1] = 8 ; - M[2][1] = 9 ; - - vpMatrix V; - vpColVector w; - vpMatrix Mrec; - vpMatrix Sigma; - - M.svdLapack(w, V); - // Here M is modified and is now equal to U - - // Construct the diagonal matrix from the singular values - Sigma.diag(w); - - // Reconstruct the initial matrix M using the decomposition - Mrec = M * Sigma * V.t(); - - // Here, Mrec is obtained equal to the initial value of M - // Mrec[0][0] = 1; - // Mrec[1][0] = 2; - // Mrec[2][0] = 0.5; - // Mrec[0][1] = 6; - // Mrec[1][1] = 8 ; - // Mrec[2][1] = 9 ; + \code + #include + #include - std::cout << "Reconstructed M matrix: \n" << Mrec << std::endl; -} + int main() + { + vpMatrix M(3,2); + M[0][0] = 1; + M[1][0] = 2; + M[2][0] = 0.5; + + M[0][1] = 6; + M[1][1] = 8 ; + M[2][1] = 9 ; + + vpMatrix V; + vpColVector w; + vpMatrix Mrec; + vpMatrix Sigma; + + M.svdLapack(w, V); + // Here M is modified and is now equal to U + + // Construct the diagonal matrix from the singular values + Sigma.diag(w); + + // Reconstruct the initial matrix M using the decomposition + Mrec = M * Sigma * V.t(); + + // Here, Mrec is obtained equal to the initial value of M + // Mrec[0][0] = 1; + // Mrec[1][0] = 2; + // Mrec[2][0] = 0.5; + // Mrec[0][1] = 6; + // Mrec[1][1] = 8 ; + // Mrec[2][1] = 9 ; + + std::cout << "Reconstructed M matrix: \n" << Mrec << std::endl; + } \endcode \sa svd(), svdEigen3(), svdOpenCV() @@ -289,7 +288,7 @@ void vpMatrix::svdLapack(vpColVector &w, vpMatrix &V) gsl_linalg_SV_decomp(&A, &V_, &S, work); if (rowNum < colNum) { - (*this) = V.transpose(); + (*this) = V; V = U; } @@ -297,26 +296,52 @@ void vpMatrix::svdLapack(vpColVector &w, vpMatrix &V) } #else { - w.resize(this->getCols()); - V.resize(this->getCols(), this->getCols()); - - integer m = (integer)(this->getCols()); - integer n = (integer)(this->getRows()); - integer lda = m; - integer ldu = m; - integer ldvt = std::min(m, n); + vpMatrix U; + unsigned int nc = getCols(); + unsigned int nr = getRows(); + + if (rowNum < colNum) { + U = this->transpose(); + nc = getRows(); + nr = getCols(); + } + else { + nc = getCols(); + nr = getRows(); + } + + w.resize(nc); + V.resize(nc, nc); + + double *a = new double[static_cast(nr * nc)]; + if (rowNum < colNum) { + memcpy(a, U.data, this->getRows() * this->getCols() * sizeof(double)); + } + else { + memcpy(a, this->data, this->getRows() * this->getCols() * sizeof(double)); + } + + integer m = (integer)(nc); + integer n = (integer)(nr); + integer lda = nc; + integer ldu = nc; + integer ldvt = std::min(nr, nc); integer info, lwork; double wkopt; double *work; - integer *iwork = new integer[8 * static_cast(std::min(n, m))]; + integer *iwork = new integer[8 * static_cast(std::min(nr, nc))]; double *s = w.data; - double *a = new double[static_cast(lda * n)]; - memcpy(a, this->data, this->getRows() * this->getCols() * sizeof(double)); double *u = V.data; - double *vt = this->data; + double *vt; + if (rowNum < colNum) { + vt = U.data; + } + else { + vt = this->data; + } lwork = -1; dgesdd_((char *)"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, &wkopt, &lwork, iwork, &info); @@ -329,7 +354,13 @@ void vpMatrix::svdLapack(vpColVector &w, vpMatrix &V) throw(vpMatrixException(vpMatrixException::fatalError, "The algorithm computing SVD failed to converge.")); } - V = V.transpose(); + if (rowNum < colNum) { + (*this) = V.transpose(); + V = U; + } + else { + V = V.transpose(); + } delete[] work; delete[] iwork; delete[] a; @@ -407,19 +438,29 @@ int main() */ void vpMatrix::svdEigen3(vpColVector &w, vpMatrix &V) { - w.resize(this->getCols()); - V.resize(this->getCols(), this->getCols()); + if (rowNum < colNum) { + w.resize(rowNum); + V.resize(colNum, rowNum); + } + else { + w.resize(colNum); + V.resize(colNum, colNum); + } Eigen::Map > M(this->data, this->getRows(), this->getCols()); - Eigen::JacobiSVD svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV); Eigen::Map w_(w.data, w.size()); - Eigen::Map > V_(V.data, V.getRows(), - V.getCols()); - Eigen::Map > U_(this->data, this->getRows(), - this->getCols()); + + if (rowNum < colNum) { + this->resize(rowNum, rowNum); + } + else { + this->resize(rowNum, colNum); + } + Eigen::Map > V_(V.data, V.getRows(), V.getCols()); + Eigen::Map > U_(this->data, rowNum, colNum); w_ = svd.singularValues(); V_ = svd.matrixV(); U_ = svd.matrixU(); diff --git a/modules/core/test/math/testMatrixPseudoInverse.cpp b/modules/core/test/math/testMatrixPseudoInverse.cpp index 5ca14afa35..914af147e7 100644 --- a/modules/core/test/math/testMatrixPseudoInverse.cpp +++ b/modules/core/test/math/testMatrixPseudoInverse.cpp @@ -200,14 +200,34 @@ void create_bench_random_matrix(unsigned int nb_matrices, unsigned int nb_rows, int test_pseudo_inverse(const std::vector &A, const std::vector &Api) { double allowed_error = 1e-3; + double error = 0; + vpMatrix A_Api, Api_A; for (unsigned int i = 0; i < A.size(); i++) { - double error = (A[i] * Api[i] * A[i] - A[i]).frobeniusNorm(); + error = (A[i] * Api[i] * A[i] - A[i]).frobeniusNorm(); if (error > allowed_error) { - std::cout << "Bad pseudo-inverse [" << i << "]: euclidean norm: " << error << std::endl; + std::cout << "Bad pseudo-inverse [" << i << "] test A A^+ A = A: euclidean norm: " << error << std::endl; + return EXIT_FAILURE; + } + error = (Api[i] * A[i] * Api[i] - Api[i]).frobeniusNorm(); + if (error > allowed_error) { + std::cout << "Bad pseudo-inverse [" << i << "] test A^+ A A^+ = A^+: euclidean norm: " << error << std::endl; + return EXIT_FAILURE; + } + A_Api = A[i] * Api[i]; + error = (A_Api.transpose() - A_Api).frobeniusNorm(); + if (error > allowed_error) { + std::cout << "Bad pseudo-inverse [" << i << "] test (A A^+)^T = A A^+: euclidean norm: " << error << std::endl; + return EXIT_FAILURE; + } + Api_A = Api[i] * A[i]; + error = (Api_A.transpose() - Api_A).frobeniusNorm(); + if (error > allowed_error) { + std::cout << "Bad pseudo-inverse [" << i << "] test (A^+ A )^T = A^+ A: euclidean norm: " << error << std::endl; return EXIT_FAILURE; } } + return EXIT_SUCCESS; } @@ -217,12 +237,8 @@ int test_pseudo_inverse(const std::vector &A, const std::vector allowed_error) { - std::cout << "Bad pseudo-inverse [" << i << "]: euclidean norm: " << error << std::endl; - return EXIT_FAILURE; - } + if (test_pseudo_inverse(A, Api) == EXIT_FAILURE) { + return EXIT_FAILURE; } // test kerA @@ -230,7 +246,6 @@ int test_pseudo_inverse(const std::vector &A, const std::vector allowed_error) { std::cout << "Bad kernel [" << i << "]: euclidean norm: " << error << std::endl; return EXIT_FAILURE; @@ -272,48 +287,163 @@ int test_pseudo_inverse_default(bool verbose, const std::vector &bench std::vector PI(size), imA(size), imAt(size), kerAt(size); std::vector sv(size); int ret = EXIT_SUCCESS; + time.clear(); - // test 0 - unsigned int test = 0; + // test 1 double t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { PI[i] = bench[i].pseudoInverse(); } - time[test] = vpTime::measureTimeMs() - t; + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI); } - // test 1 - test++; + // test 2 t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - bench[i].pseudoInverse(PI[i]); + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverse(PI[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } } - time[test] = vpTime::measureTimeMs() - t; + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI); } - // test 2 - test++; + // test 3 t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - bench[i].pseudoInverse(PI[i], sv[i]); + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverse(PI[i], sv[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } } - time[test] = vpTime::measureTimeMs() - t; + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI); } - // test 3 - test++; + // test 4 t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - bench[i].pseudoInverse(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]); + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverse(PI[i], sv[i], 1e-6, imA[i], imAt[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI); } - time[test] = vpTime::measureTimeMs() - t; + // test 5 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverse(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt); + } + + //------------------- + + // test 6 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + PI[i] = bench[i].pseudoInverse(static_cast(rank_bench)); + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI); + } + + // test 7 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverse(PI[i], static_cast(rank_bench)); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI); + } + + // test 8 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverse(PI[i], sv[i], static_cast(rank_bench)); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI); + } + + // test 9 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverse(PI[i], sv[i], static_cast(rank_bench), imA[i], imAt[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI); + } + + // test 10 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverse(PI[i], sv[i], static_cast(rank_bench), imA[i], imAt[i], kerAt[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt); } @@ -333,47 +463,129 @@ int test_pseudo_inverse_eigen3(bool verbose, const std::vector &bench, std::vector PI(size), imA(size), imAt(size), kerAt(size); std::vector sv(size); int ret = EXIT_SUCCESS; + time.clear(); - // test 0 - unsigned int test = 0; + // test 1 double t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { PI[i] = bench[i].pseudoInverseEigen3(); } - time[test] = vpTime::measureTimeMs() - t; + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI); } - // test 1 - test++; + // test 2 t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - bench[i].pseudoInverseEigen3(PI[i]); + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseEigen3(PI[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } } - time[test] = vpTime::measureTimeMs() - t; + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI); } - // test 2 - test++; + // test 3 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseEigen3(PI[i], sv[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI); + } + + // test 4 t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - bench[i].pseudoInverseEigen3(PI[i], sv[i]); + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseEigen3(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } } - time[test] = vpTime::measureTimeMs() - t; + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt); + } + + //------------------- + + // test 5 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + PI[i] = bench[i].pseudoInverseEigen3(static_cast(rank_bench)); + } + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI); } - // test 3 - test++; + // test 6 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseEigen3(PI[i], static_cast(rank_bench)); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI); + } + + // test 7 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseEigen3(PI[i], sv[i], static_cast(rank_bench)); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI); + } + + // test 8 t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - bench[i].pseudoInverseEigen3(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]); + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseEigen3(PI[i], sv[i], static_cast(rank_bench), imA[i], imAt[i], kerAt[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } } - time[test] = vpTime::measureTimeMs() - t; + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt); @@ -387,7 +599,7 @@ int test_pseudo_inverse_eigen3(bool verbose, const std::vector &bench, int test_pseudo_inverse_lapack(bool verbose, const std::vector &bench, std::vector &time) { if (verbose) - std::cout << "Test pseudo-inverse using Lapack 3rd party" << std::endl; + std::cout << "Test pseudo-inverse using Eigen3 3rd party" << std::endl; if (verbose) std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl; @@ -395,47 +607,129 @@ int test_pseudo_inverse_lapack(bool verbose, const std::vector &bench, std::vector PI(size), imA(size), imAt(size), kerAt(size); std::vector sv(size); int ret = EXIT_SUCCESS; + time.clear(); - // test 0 - unsigned int test = 0; + // test 1 double t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { PI[i] = bench[i].pseudoInverseLapack(); } - time[test] = vpTime::measureTimeMs() - t; + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI); } - // test 1 - test++; + // test 2 t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - bench[i].pseudoInverseLapack(PI[i]); + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseLapack(PI[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } } - time[test] = vpTime::measureTimeMs() - t; + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI); } - // test 2 - test++; + // test 3 t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - bench[i].pseudoInverseLapack(PI[i], sv[i]); + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseLapack(PI[i], sv[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } } - time[test] = vpTime::measureTimeMs() - t; + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI); } - // test 3 - test++; + // test 4 t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - bench[i].pseudoInverseLapack(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]); + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseLapack(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } } - time[test] = vpTime::measureTimeMs() - t; + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt); + } + + //------------------- + + // test 5 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + PI[i] = bench[i].pseudoInverseLapack(static_cast(rank_bench)); + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI); + } + + // test 6 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseLapack(PI[i], static_cast(rank_bench)); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI); + } + + // test 7 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseLapack(PI[i], sv[i], static_cast(rank_bench)); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI); + } + + // test 8 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseLapack(PI[i], sv[i], static_cast(rank_bench), imA[i], imAt[i], kerAt[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt); @@ -457,47 +751,128 @@ int test_pseudo_inverse_opencv(bool verbose, const std::vector &bench, std::vector PI(size), imA(size), imAt(size), kerAt(size); std::vector sv(size); int ret = EXIT_SUCCESS; + time.clear(); - // test 0 - unsigned int test = 0; + // test 1 double t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { PI[i] = bench[i].pseudoInverseOpenCV(); } - time[test] = vpTime::measureTimeMs() - t; + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI); } - // test 1 - test++; + // test 2 t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - bench[i].pseudoInverseOpenCV(PI[i]); + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseOpenCV(PI[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } } - time[test] = vpTime::measureTimeMs() - t; + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI); } - // test 2 - test++; + // test 3 t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - bench[i].pseudoInverseOpenCV(PI[i], sv[i]); + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseOpenCV(PI[i], sv[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } } - time[test] = vpTime::measureTimeMs() - t; + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI); } - // test 3 - test++; + // test 4 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseOpenCV(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt); + } + //------------------- + + // test 5 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + PI[i] = bench[i].pseudoInverseOpenCV(static_cast(rank_bench)); + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI); + } + + // test 6 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseOpenCV(PI[i], static_cast(rank_bench)); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI); + } + + // test 7 t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - bench[i].pseudoInverseOpenCV(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]); + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseOpenCV(PI[i], sv[i], static_cast(rank_bench)); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); + for (unsigned int i = 0; i < time.size(); i++) { + ret += test_pseudo_inverse(bench, PI); } - time[test] = vpTime::measureTimeMs() - t; + + // test 8 + t = vpTime::measureTimeMs(); + for (unsigned int i = 0; i < bench.size(); i++) { + unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols()); + unsigned int rank = bench[i].pseudoInverseOpenCV(PI[i], sv[i], static_cast(rank_bench), imA[i], imAt[i], kerAt[i]); + if (rank != rank_bench) { + if (verbose) { + std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl; + } + ret += EXIT_FAILURE; + } + } + time.push_back(vpTime::measureTimeMs() - t); for (unsigned int i = 0; i < time.size(); i++) { ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt); @@ -514,8 +889,8 @@ void save_time(const std::string &method, unsigned int nrows, unsigned int ncols if (use_plot_file) of << time[i] << "\t"; if (verbose) { - std::cout << " " << method << " svd(" << nrows << "x" << ncols << ")" - << " test " << i << ": " << time[i] << std::endl; + std::cout << " " << method << " pseudo inverse (" << nrows << "x" << ncols << ")" + << " time test " << i << ": " << time[i] << std::endl; } } } @@ -549,10 +924,12 @@ int main(int argc, const char *argv[]) if (s == 0) { nrows[s] = nb_rows; ncols[s] = nb_cols; - } else if (s == 1) { + } + else if (s == 1) { nrows[s] = nb_cols; ncols[s] = nb_cols; - } else { + } + else { nrows[s] = nb_cols; ncols[s] = nb_rows; } @@ -561,33 +938,37 @@ int main(int argc, const char *argv[]) if (use_plot_file) { of.open(plotfile.c_str()); of << "iter" - << "\t"; + << "\t"; for (unsigned int s = 0; s < nb_test_matrix_size; s++) { for (unsigned int i = 0; i < nb_svd_functions; i++) of << "\"default " << nrows[s] << "x" << ncols[s] << " test " << i << "\"" - << "\t"; + << "\t"; #if defined(VISP_HAVE_LAPACK) for (unsigned int i = 0; i < nb_svd_functions; i++) of << "\"Lapack " << nrows[s] << "x" << ncols[s] << " test " << i << "\"" - << "\t"; + << "\t"; #endif #if defined(VISP_HAVE_EIGEN3) for (unsigned int i = 0; i < nb_svd_functions; i++) of << "\"Eigen3 " << nrows[s] << "x" << ncols[s] << " test " << i << "\"" - << "\t"; + << "\t"; #endif #if defined(VISP_HAVE_OPENCV) for (unsigned int i = 0; i < nb_svd_functions; i++) of << "\"OpenCV " << nrows[s] << "x" << ncols[s] << " test " << i << "\"" - << "\t"; + << "\t"; #endif } of << std::endl; } - int ret = EXIT_SUCCESS; + int ret_default = EXIT_SUCCESS; + int ret_lapack = EXIT_SUCCESS; + int ret_eigen3 = EXIT_SUCCESS; + int ret_opencv = EXIT_SUCCESS; + for (unsigned int iter = 0; iter < nb_iterations; iter++) { if (use_plot_file) @@ -597,21 +978,21 @@ int main(int argc, const char *argv[]) std::vector bench_random_matrices; create_bench_random_matrix(nb_matrices, nrows[s], ncols[s], verbose, bench_random_matrices); - ret += test_pseudo_inverse_default(verbose, bench_random_matrices, time); + ret_default += test_pseudo_inverse_default(verbose, bench_random_matrices, time); save_time("default -", nrows[s], ncols[s], verbose, use_plot_file, of, time); #if defined(VISP_HAVE_LAPACK) - ret += test_pseudo_inverse_lapack(verbose, bench_random_matrices, time); + ret_lapack += test_pseudo_inverse_lapack(verbose, bench_random_matrices, time); save_time("Lapack -", nrows[s], ncols[s], verbose, use_plot_file, of, time); #endif #if defined(VISP_HAVE_EIGEN3) - ret += test_pseudo_inverse_eigen3(verbose, bench_random_matrices, time); + ret_eigen3 += test_pseudo_inverse_eigen3(verbose, bench_random_matrices, time); save_time("Eigen3 -", nrows[s], ncols[s], verbose, use_plot_file, of, time); #endif #if defined(VISP_HAVE_OPENCV) - ret += test_pseudo_inverse_opencv(verbose, bench_random_matrices, time); + ret_opencv += test_pseudo_inverse_opencv(verbose, bench_random_matrices, time); save_time("OpenCV -", nrows[s], ncols[s], verbose, use_plot_file, of, time); #endif } @@ -623,11 +1004,21 @@ int main(int argc, const char *argv[]) std::cout << "Result saved in " << plotfile << std::endl; } - if (ret == EXIT_SUCCESS) { - std::cout << "Test succeed" << std::endl; - } else { - std::cout << "Test failed" << std::endl; - } + std::cout << "Resume testing:" << std::endl; + std::cout << " Pseudo-inverse (default): " << (ret_default ? "failed" : "success") << std::endl; +#if defined(VISP_HAVE_LAPACK) + std::cout << " Pseudo-inverse (lapack) : " << (ret_lapack ? "failed" : "success") << std::endl; +#endif +#if defined(VISP_HAVE_EIGEN3) + std::cout << " Pseudo-inverse (eigen3) : " << (ret_eigen3 ? "failed" : "success") << std::endl; +#endif +#if defined(VISP_HAVE_OPENCV) + std::cout << " Pseudo-inverse (opencv) : " << (ret_opencv ? "failed" : "success") << std::endl; +#endif + + int ret = ret_default + ret_lapack + ret_eigen3 + ret_opencv; + + std::cout << " Global test : " << (ret ? "failed" : "success") << std::endl; return ret; #else @@ -636,7 +1027,8 @@ int main(int argc, const char *argv[]) std::cout << "Test does nothing since you dont't have Lapack, Eigen3 or OpenCV 3rd party" << std::endl; return EXIT_SUCCESS; #endif - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e.getStringMessage() << std::endl; return EXIT_FAILURE; } diff --git a/modules/core/test/math/testSvd.cpp b/modules/core/test/math/testSvd.cpp index 3c7f9420da..69dc066c56 100644 --- a/modules/core/test/math/testSvd.cpp +++ b/modules/core/test/math/testSvd.cpp @@ -30,8 +30,7 @@ * * Description: * Test various svd decompositions. - * -*****************************************************************************/ + */ /*! \example testSvd.cpp @@ -235,8 +234,7 @@ void create_bench_random_symmetric_matrix(unsigned int nb_matrices, unsigned int std::vector &bench) { if (verbose) - std::cout << "Create a bench of " << nb_matrices << " " << nb_rows << " by " << nb_rows << " symmetric matrices" - << std::endl; + std::cout << "Create a bench of " << nb_matrices << " " << nb_rows << " by " << nb_rows << " symmetric matrices" << std::endl; bench.clear(); for (unsigned int i = 0; i < nb_matrices; i++) { vpMatrix M; @@ -260,15 +258,16 @@ void create_bench_random_symmetric_matrix(unsigned int nb_matrices, unsigned int } } -int test_svd(std::vector M, std::vector U, std::vector s, std::vector V) +int test_svd(std::vector M, std::vector U, std::vector s, std::vector V, double &error) { for (unsigned int i = 0; i < M.size(); i++) { vpMatrix S; S.diag(s[i]); - vpMatrix U_S_V = U[i] * S * V[i].t(); - vpMatrix D = M[i] - U_S_V; - if (D.frobeniusNorm() > 1e-6) { - std::cout << "SVD decomposition failed" << std::endl; + vpMatrix U_S_Vt = U[i] * S * V[i].t(); + vpMatrix D = M[i] - U_S_Vt; + error = D.frobeniusNorm(); + if (error > 1e-6) { + std::cout << "SVD decomposition failed. Error: " << error << std::endl; return EXIT_FAILURE; } } @@ -296,7 +295,7 @@ int test_eigen_values(std::vector M, std::vector e, std:: } #if defined(VISP_HAVE_EIGEN3) -int test_svd_eigen3(bool verbose, const std::vector &bench, double &time) +int test_svd_eigen3(bool verbose, const std::vector &bench, double &time, double &error) { if (verbose) std::cout << "Test SVD using Eigen3 3rd party" << std::endl; @@ -315,12 +314,12 @@ int test_svd_eigen3(bool verbose, const std::vector &bench, double &ti time = vpTime::measureTimeMs() - t; - return test_svd(bench, U, s, V); + return test_svd(bench, U, s, V, error); } #endif #if defined(VISP_HAVE_LAPACK) -int test_svd_lapack(bool verbose, const std::vector &bench, double &time) +int test_svd_lapack(bool verbose, const std::vector &bench, double &time, double &error) { if (verbose) std::cout << "Test SVD using Lapack 3rd party" << std::endl; @@ -338,7 +337,7 @@ int test_svd_lapack(bool verbose, const std::vector &bench, double &ti } time = vpTime::measureTimeMs() - t; - return test_svd(bench, U, s, V); + return test_svd(bench, U, s, V, error); } int test_eigen_values_lapack(bool verbose, const std::vector &bench, double &time) @@ -366,7 +365,7 @@ int test_eigen_values_lapack(bool verbose, const std::vector &bench, d #endif #if defined(VISP_HAVE_OPENCV) -int test_svd_opencv(bool verbose, const std::vector &bench, double &time) +int test_svd_opencv(bool verbose, const std::vector &bench, double &time, double &error) { if (verbose) std::cout << "Test SVD using OpenCV 3rd party" << std::endl; @@ -384,19 +383,78 @@ int test_svd_opencv(bool verbose, const std::vector &bench, double &ti } time = vpTime::measureTimeMs() - t; - return test_svd(bench, U, s, V); + return test_svd(bench, U, s, V, error); } #endif -void save_time(const std::string &method, bool verbose, bool use_plot_file, std::ofstream &of, double time) +void save_time(const std::string &method, bool verbose, bool use_plot_file, std::ofstream &of, double time, double error) { if (use_plot_file) of << time << "\t"; if (verbose || !use_plot_file) { - std::cout << method << time << std::endl; + std::cout << method << "took " << time << "s, error = " << error << std::endl; } } +bool testAllSvds(const std::string &test_name, unsigned nb_matrices, unsigned nb_iterations, + unsigned nb_rows, unsigned nb_cols, + bool doEigenValues, bool verbose, bool use_plot_file, std::ofstream &of) +{ + int ret = EXIT_SUCCESS; + int ret_test = 0; + for (unsigned int iter = 0; iter < nb_iterations; iter++) { + std::cout << "\n-> Iteration: " << iter << std::endl; + std::vector bench_random_matrices; + create_bench_random_matrix(nb_matrices, nb_rows, nb_cols, verbose, bench_random_matrices); + std::vector bench_random_symmetric_matrices; + create_bench_random_symmetric_matrix(nb_matrices, nb_rows, verbose, bench_random_symmetric_matrices); + + if (use_plot_file) + of << test_name << iter << "\t"; + double time; + double error; + +#if defined(VISP_HAVE_LAPACK) + std::cout << "\n-- Test SVD using lapack" << std::endl; + ret_test = test_svd_lapack(verbose, bench_random_matrices, time, error); + ret += ret_test; + std::cout << test_name << ": SVD (Lapack) " << (ret_test ? "failed" : "succeed") << std::endl; + save_time("SVD (Lapack): ", verbose, use_plot_file, of, time, error); +#endif + +#if defined(VISP_HAVE_EIGEN3) + std::cout << "\n-- Test SVD using eigen" << std::endl; + ret_test = test_svd_eigen3(verbose, bench_random_matrices, time, error); + ret += ret_test; + std::cout << test_name << ": SVD (Eigen) " << (ret_test ? "failed" : "succeed") << std::endl; + save_time("SVD (Eigen3): ", verbose, use_plot_file, of, time, error); +#endif + +#if defined(VISP_HAVE_OPENCV) + std::cout << "\n-- Test SVD using OpenCV" << std::endl; + ret_test = test_svd_opencv(verbose, bench_random_matrices, time, error); + ret += ret_test; + std::cout << test_name << ": SVD (OpenCV) " << (ret_test ? "failed" : "succeed") << std::endl; + save_time("SVD (OpenCV): ", verbose, use_plot_file, of, time, error); +#endif + +#if defined(VISP_HAVE_LAPACK) + if (doEigenValues) { + std::cout << "\n-- Test Eigen Values using lapack" << std::endl; + ret_test = test_eigen_values_lapack(verbose, bench_random_symmetric_matrices, time); + ret += ret_test; + std::cout << "Eigen values (Lapack) " << (ret_test ? "failed" : "succeed") << std::endl; + error = 0.0; + save_time("Eigen values (Lapack): ", verbose, use_plot_file, of, time, error); + } +#endif + std::cout << "Result after iteration " << iter << ": " << (ret ? "failed" : "succeed") << std::endl; + if (use_plot_file) + of << std::endl; + } + return (ret == EXIT_SUCCESS); +} + int main(int argc, const char *argv[]) { try { @@ -405,7 +463,6 @@ int main(int argc, const char *argv[]) unsigned int nb_iterations = 10; unsigned int nb_rows = 6; unsigned int nb_cols = 6; - unsigned int nb_rows_sym = 5; bool verbose = false; std::string plotfile("plot-svd.csv"); bool use_plot_file = false; @@ -420,76 +477,72 @@ int main(int argc, const char *argv[]) if (use_plot_file) { of.open(plotfile.c_str()); of << "iter" - << "\t"; + << "\t"; #if defined(VISP_HAVE_LAPACK) of << "\"SVD Lapack\"" - << "\t"; + << "\t"; #endif #if defined(VISP_HAVE_EIGEN3) of << "\"SVD Eigen3\"" - << "\t"; + << "\t"; #endif #if defined(VISP_HAVE_OPENCV) of << "\"SVD OpenCV\"" - << "\t"; + << "\t"; #endif of << std::endl; } + bool success = true; + std::string test_case; + test_case = "Test case: Square matrices"; + std::cout << "\n== " << test_case << ": " << nb_rows << " x " << nb_cols << " ==" << std::endl; + bool defaultSuccess = testAllSvds(test_case, nb_matrices, nb_iterations, nb_rows, nb_cols, + true, verbose, use_plot_file, of); + std::cout << "=> " << test_case << ": " << (defaultSuccess ? "succeed" : "failed") << std::endl; - int ret = EXIT_SUCCESS; - for (unsigned int iter = 0; iter < nb_iterations; iter++) { - std::vector bench_random_matrices; - create_bench_random_matrix(nb_matrices, nb_rows, nb_cols, verbose, bench_random_matrices); - std::vector bench_random_symmetric_matrices; - create_bench_random_symmetric_matrix(nb_matrices, nb_rows_sym, verbose, bench_random_symmetric_matrices); + test_case = "Test case: More rows than columns"; + std::cout << "\n== " << test_case << ": " << nb_cols * 2 << " x " << nb_cols << " ==" << std::endl; + bool rowsSuccess = testAllSvds(test_case, nb_matrices, nb_iterations, nb_cols * 2, nb_cols, + false, verbose, use_plot_file, of); + std::cout << "=> " << test_case << ": " << (rowsSuccess ? "succeed" : "failed") << std::endl; - if (use_plot_file) - of << iter << "\t"; - double time; + test_case = "Test case: More columns than rows"; + std::cout << "\n== " << test_case << ": " << nb_rows << " x " << nb_rows * 2 << " ==" << std::endl; + bool colsSuccess = testAllSvds(test_case, nb_matrices, nb_iterations, nb_rows, nb_rows * 2, + false, verbose, use_plot_file, of); + std::cout << "=> " << test_case << ": " << (colsSuccess ? "succeed" : "failed") << std::endl; -#if defined(VISP_HAVE_LAPACK) - ret += test_svd_lapack(verbose, bench_random_matrices, time); - save_time("SVD (Lapack): ", verbose, use_plot_file, of, time); -#endif + std::cout << "\nResume:" << std::endl; + std::cout << "- Square matrices (" << nb_rows << "x" << nb_cols << "): " << (defaultSuccess ? "succeed" : "failed") << std::endl; -#if defined(VISP_HAVE_EIGEN3) - ret += test_svd_eigen3(verbose, bench_random_matrices, time); - save_time("SVD (Eigen3): ", verbose, use_plot_file, of, time); -#endif + std::cout << "- More rows case (" << nb_cols * 2 << "x" << nb_cols << "): " << (rowsSuccess ? "succeed" : "failed") << std::endl; -#if defined(VISP_HAVE_OPENCV) - ret += test_svd_opencv(verbose, bench_random_matrices, time); - save_time("SVD (OpenCV): ", verbose, use_plot_file, of, time); -#endif + std::cout << "- More columns case (" << nb_rows << "x" << nb_rows * 2 << "): " << (colsSuccess ? "succeed" : "failed") << std::endl; -#if defined(VISP_HAVE_LAPACK) - ret += test_eigen_values_lapack(verbose, bench_random_symmetric_matrices, time); - save_time("Eigen values (Lapack): ", verbose, use_plot_file, of, time); -#endif + success = defaultSuccess && rowsSuccess && colsSuccess; - if (use_plot_file) - of << std::endl; - } if (use_plot_file) { of.close(); std::cout << "Result saved in " << plotfile << std::endl; } - if (ret == EXIT_SUCCESS) { + if (success) { std::cout << "Test succeed" << std::endl; - } else { + } + else { std::cout << "Test failed" << std::endl; } - return ret; + return success ? EXIT_SUCCESS : EXIT_FAILURE; #else (void)argc; (void)argv; std::cout << "Test does nothing since you dont't have Lapack, Eigen3 or OpenCV 3rd party" << std::endl; return EXIT_SUCCESS; #endif - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e.getStringMessage() << std::endl; return EXIT_FAILURE; }