57 #include <visp3/core/vpConfig.h>
60 #include <gsl/gsl_linalg.h>
63 #include <visp3/core/vpMatrix.h>
64 #include <visp3/core/vpMath.h>
65 #include <visp3/core/vpTranslationVector.h>
66 #include <visp3/core/vpColVector.h>
67 #include <visp3/core/vpException.h>
68 #include <visp3/core/vpDebug.h>
79 unsigned int r,
unsigned int c,
80 unsigned int nrows,
unsigned int ncols)
83 if (((r + nrows) > M.
rowNum) || ((c + ncols) > M.
colNum)) {
85 "Cannot construct a sub matrix (%dx%d) starting at position (%d,%d) that is not contained in the original matrix (%dx%d)",
89 init(M, r, c, nrows, ncols);
139 unsigned int rnrows = r+nrows ;
140 unsigned int cncols = c+ncols ;
144 "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows, M.
getRows()));
147 "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols, M.
getCols()));
152 for (
unsigned int i=r ; i < rnrows; i++)
153 for (
unsigned int j=c ; j < cncols; j++)
154 (*
this)[i-r][j-c] = M[i][j] ;
196 for (
unsigned int i=0; i<
rowNum; i++) {
197 for (
unsigned int j=0; j<
colNum; j++) {
198 if (i == j) (*this)[i][j] = 1.0;
199 else (*
this)[i][j] = 0;
219 for (
unsigned int i=0;i<
rowNum;i++) {
220 double *coli = (*this)[i] ;
221 for (
unsigned int j=0;j<
colNum;j++)
256 double ** AtRowPtrs = At.
rowPtrs;
258 for(
unsigned int i = 0; i <
colNum; i++ ) {
259 double * row_ = AtRowPtrs[i];
261 for(
unsigned int j = 0; j <
rowNum; j++, col+=A_step )
303 for(
unsigned int i=0;i<
rowNum;i++){
304 for(
unsigned int j=i;j<
rowNum;j++){
310 for(
unsigned int k=0; k <
colNum ;k++)
311 ssum += *(pi++)* *(pj++);
353 s +=(*(ptr+i)) * (*(ptr+j));
363 s +=(*(ptr+i)) * (*(ptr+i));
418 for (
unsigned int i=0;i<
rowNum;i++)
419 for(
unsigned int j=0;j<
colNum;j++)
433 for (
unsigned int i=0; i<
rowNum; i++) {
434 for (
unsigned int j=0; j<
colNum; j++) {
480 unsigned int rows = A.
getRows() ;
488 for (
unsigned int i=0 ; i< rows ; i++ )
489 (*
this)[i][i] = A[i] ;
528 for (
unsigned int i=0 ; i< min_ ; i++ )
529 (*
this)[i][i] = val;
547 unsigned int rows = A.
getRows() ;
556 for (
unsigned int i=0 ; i< rows ; i++ )
571 "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
575 for (
unsigned int j=0;j<3;j++) t_out[j]=0 ;
577 for (
unsigned int j=0;j<3;j++) {
579 for (
unsigned int i=0;i<3;i++) {
610 "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
622 for (
unsigned int j=0;j<A.
colNum;j++) {
624 for (
unsigned int i=0;i<A.
rowNum;i++) {
654 "Cannot multiply (%dx%d) matrix by (%dx%d) matrix",
659 unsigned int BcolNum = B.
colNum;
660 unsigned int BrowNum = B.
rowNum;
665 double *rowptri = A.
rowPtrs[i];
667 for (j=0;j<BcolNum;j++)
670 for (k=0;k<BrowNum;k++) s += rowptri[k] * BrowPtrs[k][j];
693 "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a rotation matrix",
698 unsigned int BcolNum = B.
colNum;
699 unsigned int BrowNum = B.
rowNum;
704 double *rowptri = A.
rowPtrs[i];
706 for (j=0;j<BcolNum;j++)
709 for (k=0;k<BrowNum;k++) s += rowptri[k] * BrowPtrs[k][j];
732 "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a rotation matrix",
737 unsigned int BcolNum = B.
colNum;
738 unsigned int BrowNum = B.
rowNum;
743 double *rowptri = A.
rowPtrs[i];
745 for (j=0;j<BcolNum;j++)
748 for (k=0;k<BrowNum;k++) s += rowptri[k] * BrowPtrs[k][j];
792 "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix",
797 unsigned int RcolNum = R.
getCols();
798 unsigned int RrowNum = R.
getRows();
799 for (
unsigned int i=0;i<
rowNum;i++)
803 for (
unsigned int j=0;j<RcolNum;j++)
806 for (
unsigned int k=0;k<RrowNum;k++) s += rowptri[k] * R[k][j];
821 "Cannot multiply (%dx%d) matrix by (3x3) velocity twist matrix",
826 unsigned int VcolNum = V.
getCols();
827 unsigned int VrowNum = V.
getRows();
828 for (
unsigned int i=0;i<
rowNum;i++)
832 for (
unsigned int j=0;j<VcolNum;j++)
835 for (
unsigned int k=0;k<VrowNum;k++) s += rowptri[k] * V[k][j];
850 "Cannot multiply (%dx%d) matrix by (3x3) force/torque twist matrix",
855 unsigned int VcolNum = V.
getCols();
856 unsigned int VrowNum = V.
getRows();
857 for (
unsigned int i=0;i<
rowNum;i++)
861 for (
unsigned int j=0;j<VcolNum;j++)
864 for (
unsigned int k=0;k<VrowNum;k++) s += rowptri[k] * V[k][j];
893 "Cannot add (%dx%d) matrix with (%dx%d) matrix",
901 for (
unsigned int i=0;i<A.
rowNum;i++)
902 for(
unsigned int j=0;j<A.
colNum;j++)
903 CrowPtrs[i][j] = wB*BrowPtrs[i][j]+wA*ArowPtrs[i][j];
926 "Cannot add (%dx%d) matrix with (%dx%d) matrix",
934 for (
unsigned int i=0;i<A.
rowNum;i++) {
935 for(
unsigned int j=0;j<A.
colNum;j++) {
936 CrowPtrs[i][j] = BrowPtrs[i][j]+ArowPtrs[i][j];
964 "Cannot add (%dx%d) matrix with (%dx%d) matrix",
972 for (
unsigned int i=0;i<A.
rowNum;i++) {
973 for(
unsigned int j=0;j<A.
colNum;j++) {
974 CrowPtrs[i][j] = BrowPtrs[i][j]+ArowPtrs[i][j];
1016 "Cannot substract (%dx%d) matrix to (%dx%d) matrix",
1024 for (
unsigned int i=0;i<A.
rowNum;i++) {
1025 for(
unsigned int j=0;j<A.
colNum;j++) {
1026 CrowPtrs[i][j] = ArowPtrs[i][j]-BrowPtrs[i][j];
1053 "Cannot substract (%dx%d) matrix to (%dx%d) matrix",
1061 for (
unsigned int i=0;i<A.
rowNum;i++) {
1062 for(
unsigned int j=0;j<A.
colNum;j++) {
1063 CrowPtrs[i][j] = ArowPtrs[i][j]-BrowPtrs[i][j];
1085 "Cannot add (%dx%d) matrix to (%dx%d) matrix",
1091 for (
unsigned int i=0;i<
rowNum;i++)
1092 for(
unsigned int j=0;j<
colNum;j++)
1093 rowPtrs[i][j] += BrowPtrs[i][j];
1103 "Cannot substract (%dx%d) matrix to (%dx%d) matrix",
1108 for (
unsigned int i=0;i<
rowNum;i++)
1109 for(
unsigned int j=0;j<
colNum;j++)
1110 rowPtrs[i][j] -= BrowPtrs[i][j];
1137 for (
unsigned int i=0;i<A.
rowNum;i++)
1138 for(
unsigned int j=0;j<A.
colNum;j++)
1139 CrowPtrs[i][j]= -ArowPtrs[i][j];
1158 for (
unsigned int i=0;i<
rowNum;i++)
1160 for(
unsigned int j=0;j<
colNum;j++)
1189 unsigned int Brow = B.
getRows() ;
1190 unsigned int Bcol = B.
getCols() ;
1192 for (
unsigned int i=0;i<Brow;i++)
1193 for(
unsigned int j=0;j<Bcol;j++)
1207 for (
unsigned int i=0;i<
rowNum;i++)
1208 for(
unsigned int j=0;j<
colNum;j++)
1227 if (std::fabs(x) <= std::numeric_limits<double>::epsilon()) {
1233 for (
unsigned int i=0;i<
rowNum;i++)
1234 for(
unsigned int j=0;j<
colNum;j++)
1244 for (
unsigned int i=0;i<
rowNum;i++)
1245 for(
unsigned int j=0;j<
colNum;j++)
1255 for (
unsigned int i=0;i<
rowNum;i++)
1256 for(
unsigned int j=0;j<
colNum;j++)
1268 for (
unsigned int i=0;i<
rowNum;i++)
1269 for(
unsigned int j=0;j<
colNum;j++)
1279 if (std::fabs(x) <= std::numeric_limits<double>::epsilon())
1284 for (
unsigned int i=0;i<
rowNum;i++)
1285 for(
unsigned int j=0;j<
colNum;j++)
1314 double *optr=out.
data;
1315 for(
unsigned int j =0;j<
colNum ; j++){
1316 for(
unsigned int i =0;i<
rowNum ; i++){
1347 double *optr=out.
data;
1348 for(
unsigned int i =0;i<
dsize ; i++){
1349 *(optr++)=*(mdata++);
1371 unsigned int r1= m1.
getRows();
1372 unsigned int c1= m1.
getCols();
1373 unsigned int r2= m2.
getRows();
1374 unsigned int c2= m2.
getCols();
1378 vpERROR_TRACE(
"Kronecker prodect bad dimension of output vpMatrix") ;
1380 "In Kronecker product bad dimension of output matrix"));
1383 for(
unsigned int r =0;r<r1 ; r++){
1384 for(
unsigned int c =0;c<c1 ; c++){
1385 double alpha = m1[r][c];
1386 double *m2ptr = m2[0];
1387 unsigned int roffset= r*r2;
1388 unsigned int coffset= c*c2;
1389 for(
unsigned int rr =0;rr<r2 ; rr++){
1390 for(
unsigned int cc =0;cc<c2 ;cc++){
1391 out[roffset+rr][coffset+cc]= alpha* *(m2ptr++);
1417 unsigned int r1= m1.
getRows();
1418 unsigned int c1= m1.
getCols();
1419 unsigned int r2= m2.
getRows();
1420 unsigned int c2= m2.
getCols();
1424 for(
unsigned int r =0;r<r1 ; r++){
1425 for(
unsigned int c =0;c<c1 ; c++){
1426 double alpha = m1[r][c];
1427 double *m2ptr = m2[0];
1428 unsigned int roffset= r*r2;
1429 unsigned int coffset= c*c2;
1430 for(
unsigned int rr =0;rr<r2 ; rr++){
1431 for(
unsigned int cc =0;cc<c2 ;cc++){
1432 out[roffset+rr ][coffset+cc]= alpha* *(m2ptr++);
1448 return kron(*
this,m);
1638 #if defined (VISP_HAVE_LAPACK_C)
1640 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
1642 #elif defined (VISP_HAVE_GSL)
1653 unsigned int i,j,k,nrows,ncols;
1660 #ifdef VISP_HAVE_GSL
1669 Asvd.
resize(nrows,ncols);
1671 for (i = 0 ; i < nrows ; i++)
1673 for (j = 0 ; j < ncols ; j++)
1676 for (k=0 ; k < ncols ; k++) Asvd[i][j] += (*
this)[i][k]*w[k]*v[j][k];
1679 for (i=0;i<nrows;i++)
1681 for (j=0;j<ncols;j++)
if (fabs(A[i][j]-Asvd[i][j]) > 1e-6) pb = 1;
1685 printf(
"pb in SVD\n");
1686 std::cout <<
" A : " << std::endl << A << std::endl;
1687 std::cout <<
" Asvd : " << std::endl << Asvd << std::endl;
1801 unsigned int i, j, k ;
1803 unsigned int nrows, ncols;
1804 unsigned int nrows_orig =
getRows() ;
1805 unsigned int ncols_orig =
getCols() ;
1806 Ap.
resize(ncols_orig,nrows_orig) ;
1808 if (nrows_orig >= ncols_orig)
1824 if (nrows_orig >= ncols_orig) a = *
this;
1825 else a = (*this).
t();
1831 for (i=0 ; i < ncols ; i++)
1832 if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
1834 unsigned int rank = 0 ;
1835 for (i=0 ; i < ncols ; i++)
1836 if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
1839 for (i = 0 ; i < ncols ; i++)
1841 for (j = 0 ; j < nrows ; j++)
1845 for (k=0 ; k < ncols ; k++)
1846 if (fabs(sv[k]) > maxsv*svThreshold)
1848 a1[i][j] += v[i][k]*a[j][k]/sv[k];
1852 if (nrows_orig >= ncols_orig) Ap = a1;
1855 if (nrows_orig >= ncols_orig)
1858 imAt.
resize(ncols_orig,rank) ;
1859 for (i=0 ; i < ncols_orig ; i++)
1860 for (j=0 ; j < rank ; j++)
1861 imAt[i][j] = v[i][j] ;
1864 imA.
resize(nrows_orig,rank) ;
1865 for (i=0 ; i < nrows_orig ; i++)
1866 for (j=0 ; j < rank ; j++)
1867 imA[i][j] = a[i][j] ;
1872 imAt.
resize(ncols_orig,rank) ;
1873 for (i=0 ; i < ncols_orig ; i++)
1874 for (j=0 ; j < rank ; j++)
1875 imAt[i][j] = a[i][j] ;
1877 imA.
resize(nrows_orig,rank) ;
1878 for (i=0 ; i < nrows_orig ; i++)
1879 for (j=0 ; j < rank ; j++)
1880 imA[i][j] = v[i][j] ;
1887 vpMatrix A, ApA, AAp, AApA, ApAAp ;
1900 for (i=0;i<nrows;i++)
1902 for (j=0;j<ncols;j++)
if (fabs(AApA[i][j]-A[i][j]) > 1e-6) pb = 1;
1904 for (i=0;i<ncols;i++)
1906 for (j=0;j<nrows;j++)
if (fabs(ApAAp[i][j]-Ap[i][j]) > 1e-6) pb = 1;
1908 for (i=0;i<nrows;i++)
1910 for (j=0;j<nrows;j++)
if (fabs(AAp[i][j]-AAp[j][i]) > 1e-6) pb = 1;
1912 for (i=0;i<ncols;i++)
1914 for (j=0;j<ncols;j++)
if (fabs(ApA[i][j]-ApA[j][i]) > 1e-6) pb = 1;
1918 printf(
"pb in pseudo inverse\n");
1919 std::cout <<
" A : " << std::endl << A << std::endl;
1920 std::cout <<
" Ap : " << std::endl << Ap << std::endl;
1921 std::cout <<
" A - AApA : " << std::endl << A - AApA << std::endl;
1922 std::cout <<
" Ap - ApAAp : " << std::endl << Ap - ApAAp << std::endl;
1923 std::cout <<
" AAp - (AAp)^T : " << std::endl << AAp - AAp.
t() << std::endl;
1924 std::cout <<
" ApA - (ApA)^T : " << std::endl << ApA - ApA.
t() << std::endl;
1978 unsigned int i, j, k ;
1980 unsigned int nrows, ncols;
1981 unsigned int nrows_orig =
getRows() ;
1982 unsigned int ncols_orig =
getCols() ;
1983 Ap.
resize(ncols_orig,nrows_orig) ;
1985 if (nrows_orig >= ncols_orig)
2001 if (nrows_orig >= ncols_orig) a = *
this;
2002 else a = (*this).
t();
2008 for (i=0 ; i < ncols ; i++)
2009 if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
2011 unsigned int rank = 0 ;
2012 for (i=0 ; i < ncols ; i++)
2013 if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
2018 for (i = 0 ; i < ncols ; i++)
2020 for (j = 0 ; j < nrows ; j++)
2024 for (k=0 ; k < ncols ; k++)
2025 if (fabs(sv[k]) > maxsv*svThreshold)
2027 a1[i][j] += v[i][k]*a[j][k]/sv[k];
2031 if (nrows_orig >= ncols_orig) Ap = a1;
2034 if (nrows_orig >= ncols_orig)
2037 imAt.
resize(ncols_orig,rank) ;
2038 for (i=0 ; i < ncols_orig ; i++)
2039 for (j=0 ; j < rank ; j++)
2040 imAt[i][j] = v[i][j] ;
2043 imA.
resize(nrows_orig,rank) ;
2044 for (i=0 ; i < nrows_orig ; i++)
2045 for (j=0 ; j < rank ; j++)
2046 imA[i][j] = a[i][j] ;
2051 imAt.
resize(ncols_orig,rank) ;
2052 for (i=0 ; i < ncols_orig ; i++)
2053 for (j=0 ; j < rank ; j++)
2054 imAt[i][j] = a[i][j] ;
2056 imA.
resize(nrows_orig,rank) ;
2057 for (i=0 ; i < nrows_orig ; i++)
2058 for (j=0 ; j < rank ; j++)
2059 imA[i][j] = v[i][j] ;
2063 vpMatrix cons(ncols_orig, ncols_orig);
2066 for (j = 0; j < ncols_orig; j++)
2068 for (i = 0; i < ncols_orig; i++)
2070 if (fabs(sv[i]) <= maxsv*svThreshold)
2072 cons[i][j] = v[j][i];
2077 vpMatrix Ker (ncols_orig-rank, ncols_orig);
2079 for (j = 0; j < ncols_orig ; j++)
2082 if ( std::fabs(cons.
getRow(j).
sumSquare()) > std::numeric_limits<double>::epsilon())
2084 for (i = 0; i < cons.
getCols(); i++)
2085 Ker[k][i] = cons[j][i];
2095 vpMatrix A, ApA, AAp, AApA, ApAAp ;
2108 for (i=0;i<nrows;i++)
2110 for (j=0;j<ncols;j++)
if (fabs(AApA[i][j]-A[i][j]) > 1e-6) pb = 1;
2112 for (i=0;i<ncols;i++)
2114 for (j=0;j<nrows;j++)
if (fabs(ApAAp[i][j]-Ap[i][j]) > 1e-6) pb = 1;
2116 for (i=0;i<nrows;i++)
2118 for (j=0;j<nrows;j++)
if (fabs(AAp[i][j]-AAp[j][i]) > 1e-6) pb = 1;
2120 for (i=0;i<ncols;i++)
2122 for (j=0;j<ncols;j++)
if (fabs(ApA[i][j]-ApA[j][i]) > 1e-6) pb = 1;
2126 printf(
"pb in pseudo inverse\n");
2127 std::cout <<
" A : " << std::endl << A << std::endl;
2128 std::cout <<
" Ap : " << std::endl << Ap << std::endl;
2129 std::cout <<
" A - AApA : " << std::endl << A - AApA << std::endl;
2130 std::cout <<
" Ap - ApAAp : " << std::endl << Ap - ApAAp << std::endl;
2131 std::cout <<
" AAp - (AAp)^T : " << std::endl << AAp - AAp.
t() << std::endl;
2132 std::cout <<
" ApA - (ApA)^T : " << std::endl << ApA - ApA.
t() << std::endl;
2133 std::cout <<
" KerA : " << std::endl << kerA << std::endl;
2185 vpMatrix::getCol(
const unsigned int j,
const unsigned int i_begin,
const unsigned int column_size)
const
2190 for (
unsigned int i=0 ; i < column_size ; i++)
2191 c[i] = (*
this)[i_begin+i][j];
2239 unsigned int nb_rows =
getRows();
2241 for (
unsigned int i=0 ; i < nb_rows ; i++)
2242 c[i] = (*
this)[i][j];
2286 unsigned int nb_cols =
getCols();
2288 for (
unsigned int j=0 ; j < nb_cols ; j++)
2289 r[j] = (*
this)[i][j];
2331 vpMatrix::getRow(
const unsigned int i,
const unsigned int j_begin,
const unsigned int row_size)
const
2336 for (
unsigned int j=0 ; j < row_size ; j++)
2337 r[j] = (*
this)[i][j_begin+i];
2401 unsigned int nra = A.
getRows() ;
2402 unsigned int nrb = B.
getRows() ;
2407 "Cannot stack (%dx%d) matrix with (%dx%d) matrix",
2419 for (i=0 ; i < nra ; i++) {
2420 for (j=0 ; j < A.
getCols() ; j++)
2424 for (i=0 ; i < nrb ; i++) {
2425 for (j=0 ; j < B.
getCols() ; j++) {
2426 C[i+nra][j] = B[i][j] ;
2443 unsigned int nra = A.
getRows() ;
2448 "Cannot stack (%dx%d) matrix with (1x%d) row vector",
2460 for (i=0 ; i < nra ; i++) {
2461 for (j=0 ; j < A.
getCols() ; j++)
2465 for (j=0 ; j < r.
getCols() ; j++) {
2483 const unsigned int r,
const unsigned int c)
2512 const unsigned int r,
const unsigned int c)
2522 for(
unsigned int i=0; i<A.
getRows(); i++){
2523 for(
unsigned int j=0; j<A.
getCols(); j++){
2524 if(i >= r && i < (r + B.
getRows()) && j >= c && j < (c+B.
getCols())){
2525 C[i][j] = B[i-r][j-c];
2535 "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
2581 unsigned int nca = A.
getCols() ;
2582 unsigned int ncb = B.
getCols() ;
2587 "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix",
2599 for (i=0 ; i < C.
getRows(); i++)
2600 for (j=0 ; j < nca ; j++)
2603 for (i=0 ; i < C.
getRows() ; i++)
2604 for (j=0 ; j < ncb ; j++){
2605 C[i][nca+j] = B[i][j] ;
2636 typedef std::string::size_type size_type;
2641 std::vector<std::string> values(m*n);
2642 std::ostringstream oss;
2643 std::ostringstream ossFixed;
2644 std::ios_base::fmtflags original_flags = oss.flags();
2647 ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
2649 size_type maxBefore=0;
2650 size_type maxAfter=0;
2652 for (
unsigned int i=0;i<m;++i) {
2653 for (
unsigned int j=0;j<n;++j){
2655 oss << (*this)[i][j];
2656 if (oss.str().find(
"e")!=std::string::npos){
2658 ossFixed << (*this)[i][j];
2659 oss.str(ossFixed.str());
2662 values[i*n+j]=oss.str();
2663 size_type thislen=values[i*n+j].size();
2664 size_type p=values[i*n+j].find(
'.');
2666 if (p==std::string::npos){
2676 size_type totalLength=length;
2680 maxAfter=std::min(maxAfter, totalLength-maxBefore);
2681 if (maxAfter==1) maxAfter=0;
2686 if (intro) s <<intro;
2687 s <<
"["<<m<<
","<<n<<
"]=\n";
2689 for (
unsigned int i=0;i<m;i++) {
2691 for (
unsigned int j=0;j<n;j++){
2692 size_type p=values[i*n+j].find(
'.');
2693 s.setf(std::ios::right, std::ios::adjustfield);
2694 s.width((std::streamsize)maxBefore);
2695 s <<values[i*n+j].substr(0,p).c_str();
2698 s.setf(std::ios::left, std::ios::adjustfield);
2699 if (p!=std::string::npos){
2700 s.width((std::streamsize)maxAfter);
2701 s <<values[i*n+j].substr(p,maxAfter).c_str();
2704 s.width((std::streamsize)maxAfter);
2714 s.flags(original_flags);
2716 return (
int)(maxBefore+maxAfter);
2759 for (
unsigned int i=0; i < this->
getRows(); ++ i) {
2760 for (
unsigned int j=0; j <
this ->getCols(); ++ j) {
2761 os << (*this)[i][j] <<
", ";
2763 if (
this ->
getRows() != i+1) { os <<
";" << std::endl; }
2764 else { os <<
"]" << std::endl; }
2799 os <<
"([ " << std::endl;
2800 for (
unsigned int i=0; i < this->
getRows(); ++ i) {
2802 for (
unsigned int j=0; j < this->
getCols(); ++ j) {
2803 os << (*this)[i][j] <<
", ";
2805 os <<
"]," << std::endl;
2807 os <<
"])" << std::endl;
2840 for (
unsigned int i=0; i < this->
getRows(); ++ i) {
2841 for (
unsigned int j=0; j < this->
getCols(); ++ j) {
2842 os << (*this)[i][j];
2843 if (!(j==(this->
getCols()-1)))
2890 os <<
"vpMatrix " << matrixName
2891 <<
" (" <<
this ->getRows ()
2892 <<
", " <<
this ->getCols () <<
"); " <<std::endl;
2894 for (
unsigned int i=0; i < this->
getRows(); ++ i)
2896 for (
unsigned int j=0; j <
this ->getCols(); ++ j)
2900 os << matrixName <<
"[" << i <<
"][" << j
2901 <<
"] = " << (*this)[i][j] <<
"; " << std::endl;
2905 for (
unsigned int k = 0; k <
sizeof(double); ++ k)
2907 os <<
"((unsigned char*)&(" << matrixName
2908 <<
"[" << i <<
"][" << j <<
"]) )[" << k
2909 <<
"] = 0x" << std::hex
2910 << (
unsigned int)((
unsigned char*)& ((*this)[i][j])) [k]
2911 <<
"; " << std::endl;
2928 double vpMatrix::detByLU()
const
2945 unsigned int * perm =
new unsigned int[
rowNum];
2947 tmp.LUDcmp(perm, d);
2952 for(
unsigned int i=0;i<
rowNum;i++)
2959 "Cannot compute LU decomposition on a non square matrix (%dx%d)",
3012 const unsigned int c)
3016 for(
unsigned int i=r; i<(r+A.
getRows()); i++){
3017 for(
unsigned int j=c; j<(c+A.
getCols()); j++){
3018 (*this)[i][j] = A[i-r][j-c];
3024 "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
3071 "Cannot compute eigen values on a non square matrix (%dx%d)",
3075 #ifdef VISP_HAVE_GSL
3079 for (
unsigned int i=0; i <
rowNum; i++) {
3080 for (
unsigned int j=0; j <
rowNum; j++) {
3082 if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
3084 "Cannot compute eigen values on a non symetric matrix")) ;
3091 gsl_vector *eval = gsl_vector_alloc (rowNum);
3092 gsl_matrix *evec = gsl_matrix_alloc (rowNum,
colNum);
3094 gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (rowNum);
3095 gsl_matrix *m = gsl_matrix_alloc(rowNum,
colNum);
3097 unsigned int Atda = (
unsigned int)m->tda ;
3098 for (
unsigned int i=0 ; i <
rowNum ; i++){
3099 unsigned int k = i*Atda ;
3100 for (
unsigned int j=0 ; j <
colNum ; j++)
3101 m->data[k+j] = (*
this)[i][j] ;
3103 gsl_eigen_symmv (m, eval, evec, w);
3105 gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);
3107 for (
unsigned int i=0; i <
rowNum; i++) {
3108 evalue[i] = gsl_vector_get (eval, i);
3111 gsl_eigen_symmv_free (w);
3112 gsl_vector_free (eval);
3113 gsl_matrix_free (m);
3114 gsl_matrix_free (evec);
3121 "Eigen values computation is not implemented. You should install GSL rd party")) ;
3178 #ifdef VISP_HAVE_GSL
3184 if (rowNum != colNum) {
3186 "Cannot compute eigen values on a non square matrix (%dx%d)",
3190 #ifdef VISP_HAVE_GSL
3194 for (
unsigned int i=0; i < rowNum; i++) {
3195 for (
unsigned int j=0; j < rowNum; j++) {
3197 if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
3199 "Cannot compute eigen values on a non symetric matrix")) ;
3206 evector.resize(rowNum, colNum);
3208 gsl_vector *eval = gsl_vector_alloc (rowNum);
3209 gsl_matrix *evec = gsl_matrix_alloc (rowNum, colNum);
3211 gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (rowNum);
3212 gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
3214 unsigned int Atda = (
unsigned int)m->tda ;
3215 for (
unsigned int i=0 ; i < rowNum ; i++){
3216 unsigned int k = i*Atda ;
3217 for (
unsigned int j=0 ; j < colNum ; j++)
3218 m->data[k+j] = (*
this)[i][j] ;
3220 gsl_eigen_symmv (m, eval, evec, w);
3222 gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);
3224 for (
unsigned int i=0; i < rowNum; i++) {
3225 evalue[i] = gsl_vector_get (eval, i);
3227 Atda = (
unsigned int)evec->tda ;
3228 for (
unsigned int i=0; i < rowNum; i++) {
3229 unsigned int k = i*Atda ;
3230 for (
unsigned int j=0; j < rowNum; j++) {
3231 evector[i][j] = evec->data[k+j];
3235 gsl_eigen_symmv_free (w);
3236 gsl_vector_free (eval);
3237 gsl_matrix_free (m);
3238 gsl_matrix_free (evec);
3243 "Eigen values computation is not implemented. You should install GSL rd party")) ;
3263 unsigned int nbline =
getRows() ;
3264 unsigned int nbcol =
getCols() ;
3273 if (nbline < nbcol) A.
resize(nbcol,nbcol) ;
3274 else A.
resize(nbline,nbcol) ;
3276 for (i=0 ; i < nbline ; i++)
3278 for (j=0 ; j < nbcol ; j++)
3280 A[i][j] = (*this)[i][j] ;
3288 for (i=0 ; i < nbcol ; i++)
3289 if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
3291 unsigned int rank = 0 ;
3292 for (i=0 ; i < nbcol ; i++)
3293 if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
3298 unsigned int k = 0 ;
3299 for (j = 0 ; j < nbcol ; j++)
3302 if ( (fabs(sv[j]) <= maxsv*svThreshold) && (std::fabs(v.
getCol(j).
sumSquare()) > std::numeric_limits<double>::epsilon()))
3305 for (i=0 ; i < v.
getRows() ; i++)
3307 Ker[k][i] = v[i][j];
3358 det_ = this->detByLU();
3376 "Cannot compute the exponential of a non square (%dx%d) matrix",
3381 #ifdef VISP_HAVE_GSL
3383 double *b =
new double [size_];
3384 for (
size_t i=0; i< size_; i++)
3386 gsl_matrix_view m = gsl_matrix_view_array(this->
data,
rowNum, colNum);
3387 gsl_matrix_view em = gsl_matrix_view_array(b,
rowNum, colNum);
3388 gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
3391 memcpy(expA.
data, b, size_ *
sizeof(
double));
3412 for (
unsigned int i = 0; i <
rowNum;i++)
3415 for (
unsigned int j=0; j <
colNum; j++)
3417 sum += fabs((*
this)[i][j]);
3429 double sca = 1.0 / pow(2.0,s);
3434 for (
int k=2;k<=q;k++)
3436 c = c * ((double)(q-k+1)) / ((
double)(k*(2*q-k+1)));
3441 if (p) _expD=_expD+_expcX;
3442 else _expD=_expD- _expcX;
3447 for (
int k=1;k<=s;k++)
3480 for (
unsigned int i = 0 ; i < col ; i++)
3482 for (
unsigned int j = 0 ; j < row ; j++)
3483 M_comp[i][j]=M[i][j];
3484 for (
unsigned int j = row+1 ; j < M.
getRows() ; j++)
3485 M_comp[i][j-1]=M[i][j];
3487 for (
unsigned int i = col+1 ; i < M.
getCols(); i++)
3489 for (
unsigned int j = 0 ; j < row ; j++)
3490 M_comp[i-1][j]=M[i][j];
3491 for (
unsigned int j = row+1 ; j < M.
getRows() ; j++)
3492 M_comp[i-1][j-1]=M[i][j];
3511 for(
unsigned int i=0;i<M.
getCols();i++)
3513 if(min>w[i])min=w[i];
3514 if(max<w[i])max=w[i];
3529 "Cannot compute HLM on a non square matrix (%dx%d)",
3534 for(
unsigned int i=0;i<H.
getCols();i++)
3536 for(
unsigned int j=0;j<H.
getCols();j++)
3540 HLM[i][j]+= alpha*H[i][j];
3555 for (
unsigned int i=0;i<
dsize;i++) {
3556 double x = *(
data +i);
3576 for (
unsigned int i=0;i<
rowNum;i++){
3578 for (
unsigned int j=0; j<
colNum;j++){
3579 x += fabs (*(*(
rowPtrs + i)+j)) ;
3595 double sum_square=0.0;
3598 for (
unsigned int i=0;i<
rowNum;i++) {
3599 for(
unsigned int j=0;j<
colNum;j++) {
3608 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
3640 for (
unsigned int j =0 ; j <
getCols() ; j++) c[j] = (*
this)[i-1][j] ;
3656 for (
unsigned int i =0 ; i <
getRows() ; i++) c[i] = (*
this)[i][j-1] ;
3669 for (
unsigned int i=0;i<
rowNum;i++)
3670 for (
unsigned int j=0;j<
colNum;j++)
3671 if (i==j) (*this)[i][j] = val ;
3672 else (*
this)[i][j] = 0;
3675 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
vpMatrix operator*(const double &x, const vpMatrix &B)
std::ostream & csvPrint(std::ostream &os) const
Implementation of a matrix and operations on matrices.
vpColVector eigenValues() const
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
vp_deprecated vpRowVector row(const unsigned int i)
double infinityNorm() const
unsigned int kernel(vpMatrix &KerA, double svThreshold=1e-6) const
void stack(const double &d)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
vpMatrix & operator*=(const double x)
Multiply all the element of the matrix by x : Aij = Aij * x.
void stack(const vpMatrix &A)
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
error that can be emited by ViSP classes.
double * data
Address of the first element of the data array.
vp_deprecated void stackMatrices(const vpMatrix &A)
Implementation of a generic 2D array used as vase class of matrices and vectors.
static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
unsigned int getCols() const
Return the number of columns of the 2D array.
vpMatrix & operator+=(const vpMatrix &B)
Operation A = A + B.
vp_deprecated vpColVector column(const unsigned int j)
std::ostream & maplePrint(std::ostream &os) const
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
static Type maximum(const Type &a, const Type &b)
Implementation of a rotation matrix and operations on such kind of matrices.
vpRowVector getRow(const unsigned int i) const
unsigned int rowNum
Number of rows in the array.
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
void solveBySVD(const vpColVector &B, vpColVector &x) const
vp_deprecated void setIdentity(const double &val=1.0)
void svd(vpColVector &w, vpMatrix &v)
void diag(const double &val=1.0)
vp_deprecated void init()
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
vpColVector getCol(const unsigned int j) const
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
vpMatrix operator+(const vpMatrix &B) const
vpMatrix transpose() const
Implementation of a velocity twist matrix and operations on such kind of matrices.
unsigned int getRows() const
Return the number of rows of the 2D array.
double det(vpDetMethod method=LU_DECOMPOSITION) const
unsigned int colNum
Number of columns in the array.
int print(std::ostream &s, unsigned int length, char const *intro=0) const
void resize(const unsigned int i, const bool flagNullify=true)
Implementation of a force/torque twist matrix and operations on such kind of matrices.
void kron(const vpMatrix &m1, vpMatrix &out) const
Implementation of column vector and the associated operations.
double euclideanNorm() const
std::ostream & matlabPrint(std::ostream &os) const
vpMatrix inverseByLU() const
vpMatrix operator/(const double x) const
Cij = Aij / x (A is unchanged)
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
vpMatrix operator*(const vpMatrix &B) const
vpColVector stackColumns()
vpMatrix operator-() const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
unsigned int dsize
Current array size (rowNum * colNum)
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Class that consider the case of a translation vector.
double ** rowPtrs
Address of the first element of each rows.
vpMatrix & operator=(const vpArray2D< double > &A)
void resize(const unsigned int i, const bool flagNullify=true)
vpMatrix & operator<<(double *)