57 #include <visp/vpMatrix.h>
58 #include <visp/vpMath.h>
59 #include <visp/vpTranslationVector.h>
62 #include <visp/vpException.h>
63 #include <visp/vpMatrixException.h>
66 #include <visp/vpDebug.h>
80 #define DEBUG_LEVEL1 0
81 #define DEBUG_LEVEL2 0
135 unsigned int r,
unsigned int c,
136 unsigned int nrows,
unsigned int ncols)
140 if (((r + nrows) > m.
rowNum) || ((c + ncols) > m.
colNum))
144 "\n\t\t SubvpMatrix larger than vpMatrix")) ;
147 init(m,r,c,nrows,ncols);
175 const bool flagNullify)
181 { memset(this->
data,0,this->
dsize*
sizeof(
double)) ;}
185 const bool recopyNeeded = (ncols !=
this ->colNum);
186 double * copyTmp = NULL;
187 unsigned int rowTmp = 0, colTmp=0;
189 vpDEBUG_TRACE (25,
"Recopy case per case is required iff number of "
190 "cols has changed (structure of double array is not "
191 "the same in this case.");
194 copyTmp =
new double[this->
dsize];
195 memcpy (copyTmp,
this ->
data,
sizeof(
double)*this->
dsize);
200 this->
dsize = nrows*ncols;
201 this->
data = (
double*)realloc(this->
data, this->
dsize*
sizeof(
double));
202 if ((NULL == this->
data) && (0 != this->
dsize))
204 vpERROR_TRACE(
"\n\t\tMemory allocation error when allocating data") ;
206 "\n\t\t Memory allocation error when "
207 "allocating data")) ;
215 vpERROR_TRACE(
"\n\t\tMemory allocation error when allocating rowPtrs") ;
217 "\n\t\t Memory allocation error when "
218 "allocating rowPtrs")) ;
221 vpDEBUG_TRACE (25,
"Recomputation this->trsize array values.");
224 for (
unsigned int i=0; i<
dsize; i+=ncols) { *t++ = this->
data + i; }
229 vpDEBUG_TRACE (25,
"Recopy of this->data array values or nullify.");
231 { memset(this->
data,0,this->
dsize*
sizeof(
double)) ;}
237 const unsigned int minRow = (this->
rowNum<rowTmp)?this->
rowNum:rowTmp;
238 const unsigned int minCol = (this->
colNum<colTmp)?this->
colNum:colTmp;
239 for (
unsigned int i=0; i<this->
rowNum; ++i)
240 for (
unsigned int j=0; j<this->
colNum; ++j)
242 if ((minRow > i) && (minCol > j))
244 (*this)[i][j] = copyTmp [i*colTmp+j];
245 vpCDEBUG (25) << i <<
"x" << j <<
"<- " << i*colTmp+j
246 <<
"=" << copyTmp [i*colTmp+j] << std::endl;
248 else {(*this)[i][j] = 0;}
251 else {
vpDEBUG_TRACE (25,
"Nothing to do: already done by realloc.");}
254 if (copyTmp != NULL)
delete [] copyTmp;
269 std::cout << me << std::endl ;
273 unsigned int rnrows = r+nrows ;
274 unsigned int cncols = c+ncols ;
275 for (
unsigned int i=r ; i < rnrows; i++)
276 for (
unsigned int j=c ; j < cncols; j++)
277 (*
this)[i-r][j-c] = m[i][j] ;
324 std::cout << me << std::endl ;
349 for (
unsigned int i=0;i<
rowNum;i++)
350 for(
unsigned int j=0;j<
colNum;j++)
369 for (
unsigned int i=0; i<
rowNum; i++) {
370 for (
unsigned int j=0; j<
colNum; j++) {
400 std::cout << me << std::endl ;
406 vpERROR_TRACE(
"\n\t\tvpMatrix mismatch in vpMatrix/vpMatrix multiply") ;
408 "\n\t\tvpMatrix mismatch in "
409 "vpMatrix/vpMatrix multiply")) ;
413 unsigned int BcolNum = B.
colNum;
414 unsigned int BrowNum = B.
rowNum;
419 double *rowptri = A.
rowPtrs[i];
421 for (j=0;j<BcolNum;j++)
424 for (k=0;k<BrowNum;k++) s += rowptri[k] * BrowPtrs[k][j];
460 std::cout << me << std::endl ;
466 vpERROR_TRACE(
"\n\t\t vpMatrix mismatch in vpMatrix/vpMatrix addition") ;
468 "\n\t\t vpMatrix mismatch in "
469 "vpMatrix/vpMatrix addition")) ;
476 for (
unsigned int i=0;i<A.
rowNum;i++)
477 for(
unsigned int j=0;j<A.
colNum;j++)
478 CrowPtrs[i][j] = wB*BrowPtrs[i][j]+wA*ArowPtrs[i][j];
500 std::cout << me << std::endl ;
506 vpERROR_TRACE(
"\n\t\t vpMatrix mismatch in vpMatrix/vpMatrix addition") ;
508 "\n\t\t vpMatrix mismatch in "
509 "vpMatrix/vpMatrix addition")) ;
526 for (
unsigned int i=0;i<A.
rowNum;i++)
527 for(
unsigned int j=0;j<A.
colNum;j++)
530 CrowPtrs[i][j] = BrowPtrs[i][j]+ArowPtrs[i][j];
569 std::cout << me << std::endl ;
575 vpERROR_TRACE(
"\n\t\t vpMatrix mismatch in vpMatrix/vpMatrix substraction") ;
577 "\n\t\t vpMatrix mismatch in "
578 "vpMatrix/vpMatrix substraction")) ;
598 for (
unsigned int i=0;i<A.
rowNum;i++)
599 for(
unsigned int j=0;j<A.
colNum;j++)
601 CrowPtrs[i][j] = ArowPtrs[i][j]-BrowPtrs[i][j];
626 vpERROR_TRACE(
"\n\t\t vpMatrix mismatch in vpMatrix += addition") ;
628 "\n\t\t vpMatrix mismatch in "
629 "vpMatrix += addition")) ;
647 for (
unsigned int i=0;i<
rowNum;i++)
648 for(
unsigned int j=0;j<
colNum;j++)
649 rowPtrs[i][j] += BrowPtrs[i][j];
666 vpERROR_TRACE(
"\n\t\t vpMatrix mismatch in vpMatrix -= substraction") ;
668 "\n\t\t vpMatrix mismatch in "
669 "vpMatrix -= substraction")) ;
686 for (
unsigned int i=0;i<
rowNum;i++)
687 for(
unsigned int j=0;j<
colNum;j++)
688 rowPtrs[i][j] -= BrowPtrs[i][j];
716 std::cout << me << std::endl ;
736 for (
unsigned int i=0;i<A.
rowNum;i++)
737 for(
unsigned int j=0;j<A.
colNum;j++)
738 CrowPtrs[i][j]= -ArowPtrs[i][j];
780 for (
unsigned int i=0;i<
rowNum;i++)
781 for(
unsigned int j=0;j<
colNum;j++)
813 vpERROR_TRACE(
"vpMatrix mismatch in vpMatrix/vector multiply") ;
824 std::cout << me << std::endl ;
829 for (
unsigned int j=0;j<A.
colNum;j++)
832 for (
unsigned int i=0;i<A.
rowNum;i++)
859 vpERROR_TRACE(
"vpMatrix mismatch in vpMatrix::operator*(const vpTranslationVector)") ;
863 for (
unsigned int j=0;j<3;j++) c[j]=0 ;
865 for (
unsigned int j=0;j<3;j++) {
868 for (
unsigned int i=0;i<3;i++) {
895 std::cout << me << std::endl ;
900 unsigned int Brow = B.
getRows() ;
901 unsigned int Bcol = B.
getCols() ;
916 for (
unsigned int i=0;i<Brow;i++)
917 for(
unsigned int j=0;j<Bcol;j++)
938 std::cout << me << std::endl ;
953 for (
unsigned int i=0;i<
rowNum;i++)
954 for(
unsigned int j=0;j<
colNum;j++)
955 CrowPtrs[i][j]=
rowPtrs[i][j]*x;
978 if (std::fabs(x) <= std::numeric_limits<double>::epsilon()) {
996 for (
unsigned int i=0;i<
rowNum;i++)
997 for(
unsigned int j=0;j<
colNum;j++)
1021 for (
unsigned int i=0;i<
rowNum;i++)
1022 for(
unsigned int j=0;j<
colNum;j++)
1046 for (
unsigned int i=0;i<
rowNum;i++)
1047 for(
unsigned int j=0;j<
colNum;j++)
1063 for (
unsigned int i=0;i<
rowNum;i++)
1064 for(
unsigned int j=0;j<
colNum;j++)
1074 if (std::fabs(x) <= std::numeric_limits<double>::epsilon())
1089 for (
unsigned int i=0;i<
rowNum;i++)
1090 for(
unsigned int j=0;j<
colNum;j++)
1119 if (i==j) (*this)[i][j] = val ;
else (*
this)[i][j] = 0;
1163 for (
unsigned int i=0; i<
rowNum; i++)
1164 for (
unsigned int j=0; j<
colNum; j++)
1165 if (i == j) (*this)[i][j] = 1;
1166 else (*
this)[i][j] = 0;
1191 double *coli = (*this)[i] ;
1229 double ** AtRowPtrs = At.
rowPtrs;
1231 for(
unsigned int i = 0; i <
colNum; i++ )
1233 double *
row = AtRowPtrs[i];
1235 for(
unsigned int j = 0; j <
rowNum; j++, col+=A_step )
1279 for(
unsigned int i=0;i<
rowNum;i++){
1280 for(
unsigned int j=i;j<
rowNum;j++){
1286 for(
unsigned int k=0; k <
colNum ;k++)
1287 ssum += *(pi++)* *(pj++);
1334 s +=(*(ptr+i)) * (*(ptr+j));
1344 s +=(*(ptr+i)) * (*(ptr+i));
1383 double *optr=out.
data;
1384 for(
unsigned int j =0;j<
colNum ; j++){
1385 for(
unsigned int i =0;i<
rowNum ; i++){
1419 double *optr=out.
data;
1420 for(
unsigned int i =0;i<
dsize ; i++){
1421 *(optr++)=*(mdata++);
1442 unsigned int r1= m1.
getRows();
1443 unsigned int c1= m1.
getCols();
1444 unsigned int r2= m2.
getRows();
1445 unsigned int c2= m2.
getCols();
1450 vpERROR_TRACE(
"Kronecker prodect bad dimension of output vpMatrix") ;
1454 for(
unsigned int r =0;r<r1 ; r++){
1455 for(
unsigned int c =0;c<c1 ; c++){
1456 double alpha = m1[r][c];
1457 double *m2ptr = m2[0];
1458 unsigned int roffset= r*r2;
1459 unsigned int coffset= c*c2;
1460 for(
unsigned int rr =0;rr<r2 ; rr++){
1461 for(
unsigned int cc =0;cc<c2 ;cc++){
1462 out[roffset+rr][coffset+cc]= alpha* *(m2ptr++);
1487 unsigned int r1= m1.
getRows();
1488 unsigned int c1= m1.
getCols();
1489 unsigned int r2= m2.
getRows();
1490 unsigned int c2= m2.
getCols();
1494 for(
unsigned int r =0;r<r1 ; r++){
1495 for(
unsigned int c =0;c<c1 ; c++){
1496 double alpha = m1[r][c];
1497 double *m2ptr = m2[0];
1498 unsigned int roffset= r*r2;
1499 unsigned int coffset= c*c2;
1500 for(
unsigned int rr =0;rr<r2 ; rr++){
1501 for(
unsigned int cc =0;cc<c2 ;cc++){
1502 out[roffset+rr ][coffset+cc]= alpha* *(m2ptr++);
1517 return kron(*
this,m);
1702 #if (DEBUG_LEVEL1 == 0)
1707 #if defined (VISP_HAVE_LAPACK)
1709 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020100) // Require opencv >= 2.1.0
1711 #elif defined (VISP_HAVE_GSL)
1722 unsigned int i,j,k,nrows,ncols;
1729 #ifdef VISP_HAVE_GSL
1738 Asvd.
resize(nrows,ncols);
1740 for (i = 0 ; i < nrows ; i++)
1742 for (j = 0 ; j < ncols ; j++)
1745 for (k=0 ; k < ncols ; k++) Asvd[i][j] += (*
this)[i][k]*w[k]*v[j][k];
1748 for (i=0;i<nrows;i++)
1750 for (j=0;j<ncols;j++)
if (fabs(A[i][j]-Asvd[i][j]) > 1e-6) pb = 1;
1754 printf(
"pb in SVD\n");
1755 std::cout <<
" A : " << std::endl << A << std::endl;
1756 std::cout <<
" Asvd : " << std::endl << Asvd << std::endl;
1871 unsigned int i, j, k ;
1873 unsigned int nrows, ncols;
1874 unsigned int nrows_orig =
getRows() ;
1875 unsigned int ncols_orig =
getCols() ;
1876 Ap.
resize(ncols_orig,nrows_orig) ;
1878 if (nrows_orig >= ncols_orig)
1894 if (nrows_orig >= ncols_orig) a = *
this;
1895 else a = (*this).
t();
1901 for (i=0 ; i < ncols ; i++)
1902 if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
1904 unsigned int rank = 0 ;
1905 for (i=0 ; i < ncols ; i++)
1906 if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
1911 for (i = 0 ; i < ncols ; i++)
1913 for (j = 0 ; j < nrows ; j++)
1917 for (k=0 ; k < ncols ; k++)
1918 if (fabs(sv[k]) > maxsv*svThreshold)
1920 a1[i][j] += v[i][k]*a[j][k]/sv[k];
1924 if (nrows_orig >= ncols_orig) Ap = a1;
1927 if (nrows_orig >= ncols_orig)
1930 imAt.
resize(ncols_orig,rank) ;
1931 for (i=0 ; i < ncols_orig ; i++)
1932 for (j=0 ; j < rank ; j++)
1933 imAt[i][j] = v[i][j] ;
1936 imA.
resize(nrows_orig,rank) ;
1937 for (i=0 ; i < nrows_orig ; i++)
1938 for (j=0 ; j < rank ; j++)
1939 imA[i][j] = a[i][j] ;
1944 imAt.
resize(ncols_orig,rank) ;
1945 for (i=0 ; i < ncols_orig ; i++)
1946 for (j=0 ; j < rank ; j++)
1947 imAt[i][j] = a[i][j] ;
1949 imA.
resize(nrows_orig,rank) ;
1950 for (i=0 ; i < nrows_orig ; i++)
1951 for (j=0 ; j < rank ; j++)
1952 imA[i][j] = v[i][j] ;
1959 vpMatrix A, ApA, AAp, AApA, ApAAp ;
1972 for (i=0;i<nrows;i++)
1974 for (j=0;j<ncols;j++)
if (fabs(AApA[i][j]-A[i][j]) > 1e-6) pb = 1;
1976 for (i=0;i<ncols;i++)
1978 for (j=0;j<nrows;j++)
if (fabs(ApAAp[i][j]-Ap[i][j]) > 1e-6) pb = 1;
1980 for (i=0;i<nrows;i++)
1982 for (j=0;j<nrows;j++)
if (fabs(AAp[i][j]-AAp[j][i]) > 1e-6) pb = 1;
1984 for (i=0;i<ncols;i++)
1986 for (j=0;j<ncols;j++)
if (fabs(ApA[i][j]-ApA[j][i]) > 1e-6) pb = 1;
1990 printf(
"pb in pseudo inverse\n");
1991 std::cout <<
" A : " << std::endl << A << std::endl;
1992 std::cout <<
" Ap : " << std::endl << Ap << std::endl;
1993 std::cout <<
" A - AApA : " << std::endl << A - AApA << std::endl;
1994 std::cout <<
" Ap - ApAAp : " << std::endl << Ap - ApAAp << std::endl;
1995 std::cout <<
" AAp - (AAp)^T : " << std::endl << AAp - AAp.
t() << std::endl;
1996 std::cout <<
" ApA - (ApA)^T : " << std::endl << ApA - ApA.
t() << std::endl;
2050 unsigned int i, j, k ;
2052 unsigned int nrows, ncols;
2053 unsigned int nrows_orig =
getRows() ;
2054 unsigned int ncols_orig =
getCols() ;
2055 Ap.
resize(ncols_orig,nrows_orig) ;
2057 if (nrows_orig >= ncols_orig)
2073 if (nrows_orig >= ncols_orig) a = *
this;
2074 else a = (*this).
t();
2080 for (i=0 ; i < ncols ; i++)
2081 if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
2083 unsigned int rank = 0 ;
2084 for (i=0 ; i < ncols ; i++)
2085 if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
2090 for (i = 0 ; i < ncols ; i++)
2092 for (j = 0 ; j < nrows ; j++)
2096 for (k=0 ; k < ncols ; k++)
2097 if (fabs(sv[k]) > maxsv*svThreshold)
2099 a1[i][j] += v[i][k]*a[j][k]/sv[k];
2103 if (nrows_orig >= ncols_orig) Ap = a1;
2106 if (nrows_orig >= ncols_orig)
2109 imAt.
resize(ncols_orig,rank) ;
2110 for (i=0 ; i < ncols_orig ; i++)
2111 for (j=0 ; j < rank ; j++)
2112 imAt[i][j] = v[i][j] ;
2115 imA.
resize(nrows_orig,rank) ;
2116 for (i=0 ; i < nrows_orig ; i++)
2117 for (j=0 ; j < rank ; j++)
2118 imA[i][j] = a[i][j] ;
2123 imAt.
resize(ncols_orig,rank) ;
2124 for (i=0 ; i < ncols_orig ; i++)
2125 for (j=0 ; j < rank ; j++)
2126 imAt[i][j] = a[i][j] ;
2128 imA.
resize(nrows_orig,rank) ;
2129 for (i=0 ; i < nrows_orig ; i++)
2130 for (j=0 ; j < rank ; j++)
2131 imA[i][j] = v[i][j] ;
2135 vpMatrix cons(ncols_orig, ncols_orig);
2138 for (j = 0; j < ncols_orig; j++)
2140 for (i = 0; i < ncols_orig; i++)
2142 if (fabs(sv[i]) <= maxsv*svThreshold)
2144 cons[i][j] = v[j][i];
2149 vpMatrix Ker (ncols_orig-rank, ncols_orig);
2151 for (j = 0; j < ncols_orig ; j++)
2154 if ( std::fabs(cons.
row(j+1).
sumSquare()) > std::numeric_limits<double>::epsilon())
2156 for (i = 0; i < cons.
getCols(); i++)
2157 Ker[k][i] = cons[j][i];
2167 vpMatrix A, ApA, AAp, AApA, ApAAp ;
2180 for (i=0;i<nrows;i++)
2182 for (j=0;j<ncols;j++)
if (fabs(AApA[i][j]-A[i][j]) > 1e-6) pb = 1;
2184 for (i=0;i<ncols;i++)
2186 for (j=0;j<nrows;j++)
if (fabs(ApAAp[i][j]-Ap[i][j]) > 1e-6) pb = 1;
2188 for (i=0;i<nrows;i++)
2190 for (j=0;j<nrows;j++)
if (fabs(AAp[i][j]-AAp[j][i]) > 1e-6) pb = 1;
2192 for (i=0;i<ncols;i++)
2194 for (j=0;j<ncols;j++)
if (fabs(ApA[i][j]-ApA[j][i]) > 1e-6) pb = 1;
2198 printf(
"pb in pseudo inverse\n");
2199 std::cout <<
" A : " << std::endl << A << std::endl;
2200 std::cout <<
" Ap : " << std::endl << Ap << std::endl;
2201 std::cout <<
" A - AApA : " << std::endl << A - AApA << std::endl;
2202 std::cout <<
" Ap - ApAAp : " << std::endl << Ap - ApAAp << std::endl;
2203 std::cout <<
" AAp - (AAp)^T : " << std::endl << AAp - AAp.
t() << std::endl;
2204 std::cout <<
" ApA - (ApA)^T : " << std::endl << ApA - ApA.
t() << std::endl;
2205 std::cout <<
" KerA : " << std::endl << kerA << std::endl;
2227 for (
unsigned int i =0 ; i <
getCols() ; i++) c[i] = (*
this)[j-1][i] ;
2242 for (
unsigned int i =0 ; i <
getRows() ; i++) c[i] = (*
this)[i][j-1] ;
2292 unsigned int nra = A.
getRows() ;
2293 unsigned int nrb = B.
getRows() ;
2300 "\n\t\t incorrect matrices size")) ;
2314 for (i=0 ; i < nra ; i++)
2315 for (j=0 ; j < A.
getCols() ; j++)
2319 for (i=0 ; i < nrb ; i++)
2320 for (j=0 ; j < B.
getCols() ; j++)
2322 C[i+nra][j] = B[i][j] ;
2347 const unsigned int r,
const unsigned int c)
2378 const unsigned int r,
const unsigned int c)
2391 for(
unsigned int i=0; i<A.
getRows(); i++){
2392 for(
unsigned int j=0; j<A.
getCols(); j++){
2393 if(i >= r && i < (r + B.
getRows()) && j >= c && j < (c+B.
getCols())){
2394 C[i][j] = B[i-r][j-c];
2404 "\n\t\tIncorrect size of the matrix to insert data.");
2451 unsigned int nca = A.
getCols() ;
2452 unsigned int ncb = B.
getCols() ;
2459 "\n\t\t incorrect matrices size")) ;
2473 for (i=0 ; i < C.
getRows(); i++)
2474 for (j=0 ; j < nca ; j++)
2478 for (i=0 ; i < C.
getRows() ; i++)
2479 for (j=0 ; j < ncb ; j++)
2481 C[i][nca+j] = B[i][j] ;
2523 unsigned int rows = A.
getRows() ;
2525 this->
resize(rows,rows) ;
2534 for (
unsigned int i=0 ; i< rows ; i++ )
2535 (*
this)[i][i] = A[i] ;
2551 unsigned int rows = A.
getRows() ;
2562 for (
unsigned int i=0 ; i< rows ; i++ )
2577 for (
unsigned int i=0;i<m.
getRows();i++) {
2578 for (
unsigned int j=0;j<m.
getCols();j++){
2579 s << m[i][j] <<
" ";
2615 typedef std::string::size_type size_type;
2620 std::vector<std::string> values(m*n);
2621 std::ostringstream oss;
2622 std::ostringstream ossFixed;
2624 ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
2626 size_type maxBefore=0;
2627 size_type maxAfter=0;
2629 for (
unsigned int i=0;i<m;++i) {
2630 for (
unsigned int j=0;j<n;++j){
2632 oss << (*this)[i][j];
2633 if (oss.str().find(
"e")!=std::string::npos){
2635 ossFixed << (*this)[i][j];
2636 oss.str(ossFixed.str());
2639 values[i*n+j]=oss.str();
2640 size_type thislen=values[i*n+j].size();
2641 size_type p=values[i*n+j].find(
'.');
2643 if (p==std::string::npos){
2653 size_type totalLength=length;
2657 maxAfter=std::min(maxAfter, totalLength-maxBefore);
2658 if (maxAfter==1) maxAfter=0;
2661 std::cerr <<totalLength <<
" " <<maxBefore <<
" " <<maxAfter <<
"\n";
2663 if (intro) s <<intro;
2664 s <<
"["<<m<<
","<<n<<
"]=\n";
2666 for (
unsigned int i=0;i<m;i++) {
2668 for (
unsigned int j=0;j<n;j++){
2669 size_type p=values[i*n+j].find(
'.');
2670 s.setf(std::ios::right, std::ios::adjustfield);
2671 s.width((std::streamsize)maxBefore);
2672 s <<values[i*n+j].substr(0,p).c_str();
2675 s.setf(std::ios::left, std::ios::adjustfield);
2676 if (p!=std::string::npos){
2677 s.width((std::streamsize)maxAfter);
2678 s <<values[i*n+j].substr(p,maxAfter).c_str();
2681 s.width((std::streamsize)maxAfter);
2691 return (
int)(maxBefore+maxAfter);
2710 for (i=0; i < this->
getRows(); ++ i)
2712 for (j=0; j <
this ->getCols(); ++ j)
2714 os << (*this)[i][j] <<
", ";
2716 if (
this ->
getRows() != i+1) { os <<
";" << std::endl; }
2717 else { os <<
"]" << std::endl; }
2737 cppPrint(std::ostream & os,
const char * matrixName,
bool octet)
2741 const char defaultName [] =
"A";
2742 if (NULL == matrixName)
2744 matrixName = defaultName;
2746 os <<
"vpMatrix " << defaultName
2747 <<
" (" <<
this ->getRows ()
2748 <<
", " <<
this ->getCols () <<
"); " <<std::endl;
2750 for (i=0; i < this->
getRows(); ++ i)
2752 for (j=0; j <
this ->getCols(); ++ j)
2756 os << defaultName <<
"[" << i <<
"][" << j
2757 <<
"] = " << (*this)[i][j] <<
"; " << std::endl;
2761 for (
unsigned int k = 0; k <
sizeof(double); ++ k)
2763 os <<
"((unsigned char*)&(" << defaultName
2764 <<
"[" << i <<
"][" << j <<
"]) )[" << k
2765 <<
"] = 0x" <<std::hex<<
2766 (
unsigned int)((
unsigned char*)& ((*this)[i][j])) [k]
2767 <<
"; " << std::endl;
2789 for (
unsigned int i=0;i<
dsize;i++) {
2790 x = *(
data +i); norm += x*x;
2813 for (
unsigned int i=0;i<
rowNum;i++){
2815 for (
unsigned int j=0; j<
colNum;j++){
2816 x += fabs (*(*(
rowPtrs + i)+j)) ;
2833 double vpMatrix::detByLU()
const
2850 unsigned int * perm =
new unsigned int[
rowNum];
2852 tmp.LUDcmp(perm, d);
2857 for(
unsigned int i=0;i<
rowNum;i++)
2864 vpERROR_TRACE(
"Determinant Computation : ERR Matrix not squared") ;
2866 "\n\t\tDeterminant Computation : ERR Matrix not squared")) ;
2909 const unsigned int c)
2913 for(
unsigned int i=r; i<(r+A.
getRows()); i++){
2914 for(
unsigned int j=c; j<(c+A.
getCols()); j++){
2915 (*this)[i][j] = A[i-r][j-c];
2921 "\n\t\tIncorrect size of the matrix to insert data.");
2968 vpERROR_TRACE(
"Eigen values computation: ERR Matrix not square") ;
2970 "\n\t\tEigen values computation: ERR Matrix not square")) ;
2973 #ifdef VISP_HAVE_GSL
2977 for (
unsigned int i=0; i <
rowNum; i++) {
2978 for (
unsigned int j=0; j <
rowNum; j++) {
2980 if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
2981 vpERROR_TRACE(
"Eigen values computation: ERR Matrix not symmetric") ;
2983 "\n\t\tEigen values computation: "
2984 "ERR Matrix not symmetric")) ;
2992 gsl_vector *eval = gsl_vector_alloc (rowNum);
2993 gsl_matrix *evec = gsl_matrix_alloc (rowNum,
colNum);
2995 gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (rowNum);
2996 gsl_matrix *m = gsl_matrix_alloc(rowNum,
colNum);
2998 unsigned int Atda = m->tda ;
2999 for (
unsigned int i=0 ; i <
rowNum ; i++){
3000 unsigned int k = i*Atda ;
3001 for (
unsigned int j=0 ; j <
colNum ; j++)
3002 m->data[k+j] = (*
this)[i][j] ;
3004 gsl_eigen_symmv (m, eval, evec, w);
3006 gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);
3008 for (
unsigned int i=0; i <
rowNum; i++) {
3009 evalue[i] = gsl_vector_get (eval, i);
3012 gsl_eigen_symmv_free (w);
3013 gsl_vector_free (eval);
3014 gsl_matrix_free (m);
3015 gsl_matrix_free (evec);
3021 vpERROR_TRACE(
"Not implemented since the GSL library is not detected.") ;
3023 "\n\t\tEigen values Computation: Not implemented "
3024 "since the GSL library is not detected")) ;
3083 #ifdef VISP_HAVE_GSL
3090 vpERROR_TRACE(
"Eigen values computation: ERR Matrix not square") ;
3092 "\n\t\tEigen values computation: ERR Matrix not square")) ;
3095 #ifdef VISP_HAVE_GSL
3099 for (
unsigned int i=0; i <
rowNum; i++) {
3100 for (
unsigned int j=0; j <
rowNum; j++) {
3102 if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
3103 vpERROR_TRACE(
"Eigen values computation: ERR Matrix not symmetric") ;
3105 "\n\t\tEigen values computation: "
3106 "ERR Matrix not symmetric")) ;
3115 gsl_vector *eval = gsl_vector_alloc (rowNum);
3116 gsl_matrix *evec = gsl_matrix_alloc (rowNum,
colNum);
3118 gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (rowNum);
3119 gsl_matrix *m = gsl_matrix_alloc(rowNum,
colNum);
3121 unsigned int Atda = m->tda ;
3122 for (
unsigned int i=0 ; i <
rowNum ; i++){
3123 unsigned int k = i*Atda ;
3124 for (
unsigned int j=0 ; j <
colNum ; j++)
3125 m->data[k+j] = (*
this)[i][j] ;
3127 gsl_eigen_symmv (m, eval, evec, w);
3129 gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);
3131 for (
unsigned int i=0; i <
rowNum; i++) {
3132 evalue[i] = gsl_vector_get (eval, i);
3135 for (
unsigned int i=0; i <
rowNum; i++) {
3136 unsigned int k = i*Atda ;
3137 for (
unsigned int j=0; j <
rowNum; j++) {
3138 evector[i][j] = evec->
data[k+j];
3160 gsl_eigen_symmv_free (w);
3161 gsl_vector_free (eval);
3162 gsl_matrix_free (m);
3163 gsl_matrix_free (evec);
3167 vpERROR_TRACE(
"Not implemented since the GSL library is not detected.") ;
3169 "\n\t\tEigen values Computation: Not implemented "
3170 "since the GSL library is not detected")) ;
3189 std::cout <<
"begin Kernel" << std::endl ;
3192 unsigned int ncaptor =
getRows() ;
3193 unsigned int ddl =
getCols() ;
3197 std::cout <<
"Erreur Matrice non initialise" << std::endl ;
3201 std::cout <<
"Interaction matrix L" << std::endl ;
3202 std::cout << *this ;
3203 std::cout <<
"signaux capteurs : " << ncaptor << std::endl ;
3210 if (ncaptor > ddl) min = ddl ;
else min = ncaptor ;
3231 for (i=0 ; i < ncaptor ; i++)
3232 for (j=0 ; j < ddl ; j++)
3234 a1[i][j] = (*this)[i][j] ;
3235 a2[i][j] = (*this)[i][j] ;
3241 for (i=ncaptor ; i < ddl ; i++)
3242 for (j=0 ; j < ddl ; j++)
3256 for (i=0 ; i < ddl ; i++)
3257 if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
3261 std::cout <<
"Singular Value : (" ;
3262 for (i=0 ; i < ddl ; i++) std::cout << sv[i] <<
" , " ;
3263 std::cout <<
")" << std::endl ;
3267 unsigned int rank = 0 ;
3268 for (i=0 ; i < ddl ; i++)
3269 if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
3274 for (i = 0 ; i < ddl ; i++)
3276 for (j = 0 ; j < ncaptor ; j++)
3284 for (k=0 ; k < ddl ; k++) if (fabs(sv[k]) > maxsv*svThreshold)
3286 C[i][j] += v[i][k]*a1[j][k]/sv[k];
3292 std::cout << C << std::endl ;
3309 for (i=0 ; i < ddl ; i++)
3310 if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
3311 unsigned int rank = 0 ;
3312 for (i=0 ; i < ddl ; i++)
3313 if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
3317 for (j = 0 ; j < ddl ; j++)
3319 for (i = 0 ; i < ddl ; i++)
3322 if (fabs(sv[i]) <= maxsv*svThreshold)
3324 cons[i][j] = v[j][i];
3330 for (j = 0 ; j < ddl ; j++)
3335 if (std::fabs(cons.
row(j+1).
sumSquare()) > std::numeric_limits<double>::epsilon())
3337 for (i=0 ; i < cons.
getCols() ; i++)
3338 Ker[k][i] = cons[j][i] ;
3347 std::cout <<
"end Kernel" << std::endl ;
3388 det = this->detByLU();
3410 const bool binary,
const char *Header)
3415 file.open(filename, std::fstream::out);
3417 file.open(filename, std::fstream::out|std::fstream::binary);
3431 while (Header[i] !=
'\0')
3434 if (Header[i] ==
'\n')
3441 file << M << std::endl;
3446 while (Header[headerSize] !=
'\0') headerSize++;
3447 file.write(Header,headerSize+1);
3448 unsigned int matrixSize;
3450 file.write((
char*)&matrixSize,
sizeof(
int));
3452 file.write((
char*)&matrixSize,
sizeof(
int));
3454 for(
unsigned int i = 0; i < M.
getRows(); i++)
3456 for(
unsigned int j = 0; j < M.
getCols(); j++)
3459 file.write((
char*)&value,
sizeof(
double));
3488 file.open(filename, std::fstream::in);
3490 file.open(filename, std::fstream::in|std::fstream::binary);
3504 while ((c !=
'\0') && (c !=
'\n'))
3510 strncpy(Header, h.c_str(), h.size() + 1);
3512 unsigned int rows, cols;
3518 for(
unsigned int i = 0; i < rows; i++)
3520 for(
unsigned int j = 0; j < cols; j++)
3531 while ((c !=
'\0') && (c !=
'\n'))
3537 strncpy(Header, h.c_str(), h.size() + 1);
3539 unsigned int rows, cols;
3540 file.read((
char*)&rows,
sizeof(
unsigned int));
3541 file.read((
char*)&cols,
sizeof(
unsigned int));
3545 for(
unsigned int i = 0; i < rows; i++)
3547 for(
unsigned int j = 0; j < cols; j++)
3549 file.read((
char*)&value,
sizeof(
double));
3573 vpTRACE(
"The matrix must be square");
3575 "The matrix must be square" ));
3595 for (
unsigned int i = 0; i <
rowNum;i++)
3598 for (
unsigned int j=0; j <
colNum; j++)
3600 sum += fabs((*
this)[i][j]);
3612 double sca = 1.0 / pow(2.0,s);
3617 for (
int k=2;k<=q;k++)
3619 c = c * ((double)(q-k+1)) / ((
double)(k*(2*q-k+1)));
3624 if (p) _expD=_expD+_expcX;
3625 else _expD=_expD- _expcX;
3630 for (
int k=1;k<=s;k++)
3642 double *dataptr =
data;
3643 double min = *dataptr;
3645 for (
unsigned int i = 0; i <
dsize-1; i++)
3647 if (*dataptr < min) min = *dataptr;
3656 double *dataptr =
data;
3657 double max = *dataptr;
3659 for (
unsigned int i = 0; i <
dsize-1; i++)
3661 if (*dataptr > max) max = *dataptr;
3691 for (
unsigned int i = 0 ; i < col ; i++)
3693 for (
unsigned int j = 0 ; j <
row ; j++)
3694 M_comp[i][j]=M[i][j];
3695 for (
unsigned int j = row+1 ; j < M.
getRows() ; j++)
3696 M_comp[i][j-1]=M[i][j];
3698 for (
unsigned int i = col+1 ; i < M.
getCols(); i++)
3700 for (
unsigned int j = 0 ; j <
row ; j++)
3701 M_comp[i-1][j]=M[i][j];
3702 for (
unsigned int j = row+1 ; j < M.
getRows() ; j++)
3703 M_comp[i-1][j-1]=M[i][j];
vpMatrix operator*(const double &x, const vpMatrix &B)
friend VISP_EXPORT std::ostream & operator<<(std::ostream &s, const vpMatrix &m)
std::cout a matrix
Definition of the vpMatrix class.
unsigned int kernel(vpMatrix &KerA, double svThreshold=1e-6)
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
vpRowVector row(const unsigned int i)
Row extraction.
double infinityNorm() const
void init()
Initialization of the object matrix.
Definition of the row vector class.
static bool saveMatrix(const char *filename, const vpMatrix &M, const bool binary=false, const char *Header="")
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
double getMinValue() const
vpMatrix & operator*=(const double x)
Multiply all the element of the matrix by x : Aij = Aij * x.
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
vpMatrix & operator+=(const vpMatrix &B)
Operation A = A + B.
double sumSquare() const
return sum of the Aij^2 (for all i, for all j)
vpMatrix()
Basic constructor.
int print(std::ostream &s, unsigned int length, char const *intro=0)
vpColVector column(const unsigned int j)
Column extraction.
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
static Type maximum(const Type &a, const Type &b)
double * data
address of the first element of the data array
std::ostream & cppPrint(std::ostream &os, const char *matrixName=NULL, bool octet=false)
Print to be used as part of a C++ code later.
unsigned int trsize
Total row space.
vpColVector eigenValues()
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
void solveBySVD(const vpColVector &B, vpColVector &x) const
void setIdentity(const double &val=1.0)
double ** rowPtrs
address of the first element of each rows
void svd(vpColVector &w, vpMatrix &v)
vpMatrix & operator=(const vpMatrix &B)
Copy operator. Allow operation such as A = B.
static void multMatrixVector(const vpMatrix &A, const vpColVector &b, vpColVector &c)
static bool loadMatrix(const char *filename, vpMatrix &M, const bool binary=false, char *Header=NULL)
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
vpMatrix operator+(const vpMatrix &B) const
vpMatrix transpose() const
void kill()
Destruction of the matrix (Memory de-allocation)
double det(vpDetMethod method=LU_DECOMPOSITION) const
VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpImagePoint &ip)
static vpMatrix stackMatrices(const vpMatrix &A, const vpMatrix &B)
Stack two Matrices C = [ A B ]^T.
void diag(const vpColVector &A)
unsigned int rowNum
number of rows
double getMaxValue() const
void resize(unsigned int i)
Set the size of the Row vector.
Class that provides a data structure for the column vectors as well as a set of operations on these v...
double euclideanNorm() const
vpMatrix inverseByLU() const
unsigned int getCols() const
Return the number of columns of the matrix.
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
error that can be emited by the vpMatrix class and its derivates
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 size (rowNum * colNum)
void kron(const vpMatrix &m1, vpMatrix &out)
unsigned int colNum
number of columns
unsigned int getRows() const
Return the number of rows of the matrix.
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
Class that consider the case of a translation vector.
virtual ~vpMatrix()
Destructor (Memory de-allocation)
std::ostream & matlabPrint(std::ostream &os)
Print using matlab syntax, to be put in matlab later.
void resize(const unsigned int i, const bool flagNullify=true)