The MRPT project: ops_matrices.h Source File

00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2010  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef  mrpt_math_matrix_ops_H
00029 #define  mrpt_math_matrix_ops_H
00030 
00031 #include <mrpt/math/math_frwds.h>  // Fordward declarations
00032 
00033 #include <mrpt/math/CMatrix.h>
00034 #include <mrpt/math/CMatrixD.h>
00035 #include <mrpt/utils/CStream.h>
00036 
00037 #include <mrpt/math/CMatrixTemplateNumeric.h>
00038 #include <mrpt/math/CMatrixFixedNumeric.h>
00039 #include <mrpt/math/CVectorTemplate.h>
00040 
00041 #include <mrpt/math/ops_containers.h>           // Many generic operations
00042 #include <mrpt/math/ops_matrices_eigen.h>       // Eigenvectors are apart because it's a mess of code...
00043 #include <mrpt/math/matrices_metaprogramming.h>  // TMatrixProductType, ...
00044 
00045 
00046 /** \file ops_matrices.h
00047   * This file implements miscelaneous matrix and matrix/vector operations, plus internal functions in mrpt::math::detail
00048   *
00049   */
00050 
00051 namespace mrpt
00052 {
00053         namespace math
00054         {
00055                 /** Read operator from a CStream. The format is compatible with that of CMatrix & CMatrixD */
00056                 template <size_t NROWS,size_t NCOLS>
00057                 mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in, CMatrixFixedNumeric<float,NROWS,NCOLS> & M) {
00058                         CMatrix  aux;
00059                         in.ReadObject(&aux);
00060                         ASSERTMSG_(M.size()==aux.size(), format("Size mismatch: deserialized is %ux%u, expected is %ux%u",(unsigned)aux.getRowCount(),(unsigned)aux.getColCount(),(unsigned)NROWS,(unsigned)NCOLS))
00061                         M = aux;
00062                         return in;
00063                 }
00064                 /** Read operator from a CStream. The format is compatible with that of CMatrix & CMatrixD */
00065                 template <size_t NROWS,size_t NCOLS>
00066                 mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in, CMatrixFixedNumeric<double,NROWS,NCOLS> & M) {
00067                         CMatrixD  aux;
00068                         in.ReadObject(&aux);
00069                         ASSERTMSG_(M.size()==aux.size(), format("Size mismatch: deserialized is %ux%u, expected is %ux%u",(unsigned)aux.getRowCount(),(unsigned)aux.getColCount(),(unsigned)NROWS,(unsigned)NCOLS))
00070                         M = aux;
00071                         return in;
00072                 }
00073 
00074                 /** Write operator for writing into a CStream. The format is compatible with that of CMatrix & CMatrixD */
00075                 template <size_t NROWS,size_t NCOLS>
00076                 mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<float,NROWS,NCOLS> & M) {
00077                         CMatrix aux = CMatrixFloat(M);
00078                         out.WriteObject(&aux);
00079                         return out;
00080                 }
00081                 /** Write operator for writing into a CStream. The format is compatible with that of CMatrix & CMatrixD */
00082                 template <size_t NROWS,size_t NCOLS>
00083                 mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<double,NROWS,NCOLS> & M) {
00084                         CMatrixD aux = CMatrixDouble(M);
00085                         out.WriteObject(&aux);
00086                         return out;
00087                 }
00088 
00089                 /** Equal comparison (==)  */
00090                 template <class T,size_t NROWS, size_t NCOLS>
00091                 bool operator == (const CMatrixFixedNumeric<T,NROWS,NCOLS>& M1, const CMatrixFixedNumeric<T,NROWS,NCOLS>& M2)
00092                 {
00093                         for (size_t i=0; i < NROWS; i++)
00094                                 for (size_t j=0; j < NCOLS; j++)
00095                                         if (M1.get_unsafe(i,j)!=M2.get_unsafe(i,j))
00096                                                 return false;
00097                         return true;
00098                 }
00099 
00100                 /** Textual output stream function.
00101                   *    Use only for text output, for example:  "std::cout << mat;"
00102                   */
00103                 template <class MATRIX>
00104                 RET_TYPE_ASSERT_MRPTMATRIX(MATRIX, std::ostream)& // This expands into "std::ostream &"
00105                 operator << (std::ostream& ostrm, const MATRIX& m)
00106                 {
00107                         ostrm << std::setprecision(4);
00108                         for (size_t i=0; i < m.getRowCount(); i++)
00109                         {
00110                                 for (size_t j=0; j < m.getColCount(); j++)
00111                                         ostrm << std::setw(13) << m(i,j);
00112                                 ostrm << std::endl;
00113                         }
00114                         return ostrm;
00115                 }
00116 
00117 
00118 
00119 
00120         // ------ Implementatin of detail functions -------------
00121         namespace detail
00122         {
00123                 /** Cholesky factorization: in = out' · out  (Upper triangular version: M=U'*U )
00124                   *     Given a positive-definite symmetric matrix, this routine constructs its Cholesky decomposition.
00125                   * On input, only the upper triangle of "IN" need be given; it is not modified.
00126                   *  The Cholesky factorization is returned in "out" in the upper triangle.
00127                   *  (by AJOGD @ JAN-2007)
00128                   * Redone for efficiency (Pablo Moreno, Jan 2010).
00129                   * \return True on success, false on singular (i.e. det=0) or not positive semidefinite matrix
00130                   * \exception logic_error On non-square matrix.
00131                   * \note Complexity O(N^3)
00132                   */
00133                 template<class MATRIX1,class MATRIX2>
00134                 bool chol(const MATRIX1 &in, MATRIX2 &out)
00135                 {
00136                         typedef typename MATRIX1::value_type T; // Elements type
00137                         const size_t N=in.getRowCount();
00138                         if (in.getColCount()!=in.getRowCount()) throw std::logic_error("Error in Cholesky factorization. Matrix is not square");
00139                         out.setSize(N,N);
00140                         {       //Special case of the main loop: i=0.
00141                                 T coef=in.get_unsafe(0,0);
00142                                 if (coef<=0) return false;
00143                                 else out.get_unsafe(0,0)=(coef=sqrt(coef));
00144                                 for (size_t j=1;j<N;++j) out.get_unsafe(0,j)=in.get_unsafe(0,j)/coef;
00145                         }
00146                         T sum;
00147                         for (size_t i=1;i<N;++i)        {
00148                                 out.set_unsafe(i,0,0);
00149                                 {       //Special case of the "j" loop: j=i.
00150                                         sum=in.get_unsafe(i,i);
00151                                         for (size_t k=i-1;k<N;--k) sum-=square(out.get_unsafe(k,i));
00152                                         if (sum<=0) return false;
00153                                         else out.get_unsafe(i,i)=sqrt(sum);
00154                                 }
00155                                 for (size_t j=i+1;j<N;++j)      {
00156                                         sum=in.get_unsafe(i,j);
00157                                         for (size_t k=i-1;k<N;--k) sum-=out.get_unsafe(k,i)*out.get_unsafe(k,j);
00158                                         out.get_unsafe(i,j)=sum/out.get_unsafe(i,i);
00159                                         out.get_unsafe(j,i)=0;
00160                                 }
00161                         }
00162                         return true;
00163                 } // end "chol"
00164 
00165                 /** Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classes themselves).
00166                         * \param theMatrix It can be a CMatrixTemplate or a CMatrixFixedNumeric.
00167                         * \param file The target filename.
00168                         * \param fileFormat See TMatrixTextFileFormat. The format of the numbers in the text file.
00169                         * \param appendMRPTHeader Insert this header to the file "% File generated by MRPT. Load with MATLAB with: VAR=load(FILENAME);"
00170                         * \param userHeader Additional text to be written at the head of the file. Typically MALAB comments "% This file blah blah". Final end-of-line is not needed.
00171                         * \sa loadFromTextFile, CMatrixTemplate::inMatlabFormat, SAVE_MATRIX
00172                         */
00173                 template <class MAT>
00174                 void saveMatrixToTextFile(
00175                         const MAT &theMatrix,
00176                         const std::string &file,
00177                         TMatrixTextFileFormat fileFormat,
00178                         bool    appendMRPTHeader,
00179                         const std::string &userHeader
00180                         )
00181                 {
00182                         using namespace mrpt::system;
00183 
00184                         MRPT_START;
00185 
00186                         FILE    *f=os::fopen(file.c_str(),"wt");
00187                         if (!f)
00188                                 THROW_EXCEPTION_CUSTOM_MSG1("saveToTextFile: Error opening file '%s' for writing a matrix as text.", file.c_str());
00189 
00190                         if (!userHeader.empty())
00191                                 fprintf(f,"%s",userHeader.c_str() );
00192 
00193                         if (appendMRPTHeader)
00194                                 fprintf(f,"%% File generated with MRPT %s at %s\n%%-----------------------------------------------------------------\n",
00195                                         mrpt::system::MRPT_getVersion().c_str(),
00196                                         mrpt::system::dateTimeLocalToString( mrpt::system::now() ).c_str() );
00197 
00198                         for (size_t i=0; i < theMatrix.getRowCount(); i++)
00199                         {
00200                                 for (size_t j=0; j < theMatrix.getColCount(); j++)
00201                                 {
00202                                         switch(fileFormat)
00203                                         {
00204                                         case MATRIX_FORMAT_ENG: os::fprintf(f,"%.16e",static_cast<double>(theMatrix(i,j))); break;
00205                                         case MATRIX_FORMAT_FIXED: os::fprintf(f,"%.16f",static_cast<double>(theMatrix(i,j))); break;
00206                                         case MATRIX_FORMAT_INT: os::fprintf(f,"%i",static_cast<int>(theMatrix(i,j))); break;
00207                                         default:
00208                                                 THROW_EXCEPTION("Unsupported value for the parameter 'fileFormat'!");
00209                                         };
00210                                         // Separating blank space
00211                                         if (j<(theMatrix.getColCount()-1)) os::fprintf(f," ");
00212                                 }
00213                                 os::fprintf(f,"\n");
00214                         }
00215                         os::fclose(f);
00216                         MRPT_END;
00217                 }
00218 
00219                 /** Dump matrix in matlab format.
00220                   *  This template method can be instantiated for matrices of the types: int, long, unsinged int, unsigned long, float, double, long double
00221                   */
00222                 template <class MATRIX>
00223                 std::string  matrix_inMatlabFormat(const MATRIX &m,const size_t decimal_digits)
00224                 {
00225                         std::stringstream  s;
00226                         s << "[" << std::scientific;
00227                         s.precision(decimal_digits);
00228                         for (size_t i=0;i<m.getRowCount();i++)
00229                         {
00230                                 for (size_t j=0;j<m.getColCount();j++)
00231                                         s << m.get_unsafe(i,j) << " ";
00232                                 if (i<m.getRowCount()-1) s << ";";
00233                         }
00234                         s << "]";
00235                         return s.str();
00236                 }
00237 
00238 
00239                 /** The trace of a matrix (the sum of its diagonal)
00240                   */
00241                 template<class MATRIX1>
00242                 typename MATRIX1::value_type trace(const MATRIX1 &m)
00243                 {
00244                         if (!m.IsSquare()) throw std::logic_error("Error in trace: matrix is not square");
00245                         const size_t N = size(m,1);
00246                         typename MATRIX1::value_type ret=0;
00247                         for (size_t i=0;i<N;i++)
00248                                 ret+=m.get_unsafe(i,i);
00249                         return ret;
00250                 }
00251 
00252                 /** Computes the vector v = A * a, where "a" is a column vector of the appropriate length. */
00253                 template<class MATRIX1, class OTHERVECTOR1,class OTHERVECTOR2>
00254                 void multiply_Ab(const MATRIX1 &m, const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput ) {
00255                         MRPT_START
00256                         size_t N=m.getRowCount();
00257                         size_t M=m.getColCount();
00258                         ASSERT_(vIn.size()==M)
00259                         vOut.resize(N);
00260                         if (!accumToOutput) for (size_t i=0;i<N;++i) vOut[i]=typename OTHERVECTOR2::value_type(0);
00261                         for (size_t i=0;i<N;++i)        {
00262                                 typename MATRIX1::value_type &accum=vOut[i];
00263                                 for (size_t j=0;j<M;++j) accum+=m.get_unsafe(i,j)*vIn[j];
00264                         }
00265                         MRPT_END
00266                 }
00267 
00268                 /** Computes the vector v = A<sup>T</sup> * a, where "a" is a column vector of the appropriate length. */
00269                 template<class MATRIX1, class OTHERVECTOR1,class OTHERVECTOR2>
00270                 void multiply_Atb(const MATRIX1 &m, const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput ) {
00271                         MRPT_START
00272                         size_t N=m.getColCount();
00273                         size_t M=m.getRowCount();
00274                         ASSERT_(vIn.size()==M)
00275                         vOut.resize(N);
00276                         if (!accumToOutput) for (size_t i=0;i<N;++i) vOut[i]=typename OTHERVECTOR2::value_type(0);
00277                         for (size_t i=0;i<N;++i)        {
00278                                 typename MATRIX1::value_type &accum=vOut[i];
00279                                 for (size_t j=0;j<M;++j) accum+=m.get_unsafe(j,i)*vIn[j];
00280                         }
00281                         MRPT_END
00282                 }
00283 
00284                 /** RESULT = A * A^t  */
00285                 template <class MATRIX1,class MATRIX2>
00286                 void multiply_AAt( const MATRIX1& m1, MATRIX2& RESULT )
00287                 {
00288                         typedef typename MATRIX2::value_type T;
00289                         const size_t M1R = m1.getRowCount();
00290                         const size_t M1C = m1.getColCount();
00291                         RESULT.setSize(M1R,M1R);
00292 
00293                         // If m1 is me, make a copy:
00294                         if ((void*)&m1==(void*)&RESULT)
00295                         {
00296                                 // Save result in a temporary matrix:
00297                                 std::vector<T>  temp(M1R*M1R); // Passed to vector() due to dynamic size...
00298 
00299                                 T  *ptr = &temp[0];
00300                                 size_t i;
00301                                 for (i=0; i < M1R; i++)
00302                                 {
00303                                         for (size_t j=i; j < M1R; j++)
00304                                         {
00305                                                 T accum = 0;
00306                                                 for (size_t k=0; k < M1C; k++)
00307                                                         accum += m1.get_unsafe(i,k) * m1.get_unsafe(j,k);
00308                                                 *(ptr++) = accum;
00309                                         }
00310                                 }
00311                                 // Copy from temp:
00312                                 ptr = &temp[0];
00313                                 for (i=0; i < M1R; i++)
00314                                         for (size_t j=i; j < M1R; j++)
00315                                                 RESULT.get_unsafe(i,j) = RESULT.get_unsafe(j,i) = *(ptr++);
00316                         }
00317                         else
00318                         {
00319                                 // Work directly over the data:
00320                                 for (size_t i=0; i < M1R; i++)
00321                                         for (size_t j=i; j < M1R; j++)
00322                                         {
00323                                                 T accum = 0;
00324                                                 for (size_t k=0; k < M1C; k++)
00325                                                         accum += m1.get_unsafe(i,k) * m1.get_unsafe(j,k);
00326                                                 RESULT.get_unsafe(i,j) = RESULT.get_unsafe(j,i) = accum;
00327                                         }
00328                         }
00329                 }
00330 
00331                 /** RESULT = A^t * A  */
00332                 template <class MATRIX1,class MATRIX2>
00333                 void multiply_AtA( const MATRIX1& m1, MATRIX2& RESULT )
00334                 {
00335                         typedef typename MATRIX2::value_type T;
00336                         const size_t M1R = m1.getRowCount();
00337                         const size_t M1C = m1.getColCount();
00338                         RESULT.setSize(M1C,M1C);
00339 
00340                         // If m1 is me, make a copy:
00341                         if ((void*)&m1==(void*)&RESULT)
00342                         {
00343                                 // Save result in a temporary matrix:
00344                                 std::vector<T>  temp(M1C*M1C); // Passed to vector() due to dynamic size...
00345 
00346                                 T  *ptr = &temp[0];
00347                                 size_t i;
00348                                 for (i=0; i < M1C; i++)
00349                                 {
00350                                         for (size_t j=i; j < M1C; j++)
00351                                         {
00352                                                 T accum = 0;
00353                                                 for (size_t k=0; k < M1R; k++)
00354                                                         accum += m1.get_unsafe(i,k) * m1.get_unsafe(j,k);
00355                                                 *(ptr++) = accum;
00356                                         }
00357                                 }
00358                                 // Copy from temp:
00359                                 ptr = &temp[0];
00360                                 for (i=0; i < M1C; i++)
00361                                         for (size_t j=i; j < M1C; j++)
00362                                                 RESULT.get_unsafe(i,j) = RESULT.get_unsafe(j,i) = *(ptr++);
00363                         }
00364                         else
00365                         {
00366                                 // Work directly over the data:
00367                                 for (size_t i=0; i < M1C; i++)
00368                                 {
00369                                         for (size_t j=i; j < M1C; j++)
00370                                         {
00371                                                 T accum = 0;
00372                                                 for (size_t k=0; k < M1R; k++)
00373                                                         accum += m1.get_unsafe(k,i) * m1.get_unsafe(k,j);
00374                                                 RESULT.get_unsafe(i,j) = RESULT.get_unsafe(j,i) = accum;
00375                                         }
00376                                 }
00377                         }
00378                 }
00379 
00380                 /** Multiply 2 matrices: RESULT = A * B  */
00381                 template <class MATRIX1,class MATRIX2,class MATRIXRES>
00382                 void multiply_AB(const MATRIX1& m1,const MATRIX2& m2, MATRIXRES& RESULT )
00383                 {
00384                         MRPT_START
00385 
00386                         typedef typename MATRIXRES::value_type T;
00387 
00388                         const size_t NROWS = m1.getRowCount();
00389                         const size_t NCOLS = m2.getColCount();
00390                         const size_t M1C   = m1.getColCount();
00391                         ASSERTMSG_(M1C==m2.getRowCount(),format("Invalid matrix sizes in multiplication: %ux%u * %ux%u",(unsigned int)NROWS,(unsigned int)M1C,(unsigned int)m2.getRowCount(),(unsigned int)NCOLS ));
00392 
00393                         RESULT.setSize(NROWS,NCOLS);
00394 
00395                         // If one of the matrices is me, make a copy:
00396                         if ( (void*)(&m1)==(void*)(&RESULT) || (void*)(&m2)==(void*)&RESULT)
00397                         {
00398                                 // Save result in a temporary matrix:
00399                                 std::vector<T> temp(NROWS*NCOLS);  // This cannot be a plain array due to unknown size at compile time for all kind of matrices...
00400                                 size_t out_idx = 0;
00401                                 for (size_t i=0; i < NROWS; i++)
00402                                 {
00403                                         for (size_t j=0; j < NCOLS; j++)
00404                                         {
00405                                                 T accum = 0;
00406                                                 for (size_t k=0; k < M1C; k++)
00407                                                         accum += m1.get_unsafe(i,k) * m2.get_unsafe(k,j);
00408                                                 temp[out_idx++] = accum;
00409                                         }
00410                                 }
00411 
00412                                 // Copy from temp:
00413                                 T* ptr = &temp[0];
00414                                 for (size_t i=0; i < NROWS; i++)
00415                                         for (size_t j=0; j < NCOLS; j++)
00416                                                 RESULT.get_unsafe(i,j)= *(ptr++);
00417                         }
00418                         else
00419                         {
00420                                 // Work directly over the data:
00421                                 for (size_t i=0; i < NROWS; i++)
00422                                 {
00423                                         for (size_t j=0; j < NCOLS; j++)
00424                                         {
00425                                                 T accum = 0;
00426                                                 for (size_t k=0; k < M1C; k++)
00427                                                         accum += m1.get_unsafe(i,k) * m2.get_unsafe(k,j);
00428                                                 RESULT.get_unsafe(i,j)=accum;
00429                                         }
00430                                 }
00431                         }
00432 
00433                         MRPT_END
00434                 } // end multiply
00435 
00436 
00437                 /** R = H * C * H^t (with C symmetric) */
00438                 template <typename MAT_H, typename MAT_C, typename MAT_R>
00439                 void multiply_HCHt(
00440                         const MAT_H &H,
00441                         const MAT_C &C,
00442                         MAT_R &R,
00443                         bool accumResultInOutput,
00444                         bool allow_submatrix_mult)
00445                 {
00446                         MRPT_START
00447 
00448                         ASSERTMSG_( (void*)&C != (void*)&H, "C and H must be different matrices." )
00449                         ASSERTMSG_( (void*)&R != (void*)&H, "R and H must be different matrices." )
00450                         ASSERTMSG_( (void*)&C != (void*)&R,  "C and R must be different matrices.")
00451                         ASSERT_(C.IsSquare())
00452 
00453                         if (allow_submatrix_mult)
00454                                  ASSERT_(C.getRowCount()>=H.getColCount())
00455                         else ASSERT_(C.getRowCount()==H.getColCount())
00456 
00457                         const size_t N=H.getRowCount();
00458                         const size_t M=H.getColCount();
00459 
00460                         R.setSize(N,N); // Set output size. For fixed size matrices this does nothing.
00461 
00462                         // Create R with the type of MAT_H * MAT_C
00463                         MAT_TYPE_PRODUCT_OF(MAT_H,MAT_C) R_;
00464                         R_.setSize(N,M);
00465 
00466                         // First compute R_ = H * C:
00467                         for (size_t i=0;i<N;i++)
00468                                 for (size_t j=0;j<M;j++)
00469                                 {
00470                                         typename MAT_H::value_type sumAccum = 0;
00471                                         for (size_t l=0;l<M;l++)
00472                                                 sumAccum += H.get_unsafe(i,l) * C.get_unsafe(l,j);
00473                                         R_.get_unsafe(i,j)  = sumAccum;
00474                                 }
00475 
00476                         // Now compute R = R_ * (H^t):
00477                         for (size_t i=0;i<N;i++)
00478                                 for (size_t j=i;j<N;j++)
00479                                 {
00480                                         typename MAT_H::value_type sumAccum = accumResultInOutput ? R.get_unsafe(i,j) : 0;
00481                                         for (size_t l=0;l<M;l++)
00482                                                 sumAccum += R_.get_unsafe(i,l) * H.get_unsafe(j,l);
00483                                         R.get_unsafe(i,j) = R.get_unsafe(j,i) = sumAccum;
00484                                 }
00485                         MRPT_END
00486                 }
00487 
00488                 /** r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C) */
00489                 template <typename VECTOR_H, typename MAT_C>
00490                 typename MAT_C::value_type
00491                 multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
00492                 {
00493                         MRPT_START
00494                         ASSERT_( C.IsSquare() )
00495 
00496                         const size_t M = H.size();
00497 
00498                         // This is 1xM matrix, C is a MxM matrix
00499                         ASSERT_( size(C,1)==M );
00500 
00501                         typename VECTOR_H::value_type sumAccum = 0;
00502                         typename VECTOR_H::const_iterator itL=H.begin();
00503                         for (size_t l=0;l<M; ++l, ++itL)
00504                         {
00505                                 typename VECTOR_H::value_type sumAccumInner = 0;
00506                                 typename VECTOR_H::const_iterator it;
00507                                 size_t k;
00508                                 for (k=0,it=H.begin();it!=H.end();++it,++k)
00509                                         sumAccumInner += *it * C.get_unsafe(k,l);
00510                                 sumAccum += sumAccumInner * (*itL);
00511                         }
00512                         return sumAccum;
00513                         MRPT_END
00514                 }
00515 
00516                 /** R = H^t * C * H  (with C symmetric) */
00517                 template <typename MAT_H, typename MAT_C, typename MAT_R>
00518                 void multiply_HtCH(
00519                         const MAT_H &H,
00520                         const MAT_C &C,
00521                         MAT_R &R,
00522                         bool accumResultInOutput,
00523                         bool allow_submatrix_mult)
00524                 {
00525                         MRPT_START
00526 
00527                         ASSERTMSG_( (void*)&C != (void*)&H, "C and H must be different matrices." )
00528                         ASSERTMSG_( (void*)&R != (void*)&H, "R and H must be different matrices." )
00529                         ASSERTMSG_( (void*)&C != (void*)&R,  "C and R must be different matrices.")
00530                         ASSERT_(C.IsSquare())
00531 
00532                         if (allow_submatrix_mult)
00533                                  ASSERT_(C.getRowCount()>=H.getRowCount())
00534                         else ASSERT_(C.getRowCount()==H.getRowCount())
00535 
00536                         R.setSize( H.getColCount(), H.getColCount()); // Set output size. For fixed size matrices this does nothing.
00537 
00538                         MAT_H R_;
00539 
00540                         const size_t M=H.getRowCount();
00541                         const size_t N=H.getColCount();
00542 
00543                         // First compute R_ = H * C:
00544                         for (size_t i=0;i<N;i++)
00545                                 for (size_t j=0;j<M;j++)
00546                                 {
00547                                         typename MAT_H::value_type sumAccum = 0;
00548                                         for (size_t l=0;l<M;l++)
00549                                                 sumAccum += H.get_unsafe(l,i) * C.get_unsafe(l,j);
00550                                         R_.get_unsafe(i,j)  = sumAccum;
00551                                 }
00552 
00553                         // Now compute R = R_ * (H^t):
00554                         for (size_t i=0;i<N;i++)
00555                                 for (size_t j=i;j<N;j++)
00556                                 {
00557                                         typename MAT_H::value_type sumAccum = accumResultInOutput ? R.get_unsafe(i,j) : 0;
00558                                         for (size_t l=0;l<M;l++)
00559                                                 sumAccum += R_.get_unsafe(i,l) * H.get_unsafe(l,j);
00560                                         R.get_unsafe(i,j) = R.get_unsafe(j,i) = sumAccum;
00561                                 }
00562                         MRPT_END
00563                 }
00564 
00565                 /** Matrix multiplication of this matrix with a submatrix of 'A', saving the result in a third matrix.
00566                   *   OUT = B * A
00567                   */
00568                 template <class MAT_X,class MAT_A,class MAT_OUT>
00569                 void multiply_subMatrix (
00570                         const MAT_X &X,
00571                         const MAT_A &A,
00572                         MAT_OUT &outResult,
00573                         const size_t A_cols_offset,
00574                         const size_t A_rows_offset,
00575                         const size_t A_col_count)
00576                 {
00577                         MRPT_START
00578                         // The output will be NxM:
00579                         const size_t  N = X.getRowCount();
00580                         const size_t  M = A_col_count;
00581                 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00582                         ASSERT_( A.getColCount() >= A_col_count + A_cols_offset );
00583                         ASSERT_( A.getRowCount() >= N + A_rows_offset );
00584                 #endif
00585                         outResult.setSize(N,M);
00586                         for (size_t i=0; i < N; i++)
00587                                 for (size_t j=0; j < M; j++)
00588                                 {
00589                                         typename MAT_OUT::value_type tmp = 0;
00590                                         for (size_t k=0; k < X.getColCount(); k++)
00591                                                 tmp += X.get_unsafe(i,k) * A.get_unsafe(k+A_rows_offset,j+A_cols_offset);
00592                                         outResult.get_unsafe(i,j) = tmp;
00593                                 }
00594                         MRPT_END
00595                 }
00596 
00597                 /**     RES = A*B*C   \sa multiply_ABCt */
00598                 template <class MAT_A,class MAT_B,class MAT_C,class MAT_OUT>
00599                 void multiply_ABC(const MAT_A &A, const MAT_B &B, const MAT_C &C, MAT_OUT & RES)
00600                 {
00601                         const size_t NROWS = A.getRowCount(); // A: NROWS x N1
00602                         const size_t N1    = B.getRowCount(); // B: N1 x N2
00603                         const size_t N2    = B.getColCount(); // C: NCOLS x N2
00604                         const size_t NCOLS = C.getColCount();
00605                         ASSERT_( A.getColCount()==B.getRowCount() && B.getColCount()==C.getRowCount() )
00606 
00607                         RES.zeros();
00608                         for (size_t i=0;i<NROWS;i++)
00609                                 for (size_t l=0;l<N2;l++)
00610                                 {
00611                                         typename MAT_OUT::value_type sumAccumInner = 0;
00612                                         for (size_t k=0;k<N1;k++)
00613                                                 sumAccumInner += A.get_unsafe(i,k) * B.get_unsafe(k,l);
00614                                         for (size_t j=0;j<NCOLS;j++)
00615                                                 RES.get_unsafe(i,j) += sumAccumInner * C.get_unsafe(l,j);
00616                                 }
00617                 }
00618 
00619                 /**     RES = A*B*(C^t)   \sa multiply_ABC */
00620                 template <class MAT_A,class MAT_B,class MAT_C,class MAT_OUT>
00621                 void multiply_ABCt(const MAT_A &A, const MAT_B &B, const MAT_C &C, MAT_OUT & RES)
00622                 {
00623                         const size_t NROWS = A.getRowCount(); // A: NROWS x N1
00624                         const size_t N1    = B.getRowCount(); // B: N1 x N2
00625                         const size_t N2    = B.getColCount(); // C: (NCOLS x N2)^t
00626                         const size_t NCOLS = C.getRowCount();
00627                         ASSERT_( A.getColCount()==B.getRowCount() && B.getColCount()==C.getColCount() )
00628 
00629                         RES.zeros();
00630                         for (size_t i=0;i<NROWS;i++)
00631                                 for (size_t l=0;l<N2;l++)
00632                                 {
00633                                         typename MAT_OUT::value_type sumAccumInner = 0;
00634                                         for (size_t k=0;k<N1;k++)
00635                                                 sumAccumInner += A.get_unsafe(i,k) * B.get_unsafe(k,l);
00636                                         for (size_t j=0;j<NCOLS;j++)
00637                                                 RES.get_unsafe(i,j) += sumAccumInner * C.get_unsafe(j,l);
00638                                 }
00639                 }
00640 
00641                 template <class MAT_A,class MAT_B,class MAT_OUT>
00642                 void multiply_ABt(const MAT_A &m1,const MAT_B &m2, MAT_OUT &out)
00643                 {
00644                         MRPT_START
00645                         typedef typename MAT_OUT::value_type T;
00646                         const size_t M1R = m1.getRowCount();
00647                         const size_t M1C = m1.getColCount();
00648                         const size_t M2C = m2.getRowCount();
00649 
00650                         ASSERTMSG_(m1.getColCount() == m2.getColCount(),"multiply_ABt: Inconsistent matrix sizes in multiplication!")
00651 
00652                         // If one of the matrices is OUT, make a copy:
00653                         if ((void*)&m1==(void*)&out|| (void*)&m2==(void*)&out)
00654                         {
00655                                 // Save result in a temporary matrix:
00656                                 std::vector<T> temp(M1R*M2C);
00657 
00658                                 T  *ptr = &temp[0];
00659                                 for (size_t i=0; i < M1R; i++)
00660                                         for (size_t j=0; j < M2C; j++)
00661                                         {
00662                                                 T accum = 0;
00663                                                 for (size_t k=0; k < M1C; k++)
00664                                                         accum += m1.get_unsafe(i,k) * m2.get_unsafe(j,k);
00665                                                 *(ptr++) = accum;
00666                                         }
00667                                 // Copy from temp:
00668                                 out.setSize(M1R,M2C);
00669                                 ptr = &temp[0];
00670                                 for (size_t i=0; i < M1R; i++)
00671                                         for (size_t j=0; j < M2C; j++)
00672                                                 out.set_unsafe(i,j,  *(ptr++) );
00673                         }
00674                         else
00675                         {
00676                                 // Work directly over the data:
00677                                 out.setSize( M1R,M2C );
00678                                 for (size_t i=0; i < M1R; i++)
00679                                         for (size_t j=0; j < M2C; j++)
00680                                         {
00681                                                 T accum = 0;
00682                                                 for (size_t k=0; k < M1C; k++)
00683                                                         accum += m1.get_unsafe(i,k) * m2.get_unsafe(j,k); //(k,j);
00684                                                 out.set_unsafe(i,j,accum);
00685                                         }
00686                         }
00687                         MRPT_END
00688                 }
00689 
00690                 template <class MAT_A,class MAT_B,class MAT_OUT>
00691                 void multiply_result_is_symmetric(const MAT_A &m1,const MAT_B &m2, MAT_OUT &out)
00692                 {
00693                         MRPT_START
00694                         typedef typename MAT_OUT::value_type T;
00695                         const size_t M1R = m1.getRowCount();
00696                         const size_t M1C = m1.getColCount();
00697                         const size_t M2C = m2.getColCount();
00698 
00699                         ASSERTMSG_(m1.getColCount() == m2.getRowCount(),"multiply_result_is_symmetric: Inconsistent matrix sizes in multiplication!")
00700 
00701                         // If one of the matrices is me, make a copy:
00702                         if ((void*)&m1==(void*)&out || (void*)&m2==(void*)&out)
00703                         {
00704                                 // Save result in a temporary matrix:
00705                                 std::vector<T> temp(M1R*M2C);
00706                                 T  *ptr = &temp[0];
00707                                 for (size_t i=0; i < M1R; i++)
00708                                         for (size_t j=i; j < M2C; j++)
00709                                         {
00710                                                 T accum = 0;
00711                                                 for (size_t k=0; k < M1C; k++)
00712                                                         accum += m1.get_unsafe(i,k) * m2.get_unsafe(k,j);
00713                                                 *(ptr++) = accum;
00714                                         }
00715 
00716                                 // Copy from temp:
00717                                 out.setSize(M1R,M2C);
00718                                 ptr = &temp[0];
00719                                 for (size_t i=0; i < M1R; i++)
00720                                         for (size_t j=i; j < M1C; j++)
00721                                                 out.get_unsafe(i,j) = out.get_unsafe(j,i) = *(ptr++);
00722                         }
00723                         else
00724                         {
00725                                 // Work directly over the data:
00726                                 out.setSize( M1R,M2C );
00727                                 for (size_t i=0; i < M1R; i++)
00728                                         for (size_t j=i; j < M2C; j++)
00729                                         {
00730                                                 T accum = 0;
00731                                                 for (size_t k=0; k < M1C; k++)
00732                                                         accum += m1.get_unsafe(i,k) * m2.get_unsafe(k,j);
00733                                                  out.set_unsafe(i,j,accum);
00734                                         }
00735                                 for (size_t i=0; i < M1R; i++)
00736                                         for (size_t j=0; j<i; j++)
00737                                                 out.get_unsafe(j,i) = out.get_unsafe(i,j);
00738                         }
00739                         MRPT_END
00740                 }
00741 
00742                 // Insert transposed matrix:
00743                 template <class MAT1,class MAT2>
00744                 void insertMatrixTransposeInto( MAT1 &M,const size_t nRow,const size_t nCol,const MAT2 &in)
00745                 {
00746                         ASSERTMSG_( (nRow+size(in,2) <= M.getRowCount() ) && (nCol+size(in,1)<= M.getColCount()), "insertMatrix: Row or Col index out of bounds")
00747                         for (size_t c=0;c<size(in,2);c++)
00748                                 for (size_t r=0;r<size(in,1);r++)
00749                                         M.get_unsafe(nRow+c,nCol+r) = in.get_unsafe(r,c);
00750                 }
00751 
00752                 // Insert matrix
00753                 template <class MAT1,class MAT2>
00754                 void insertMatrixInto( MAT1 &M,const size_t nRow,const size_t nCol,const MAT2 &in)
00755                 {
00756                         ASSERTMSG_( (nRow+size(in,1) <= M.getRowCount() ) && (nCol+size(in,2) <= M.getColCount()), "insertMatrix: Row or Col index out of bounds")
00757                         for (size_t r=0;r<size(in,1);r++)
00758                                 for (size_t c=0;c<size(in,2);c++)
00759                                         M.get_unsafe(nRow+r,nCol+c) = in.get_unsafe(r,c);
00760                 }
00761 
00762                 /** Extract a submatrix - The output matrix must be set to the required size before call. */
00763                 template <class MATORG, class MATDEST>
00764                 void extractMatrix(
00765                         const MATORG &M,
00766                         const size_t first_row,
00767                         const size_t first_col,
00768                         MATDEST &outMat)
00769                 {
00770                         const size_t NR = outMat.getRowCount();
00771                         const size_t NC = outMat.getColCount();
00772                         ASSERTMSG_( (first_row+NR <= M.getRowCount() ) && (first_col+NC <= M.getColCount()), "extractMatrix: Row or Col index out of bounds")
00773                         for (size_t r=0;r<NR;r++)
00774                                 for (size_t c=0;c<NC;c++)
00775                                         outMat.get_unsafe(r,c) = M.get_unsafe(first_row+r,first_col+c);
00776                 }
00777 
00778 
00779                 /** @name Matrix inverses - Implementation
00780                         @{ */
00781                 // templates for special cases:
00782                 template <class MATRIXIN,class MATRIXOUT>
00783                 inline void invMatrix_special_2x2( const MATRIXIN &M, MATRIXOUT &out_inv )
00784                 {
00785                         typedef typename MATRIXIN::value_type T;
00786                         // | a11 a12 |-1             |  a22 -a12 |
00787                         // | a21 a22 |    =  1/DET * | -a21  a11 |
00788                         //
00789                         const T det = M.det();
00790                         ASSERTMSG_(det!=0,"Singular matrix")
00791                         const T det_inv = 1.0 / det;
00792                         out_inv.setSize(2,2);
00793                         out_inv._E(1,1) =  M._E(2,2) * det_inv;
00794                         out_inv._E(1,2) = -M._E(1,2) * det_inv;
00795                         out_inv._E(2,1) = -M._E(2,1) * det_inv;
00796                         out_inv._E(2,2) =  M._E(1,1) * det_inv;
00797                 }
00798                 template <class MATRIXIN,class MATRIXOUT>
00799                 inline void invMatrix_special_3x3( const MATRIXIN &M, MATRIXOUT &out_inv )
00800                 {
00801                         typedef typename MATRIXIN::value_type T;
00802                         // | a11 a12 a13 |-1             |   a33a22-a32a23  -(a33a12-a32a13)   a23a12-a22a13  |
00803                         // | a21 a22 a23 |    =  1/DET * | -(a33a21-a31a23)   a33a11-a31a13  -(a23a11-a21a13) |
00804                         // | a31 a32 a33 |               |   a32a21-a31a22  -(a32a11-a31a12)   a22a11-a21a12  |
00805                         const T det = M.det();
00806                         ASSERTMSG_(det!=0,"Singular matrix")
00807                         const T det_inv = 1.0 / det;
00808                         out_inv.setSize(3,3);
00809                         out_inv._E(1,1) =  (M._E(3,3)*M._E(2,2)-M._E(3,2)*M._E(2,3) ) * det_inv;
00810                         out_inv._E(1,2) =  (-M._E(3,3)*M._E(1,2)+M._E(3,2)*M._E(1,3) )* det_inv;
00811                         out_inv._E(1,3) =  (M._E(2,3)*M._E(1,2)-M._E(2,2)*M._E(1,3) )* det_inv;
00812                         out_inv._E(2,1) =  (-M._E(3,3)*M._E(2,1)+M._E(3,1)*M._E(2,3))* det_inv;
00813                         out_inv._E(2,2) =  (M._E(3,3)*M._E(1,1)-M._E(3,1)*M._E(1,3))* det_inv;
00814                         out_inv._E(2,3) =  (-M._E(2,3)*M._E(1,1)+M._E(2,1)*M._E(1,3))* det_inv;
00815                         out_inv._E(3,1) =  (M._E(3,2)*M._E(2,1)-M._E(3,1)*M._E(2,2))* det_inv;
00816                         out_inv._E(3,2) =  (-M._E(3,2)*M._E(1,1)+M._E(3,1)*M._E(1,2))* det_inv;
00817                         out_inv._E(3,3) =  (M._E(2,2)*M._E(1,1)-M._E(2,1)*M._E(1,2))* det_inv;
00818                 }
00819 
00820                 // specializations that call the above special cases:
00821                 template <> inline void invMatrix<CMatrixFixedNumeric<float,2,2>,CMatrixFixedNumeric<float,2,2> >( const CMatrixFixedNumeric<float,2,2> &M, CMatrixFixedNumeric<float,2,2> &out_inv ) { invMatrix_special_2x2(M,out_inv);       }
00822                 template <> inline void invMatrix_destroySrc<CMatrixFixedNumeric<float,2,2>,CMatrixFixedNumeric<float,2,2> >( CMatrixFixedNumeric<float,2,2> &M, CMatrixFixedNumeric<float,2,2> &out_inv )  { invMatrix_special_2x2(M,out_inv); }
00823                 template <> inline void invMatrix<CMatrixFixedNumeric<double,2,2>,CMatrixFixedNumeric<double,2,2> >( const CMatrixFixedNumeric<double,2,2> &M, CMatrixFixedNumeric<double,2,2> &out_inv ) { invMatrix_special_2x2(M,out_inv);   }
00824                 template <> inline void invMatrix_destroySrc<CMatrixFixedNumeric<double,2,2>,CMatrixFixedNumeric<double,2,2> >( CMatrixFixedNumeric<double,2,2> &M, CMatrixFixedNumeric<double,2,2> &out_inv )  { invMatrix_special_2x2(M,out_inv);     }
00825                 template <> inline void invMatrix<CMatrixFixedNumeric<float,3,3>,CMatrixFixedNumeric<float,3,3> >( const CMatrixFixedNumeric<float,3,3> &M, CMatrixFixedNumeric<float,3,3> &out_inv ) { invMatrix_special_3x3(M,out_inv);       }
00826                 template <> inline void invMatrix_destroySrc<CMatrixFixedNumeric<float,3,3>,CMatrixFixedNumeric<float,3,3> >( CMatrixFixedNumeric<float,3,3> &M, CMatrixFixedNumeric<float,3,3> &out_inv )  { invMatrix_special_3x3(M,out_inv); }
00827                 template <> inline void invMatrix<CMatrixFixedNumeric<double,3,3>,CMatrixFixedNumeric<double,3,3> >( const CMatrixFixedNumeric<double,3,3> &M, CMatrixFixedNumeric<double,3,3> &out_inv ) { invMatrix_special_3x3(M,out_inv);   }
00828                 template <> inline void invMatrix_destroySrc<CMatrixFixedNumeric<double,3,3>,CMatrixFixedNumeric<double,3,3> >( CMatrixFixedNumeric<double,3,3> &M, CMatrixFixedNumeric<double,3,3> &out_inv )  { invMatrix_special_3x3(M,out_inv);     }
00829 
00830                 template <class MATRIXIN,class MATRIXOUT>
00831                 inline void  invMatrix( const MATRIXIN &M, MATRIXOUT &out_inv )
00832                 {
00833                         MAT_TYPE_SAMESIZE_OF(MATRIXIN) temp;
00834                         temp.assignMatrix(M);
00835                         invMatrix_destroySrc(temp,out_inv);  // temp is destroyed in inv_fast
00836                 }
00837 
00838                 template <class MATRIXIN,class MATRIXOUT>
00839                 inline void  invMatrix_destroySrc( MATRIXIN &M, MATRIXOUT &out_inv )
00840                 {
00841 #if 0
00842                         out_inv.setIdentity(M.getRowCount());
00843                         mrpt::math::detail::fastLeftDivideSquare(out_inv,M);
00844                         return;
00845 #else
00846                         typedef typename MATRIXIN::value_type T;
00847                         ASSERTMSG_(M.IsSquare(),"Inversion of non-square matrix")
00848                         // Check this here for dynamic-size matrices:
00849                         const size_t NROWS = M.getRowCount();
00850                         if (NROWS==3) return invMatrix_special_3x3(M,out_inv);
00851                         else if (NROWS==2) return invMatrix_special_2x2(M,out_inv);
00852 
00853                         // Generic algorithm for any-size matrices:
00854                         T a1,a2;
00855                         out_inv.setSize(NROWS,NROWS);
00856                         out_inv.unit();
00857                         for (size_t k=0; k < NROWS; k++)
00858                         {
00859                                 int indx = M.pivot(k);
00860                                 if (indx == -1)
00861                                         THROW_EXCEPTION( "Inversion of a singular matrix");
00862 
00863                                 if (indx != 0)
00864                                         out_inv.swapRows(k,indx);       // Swap rows in out_inv - they have been ALREADY SWAPPED in M within pivot()
00865 
00866                                 a1 = M.get_unsafe(k,k);
00867                                 ASSERTDEB_(a1!=0)
00868                                 const T a1_i = 1/a1;
00869                                 for (size_t j=0; j < NROWS; j++)
00870                                 {
00871                                         M.get_unsafe(k,j) *= a1_i;
00872                                         out_inv.get_unsafe(k,j) *= a1_i;
00873                                 }
00874                                 for (size_t i=0; i < NROWS; i++)
00875                                 {
00876                                         if (i != k)
00877                                         {
00878                                                 a2 = M.get_unsafe(i,k);
00879                                                 for (size_t j=0; j < NROWS; j++)
00880                                                 {
00881                                                         M.get_unsafe(i,j)  -= a2 * M.get_unsafe(k,j);
00882                                                         out_inv.get_unsafe(i,j) -= a2 * out_inv.get_unsafe(k,j);
00883                                                 }
00884                                         }
00885                                 }
00886                         }
00887 #endif
00888                 }
00889 
00890                 /** @} */  // END OF MATRIX INVERSES
00891 
00892 
00893                 /** @name Matrix determinants - implementation
00894                    @{ */
00895                 // Special cases:
00896                 template <class MATRIX> inline typename MATRIX::value_type
00897                 detMatrix_special_2x2(const MATRIX &M)
00898                 {
00899                         return M._E(1,1)*M._E(2,2) - M._E(1,2)*M._E(2,1);
00900                 }
00901                 template <class MATRIX> inline typename MATRIX::value_type
00902                 detMatrix_special_3x3(const MATRIX &M)
00903                 {
00904                         return M._E(1,1)*(M._E(3,3)*M._E(2,2)-M._E(3,2)*M._E(2,3))-
00905                                 M._E(2,1)*(M._E(3,3)*M._E(1,2)-M._E(3,2)*M._E(1,3))+
00906                                 M._E(3,1)*(M._E(2,3)*M._E(1,2)-M._E(2,2)*M._E(1,3));
00907                 }
00908                 template <class MATRIX> typename MATRIX::value_type
00909                 detMatrix_special_4x4(const MATRIX &M)
00910                 {
00911                         typedef typename MATRIX::value_type T;
00912                         const T D1 = M._E(1+1,1+1)*(M._E(3+1,3+1)*M._E(2+1,2+1)-M._E(3+1,2+1)*M._E(2+1,3+1))-
00913                                      M._E(2+1,1+1)*(M._E(3+1,3+1)*M._E(1+1,2+1)-M._E(3+1,2+1)*M._E(1+1,3+1))+
00914                                      M._E(3+1,1+1)*(M._E(2+1,3+1)*M._E(1+1,2+1)-M._E(2+1,2+1)*M._E(1+1,3+1));
00915                         const T D2 = M._E(1+1,1)*(M._E(3+1,3+1)*M._E(2+1,2+1)-M._E(3+1,2+1)*M._E(2+1,3+1))-
00916                                      M._E(2+1,1)*(M._E(3+1,3+1)*M._E(1+1,2+1)-M._E(3+1,2+1)*M._E(1+1,3+1))+
00917                                      M._E(3+1,1)*(M._E(2+1,3+1)*M._E(1+1,2+1)-M._E(2+1,2+1)*M._E(1+1,3+1));
00918                         const T D3 = M._E(1+1,1)*(M._E(3+1,3+1)*M._E(2+1,2)-M._E(3+1,2)*M._E(2+1,3+1))-
00919                                      M._E(2+1,1)*(M._E(3+1,3+1)*M._E(1+1,2)-M._E(3+1,2)*M._E(1+1,3+1))+
00920                                      M._E(3+1,1)*(M._E(2+1,3+1)*M._E(1+1,2)-M._E(2+1,2)*M._E(1+1,3+1));
00921                         const T D4 = M._E(1+1,1)*(M._E(3+1,3)*M._E(2+1,2)-M._E(3+1,2)*M._E(2+1,3))-
00922                                      M._E(2+1,1)*(M._E(3+1,3)*M._E(1+1,2)-M._E(3+1,2)*M._E(1+1,3))+
00923                                      M._E(3+1,1)*(M._E(2+1,3)*M._E(1+1,2)-M._E(2+1,2)*M._E(1+1,3));
00924                         return M._E(1,1)*D1 - M._E(1,2)*D2 + M._E(1,3)*D3 - M._E(1,4)*D4;
00925                 }
00926 
00927                 // Specializations that call the above functions:
00928                 template <> inline float  detMatrix<CMatrixFixedNumeric<float,2,2> >(const CMatrixFixedNumeric<float,2,2> &M) { return detMatrix_special_2x2(M); }
00929                 template <> inline double detMatrix<CMatrixFixedNumeric<double,2,2> >(const CMatrixFixedNumeric<double,2,2> &M)  { return detMatrix_special_2x2(M); }
00930                 template <> inline float  detMatrix<CMatrixFixedNumeric<float,3,3> >(const CMatrixFixedNumeric<float,3,3> &M)  { return detMatrix_special_3x3(M); }
00931                 template <> inline double detMatrix<CMatrixFixedNumeric<double,3,3> >(const CMatrixFixedNumeric<double,3,3> &M) { return detMatrix_special_3x3(M); }
00932                 template <> inline float  detMatrix<CMatrixFixedNumeric<float,4,4> >(const CMatrixFixedNumeric<float,4,4> &M)  { return detMatrix_special_4x4(M); }
00933                 template <> inline double detMatrix<CMatrixFixedNumeric<double,4,4> >(const CMatrixFixedNumeric<double,4,4> &M) { return detMatrix_special_4x4(M); }
00934 
00935                 template <class MATRIX> RET_ELEMENT_ASSERT_MRPTCONTAINER(MATRIX)
00936                 detMatrix(const MATRIX& M)
00937                 {
00938                         typedef typename MATRIX::value_type T;
00939                         ASSERTMSG_(M.IsSquare(),"Inversion of non-square matrix")
00940                         // Check this here for dynamic-size matrices:
00941                         const size_t NROWS = M.getRowCount();
00942                         if (NROWS==3) return detMatrix_special_3x3(M);
00943                         else if (NROWS==2) return detMatrix_special_2x2(M);
00944                         // general case:
00945                         MAT_TYPE_SAMESIZE_OF(MATRIX) temp;
00946                         temp.assignMatrix(M);
00947                         T detVal = T(1);
00948                         for (size_t k=0; k < NROWS; k++)
00949                         {
00950                                 int     indx = temp.pivot(k);
00951                                 if (indx == -1) return 0;
00952                                 if (indx != 0)  detVal = - detVal;
00953                                 const T temp_kk = temp.get_unsafe(k,k);
00954                                 const T temp_kk_inv = T(1)/temp_kk;
00955                                 detVal = detVal * temp_kk;
00956                                 for (size_t i=k+1; i < NROWS; i++)
00957                                 {
00958                                         T piv = temp.get_unsafe(i,k)*temp_kk_inv; // was: / temp.get_unsafe(k,k);
00959                                         for (size_t j=k+1; j < NROWS; j++)
00960                                                 temp.get_unsafe(i,j) -= piv * temp.get_unsafe(k,j);
00961                                 }
00962                         }
00963                         return detVal;
00964                 }
00965 
00966                 /** @} */  // END OF MATRIX DETERMINANTS
00967 
00968                 /** Matrix pivoting - used for detMatrix, etc. */
00969                 template <class MATRIX>
00970                 int matrix_pivot(MATRIX &M, const size_t row)
00971                 {
00972                         typedef typename MATRIX::value_type T;
00973                         size_t k = row;
00974                         const size_t NROWS = M.getRowCount();
00975                         T temp, amax = -1;
00976                         for (size_t i=row; i < NROWS; i++)
00977                                 if ( (temp = std::abs( M.get_unsafe(i,row))) > amax && temp != 0)
00978                                 {
00979                                         amax = temp;
00980                                         k = i;
00981                                 }
00982                         if (M.get_unsafe(k,row) == T(0)) return -1; // No one found.
00983                         if (k != row)
00984                         {       // Swap rows "k" & "row":
00985                                 M.swapRows(k,row);
00986                                 return static_cast<int>( k );
00987                         }
00988                         return 0;
00989                 }
00990 
00991                 template <class MAT,class VEC>
00992                 void  extractRowFromMatrix(const MAT &m, size_t nRow, VEC &out, const size_t startingCol = 0)
00993                 {
00994         #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00995                         if (nRow>=size(m,1))
00996                                 THROW_EXCEPTION("extractRow: Row index out of bounds");
00997         #endif
00998                         const size_t n = size(m,2) - startingCol ;
00999                         out.resize( n );
01000                         for (size_t i=0;i<n;i++)
01001                                 out[i] = static_cast<typename MAT::value_type>( m.get_unsafe(nRow,i+startingCol));
01002                 }
01003 
01004                 template <class MAT,class VEC>
01005                 void  extractColFromMatrix(const MAT &m, size_t nCol, VEC &out, const size_t startingRow = 0)
01006                 {
01007         #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
01008                         if (nCol>=size(m,2))
01009                                 THROW_EXCEPTION("extractCol: Col index out of bounds");
01010         #endif
01011                         const size_t n = size(m,1) - startingRow;
01012                         out.resize( n );
01013                         for (size_t i=0;i<n;i++)
01014                                 out[i] = static_cast<typename MAT::value_type>( m.get_unsafe(i+startingRow,nCol));
01015                 }
01016 
01017                 template <class MAT,class VEC>
01018                 void  insertRowToMatrix(MAT &m, size_t nRow, const VEC &in, const size_t startingCol=0)
01019                 {
01020         #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
01021                         if (nRow>=size(m,1))
01022                                 THROW_EXCEPTION("insertRow: Row index out of bounds");
01023         #endif
01024                         ASSERT_( (size(m,2)-startingCol)==in.size() )
01025                         const size_t n = size(m,2) - startingCol ;
01026                         for (size_t i=0;i<n;i++)
01027                                 m.set_unsafe(nRow,i+startingCol, static_cast<typename MAT::value_type>(in[i]) );
01028                 }
01029 
01030                 template <class MAT,class VEC>
01031                 void  insertColToMatrix(MAT &m, size_t nCol, const VEC &in, const size_t startingRow=0)
01032                 {
01033         #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
01034                         if (nCol>=size(m,2))
01035                                 THROW_EXCEPTION("insertCol: Col index out of bounds");
01036         #endif
01037                         ASSERT_( (size(m,1)-startingRow)==in.size() )
01038                         const size_t n = size(m,1) - startingRow;
01039                         for (size_t i=0;i<n;i++)
01040                                 m.set_unsafe(i+startingRow,nCol, static_cast<typename MAT::value_type>(in[i]));
01041                 }
01042 
01043 
01044         } // end detail
01045 
01046         /** Just like s=H.multiply_HCHt_scalar(C), but defined in mrpt::math for backward compatibility. */
01047         template <class MAT1,class MAT2>
01048         inline typename MAT1::value_type multiply_HCHt_scalar(const MAT1 &H, const MAT2 &C) {
01049                 return detail::multiply_HCHt_scalar(H,C);
01050         }
01051 
01052         /** Binary matrix division operator A/B = A*inv(B) */
01053         template <class T>
01054         inline CMatrixTemplateNumeric<T>  operator / (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
01055         {       // JL: This is hard to generalize for any mix of dyn. fix. matrices, but I don't think it's worth in any case...
01056                 return (m1 * !m2);
01057         }
01058 
01059         /** binary power operator */
01060         template <class T>
01061         inline CMatrixTemplateNumeric<T>  operator ^ (const CMatrixTemplateNumeric<T>& m, const unsigned int pow)
01062         {
01063                 CMatrixTemplateNumeric<T>       temp(m);
01064                 temp ^= pow;
01065                 return temp;
01066         }
01067 
01068         /** unary transpose operator ~ */
01069         template <class MAT>
01070         inline MAT_TYPE_TRANSPOSE_OF(MAT)
01071                 operator ~ (const MAT& m)
01072         {
01073                 const size_t R=m.getRowCount();
01074                 const size_t C=m.getColCount();
01075                 MAT_TYPE_TRANSPOSE_OF(MAT) temp; temp.setSize(C,R); // transpose
01076                 for (size_t i=0;i<R;i++)
01077                         for (size_t j=0;j<C;j++)
01078                                 temp.get_unsafe(j,i) = m.get_unsafe(i,j);
01079                 return temp;
01080         }
01081 
01082         /** Unary inversion operator. */
01083         template <class MATRIX>
01084         inline RET_MAT_ASSERT_MRPTMATRIX(MATRIX)
01085         operator !(const MATRIX &m) {
01086                 RET_MAT_ASSERT_MRPTMATRIX(MATRIX) ret(UNINITIALIZED_MATRIX);
01087                 m.inv(ret);
01088                 return ret;
01089         }
01090 
01091 
01092         // Operator * uses MAT_TYPE_PRODUCT_OF to manage these four cases:
01093         //  FIX * FIX -> FIX (with the correct sizes)
01094         //  DYN * FIX -> DYN
01095         //  FIX * DYN -> DYN
01096         //  DYN * DYN -> DYN
01097         // --------------------------------------------------
01098         /** Matrix multiplication operator: A * B -> RES
01099           *  The meaning of the lengthy macros in the declaration is:
01100           *   - MAT_TYPE_PRODUCT_OF: Return type is the correct one for A*B, e.g. NxM * MxK -> NxK,  DYN*DYN -> DYN.
01101           *   - RET_MAT_ASSERT_MRPTMATRIX: Assure that the compiler will use this "operator*" only with MRPT matrices.
01102           */
01103         template <class MAT1,class MAT2>
01104         inline MAT_TYPE_PRODUCT_OF(MAT1,MAT2)
01105         operator * ( const MAT1 &A,
01106                      const MAT2 &B  )
01107         {
01108                 MAT_TYPE_PRODUCT_OF(MAT1,MAT2)  RES(UNINITIALIZED_MATRIX);
01109                 detail::multiply_AB(A,B, RES);
01110                 return RES;
01111         }
01112 
01113         /** Computes the mean vector and covariance from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.
01114           * \param v The set of data as a NxM matrix, of types: CMatrixTemplateNumeric or CMatrixFixedNumeric
01115           * \param out_mean The output M-vector for the estimated mean.
01116           * \param out_cov The output MxM matrix for the estimated covariance matrix, this can also be either a fixed-size of dynamic size matrix.
01117           * \sa math::mean,math::stddev, math::cov
01118           */
01119         template<class MAT_IN, class MAT_OUT>
01120         void meanAndCov(
01121                 const MAT_IN &v,
01122                 vector_double   &out_mean,
01123                 MAT_OUT                 &out_cov
01124                 )
01125         {
01126                 const size_t N = v.getRowCount();
01127                 ASSERTMSG_(N>0,"The input matrix contains no elements");
01128                 const double N_inv = 1.0/N;
01129 
01130                 const size_t M = v.getColCount();
01131                 ASSERTMSG_(M>0,"The input matrix contains rows of length 0");
01132 
01133                 // First: Compute the mean
01134                 out_mean.assign(M,0);
01135                 for (size_t i=0;i<N;i++)
01136                         for (size_t j=0;j<M;j++)
01137                                 out_mean[j]+=v.get_unsafe(i,j);
01138                 out_mean*=N_inv;
01139 
01140                 // Second: Compute the covariance
01141                 //  Save only the above-diagonal part, then after averaging
01142                 //  duplicate that part to the other half.
01143                 out_cov.zeros(M,M);
01144                 for (size_t i=0;i<N;i++)
01145                 {
01146                         for (size_t j=0;j<M;j++)
01147                                 out_cov.get_unsafe(j,j)+=square(v.get_unsafe(i,j)-out_mean[j]);
01148 
01149                         for (size_t j=0;j<M;j++)
01150                                 for (size_t k=j+1;k<M;k++)
01151                                         out_cov.get_unsafe(j,k)+=(v.get_unsafe(i,j)-out_mean[j])*(v.get_unsafe(i,k)-out_mean[k]);
01152                 }
01153                 for (size_t j=0;j<M;j++)
01154                         for (size_t k=j+1;k<M;k++)
01155                                 out_cov.get_unsafe(k,j) = out_cov.get_unsafe(j,k);
01156                 out_cov*=N_inv;
01157         }
01158 
01159         /** Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.
01160           * \param v The set of data, as a NxM matrix.
01161           * \param out_cov The output MxM matrix for the estimated covariance matrix.
01162           * \sa math::mean,math::stddev, math::cov
01163           */
01164         template<class MATRIX>
01165         inline MAT_TYPE_COVARIANCE_OF(MATRIX) cov( const MATRIX &v )
01166         {
01167                 vector_double m;
01168                 MAT_TYPE_COVARIANCE_OF(MATRIX) C;
01169                 meanAndCov(v,m,C);
01170                 return C;
01171         }
01172 
01173         /** A useful macro for saving matrixes to a file while debugging. */
01174         #define SAVE_MATRIX(M) \
01175                 M.saveToTextFile(mrpt::format("%s.txt",#M));
01176 
01177 
01178         // ------ Implementatin of Vicinity templates -------------
01179         namespace detail        {
01180                 /**
01181                   * Vicinity traits class specialization for matrices.
01182                   */
01183                 template<typename T> class VicinityTraits<CMatrixTemplate<T> >  {
01184                 public:
01185                         inline static void initialize(CMatrixTemplate<T> &mat,size_t N) {
01186                                 mat.setSize(N,N,true);
01187                         }
01188                         inline static void insertInContainer(CMatrixTemplate<T> &mat,size_t r,size_t c,const T &t)      {
01189                                 mat.get_unsafe(r,c)=t;
01190                         }
01191                 };
01192                 /**
01193                   * Vicinity traits class specialization for vectors. It ignores the spatial distribution, inserting elements at the end of the vector.
01194                   */
01195                 template<typename T> class VicinityTraits<std::vector<T> >      {
01196                 public:
01197                         inline static void initialize(std::vector<T> &vec,size_t N)     {
01198                                 vec.reserve(N*N);
01199                         }
01200                         inline static void insertInContainer(std::vector<T> &vec,size_t,size_t,const T &t)      {
01201                                 vec.push_back(t);
01202                         }
01203                 };
01204                 /**
01205                   * Template specialization for getVicinity.
01206                   *
01207                   * ( _ * _ )
01208                   * ( * _ * )
01209                   * ( _ * _ )
01210                   *
01211                   */
01212                 template<typename MatrixType,typename T,typename ReturnType> struct getVicinity<MatrixType,T,ReturnType,4>      {
01213                 public:
01214                         static ReturnType get(size_t r,size_t c,const MatrixType &mat)  {
01215                                 mat.ASSERT_ENOUGHROOM<1>(r,c);
01216                                 ReturnType res;
01217                                 VicinityTraits<ReturnType>::initialize(res,3);
01218                                 VicinityTraits<ReturnType>::insertInContainer(res,0,1,mat.get_unsafe(r-1,c));
01219                                 VicinityTraits<ReturnType>::insertInContainer(res,1,0,mat.get_unsafe(r,c-1));
01220                                 VicinityTraits<ReturnType>::insertInContainer(res,1,2,mat.get_unsafe(r,c+1));
01221                                 VicinityTraits<ReturnType>::insertInContainer(res,2,1,mat.get_unsafe(r+1,c));
01222                                 return res;
01223                         }
01224                 };
01225                 /**
01226                   * Template specialization for getVicinity.
01227                   *
01228                   * ( _ * _ )
01229                   * ( * * * )
01230                   * ( _ * _ )
01231                   *
01232                   */
01233                 template<typename MatrixType,typename T,typename ReturnType> struct getVicinity<MatrixType,T,ReturnType,5>      {
01234                 public:
01235                         static ReturnType get(size_t r,size_t c,const MatrixType &mat)  {
01236                                 mat.ASSERT_ENOUGHROOM<1>(r,c);
01237                                 ReturnType res;
01238                                 VicinityTraits<ReturnType>::initialize(res,3);
01239                                 VicinityTraits<ReturnType>::insertInContainer(res,0,1,mat.get_unsafe(r-1,c));
01240                                 for (int i=-1;i<=1;++i) VicinityTraits<ReturnType>::insertInContainer(res,1,i+1,mat.get_unsafe(r,c+i));
01241                                 VicinityTraits<ReturnType>::insertInContainer(res,2,1,mat.get_unsafe(r+1,c));
01242                                 return res;
01243                         }
01244                 };
01245                 /**
01246                   * Template specialization for getVicinity.
01247                   *
01248                   * ( * * * )
01249                   * ( * _ * )
01250                   * ( * * * )
01251                   *
01252                   */
01253                 template<typename MatrixType,typename T,typename ReturnType> struct getVicinity<MatrixType,T,ReturnType,8>      {
01254                 public:
01255                         static ReturnType get(size_t r,size_t c,const MatrixType &mat)  {
01256                                 mat.ASSERT_ENOUGHROOM<1>(r,c);
01257                                 ReturnType res;
01258                                 VicinityTraits<ReturnType>::initialize(res,3);
01259                                 for (int i=-1;i<=1;++i) for (int j=-1;j<=1;++j) if (i||j) VicinityTraits<ReturnType>::insertInContainer(res,i+1,j+1,mat.get_unsafe(r+i,c+j));
01260                                 return res;
01261                         }
01262                 };
01263                 /**
01264                   * Template specialization for getVicinity.
01265                   *
01266                   * ( * * * )
01267                   * ( * * * )
01268                   * ( * * * )
01269                   *
01270                   */
01271                 template<typename MatrixType,typename T,typename ReturnType> struct getVicinity<MatrixType,T,ReturnType,9>      {
01272                 public:
01273                         static ReturnType get(size_t r,size_t c,const MatrixType &mat)  {
01274                                 mat.ASSERT_ENOUGHROOM<1>(r,c);
01275                                 ReturnType res;
01276                                 VicinityTraits<ReturnType>::initialize(res,3);
01277                                 for (int i=-1;i<=1;++i) for (int j=-1;j<=1;++j) VicinityTraits<ReturnType>::insertInContainer(res,i+1,j+1,mat.get_unsafe(r+i,c+j));
01278                                 return res;
01279                         }
01280                 };
01281                 /**
01282                   * Template specialization for getVicinity.
01283                   *
01284                   * ( _ _ * _ _ )
01285                   * ( _ * * * _ )
01286                   * ( * * _ * * )
01287                   * ( _ * * * _ )
01288                   * ( _ _ * _ _ )
01289                   *
01290                   */
01291                 template<typename MatrixType,typename T,typename ReturnType> struct getVicinity<MatrixType,T,ReturnType,12>     {
01292                 public:
01293                         static ReturnType get(size_t r,size_t c,const MatrixType &mat)  {
01294                                 mat.ASSERT_ENOUGHROOM<2>(r,c);
01295                                 ReturnType res;
01296                                 VicinityTraits<ReturnType>::initialize(res,5);
01297                                 VicinityTraits<ReturnType>::insertInContainer(res,0,2,mat.get_unsafe(r-2,c));
01298                                 for (int i=-1;i<=1;++i) VicinityTraits<ReturnType>::insertInContainer(res,1,i+2,mat.get_unsafe(r-1,c+i));
01299                                 for (int i=-2;i<=2;++i) if (i) VicinityTraits<ReturnType>::insertInContainer(res,2,i+2,mat.get_unsafe(r,c+i));
01300                                 for (int i=-1;i<=1;++i) VicinityTraits<ReturnType>::insertInContainer(res,3,i+2,mat.get_unsafe(r+1,c+i));
01301                                 VicinityTraits<ReturnType>::insertInContainer(res,4,2,mat.get_unsafe(r+2,c));
01302                                 return res;
01303                         }
01304                 };
01305                 /**
01306                   * Template specialization for getVicinity.
01307                   *
01308                   * ( _ _ * _ _ )
01309                   * ( _ * * * _ )
01310                   * ( * * * * * )
01311                   * ( _ * * * _ )
01312                   * ( _ _ * _ _ )
01313                   *
01314                   */
01315                 template<typename MatrixType,typename T,typename ReturnType> struct getVicinity<MatrixType,T,ReturnType,13>     {
01316                 public:
01317                         static ReturnType get(size_t r,size_t c,const MatrixType &mat)  {
01318                                 mat.ASSERT_ENOUGHROOM<2>(r,c);
01319                                 ReturnType res;
01320                                 VicinityTraits<ReturnType>::initialize(res,5);
01321                                 VicinityTraits<ReturnType>::insertInContainer(res,0,2,mat.get_unsafe(r-2,c));
01322                                 for (int i=-1;i<=1;++i) VicinityTraits<ReturnType>::insertInContainer(res,1,i+2,mat.get_unsafe(r-1,c+i));
01323                                 for (int i=-2;i<=2;++i) VicinityTraits<ReturnType>::insertInContainer(res,2,i+2,mat.get_unsafe(r,c+i));
01324                                 for (int i=-1;i<=1;++i) VicinityTraits<ReturnType>::insertInContainer(res,3,i+2,mat.get_unsafe(r+1,c+i));
01325                                 VicinityTraits<ReturnType>::insertInContainer(res,4,2,mat.get_unsafe(r+2,c));
01326                                 return res;
01327                         }
01328                 };
01329                 /**
01330                   * Template specialization for getVicinity.
01331                   *
01332                   * ( _ * * * _ )
01333                   * ( * * * * * )
01334                   * ( * * _ * * )
01335                   * ( * * * * * )
01336                   * ( _ * * * _ )
01337                   *
01338                   */
01339                 template<typename MatrixType,typename T,typename ReturnType> struct getVicinity<MatrixType,T,ReturnType,20>     {
01340                 public:
01341                         static ReturnType get(size_t r,size_t c,const MatrixType &mat)  {
01342                                 mat.ASSERT_ENOUGHROOM<2>(r,c);
01343                                 ReturnType res;
01344                                 VicinityTraits<ReturnType>::initialize(res,5);
01345                                 for (int i=-1;i<=1;++i) VicinityTraits<ReturnType>::insertInContainer(res,0,i+2,mat.get_unsafe(r-2,c+i));
01346                                 for (int i=-2;i<=2;++i) VicinityTraits<ReturnType>::insertInContainer(res,1,i+2,mat.get_unsafe(r-1,c+i));
01347                                 for (int i=-2;i<=2;++i) if (i) VicinityTraits<ReturnType>::insertInContainer(res,2,i+2,mat.get_unsafe(r,c+i));
01348                                 for (int i=-2;i<=2;++i) VicinityTraits<ReturnType>::insertInContainer(res,3,i+2,mat.get_unsafe(r+1,c+i));
01349                                 for (int i=-1;i<=1;++i) VicinityTraits<ReturnType>::insertInContainer(res,4,i+2,mat.get_unsafe(r+2,c+i));
01350                                 return res;
01351                         }
01352                 };
01353                 /**
01354                   * Template specialization for getVicinity.
01355                   *
01356                   * ( _ * * * _ )
01357                   * ( * * * * * )
01358                   * ( * * * * * )
01359                   * ( * * * * * )
01360                   * ( _ * * * _ )
01361                   *
01362                   */
01363                 template<typename MatrixType,typename T,typename ReturnType> struct getVicinity<MatrixType,T,ReturnType,21>     {
01364                 public:
01365                         static ReturnType get(size_t r,size_t c,const MatrixType &mat)  {
01366                                 mat.ASSERT_ENOUGHROOM<2>(r,c);
01367                                 ReturnType res;
01368                                 VicinityTraits<ReturnType>::initialize(res,5);
01369                                 for (int i=-1;i<=1;++i) VicinityTraits<ReturnType>::insertInContainer(res,0,i+2,mat.get_unsafe(r-2,c+i));
01370                                 for (int i=-2;i<=2;++i) VicinityTraits<ReturnType>::insertInContainer(res,1,i+2,mat.get_unsafe(r-1,c+i));
01371                                 for (int i=-2;i<=2;++i) VicinityTraits<ReturnType>::insertInContainer(res,2,i+2,mat.get_unsafe(r,c+i));
01372                                 for (int i=-2;i<=2;++i) VicinityTraits<ReturnType>::insertInContainer(res,3,i+2,mat.get_unsafe(r+1,c+i));
01373                                 for (int i=-1;i<=1;++i) VicinityTraits<ReturnType>::insertInContainer(res,4,i+2,mat.get_unsafe(r+2,c+i));
01374                                 return res;
01375                         }
01376                 };
01377                 /**
01378                   * Template specialization for getVicinity.
01379                   *
01380                   * ( * * * * * )
01381                   * ( * * * * * )
01382                   * ( * * _ * * )
01383                   * ( * * * * * )
01384                   * ( * * * * * )
01385                   *
01386                   */
01387                 template<typename MatrixType,typename T,typename ReturnType> struct getVicinity<MatrixType,T,ReturnType,24>     {
01388                 public:
01389                         static ReturnType get(size_t r,size_t c,const MatrixType &mat)  {
01390                                 mat.ASSERT_ENOUGHROOM<2>(r,c);
01391                                 ReturnType res;
01392                                 VicinityTraits<ReturnType>::initialize(res,5);
01393                                 for (int i=-2;i<=2;++i) for (int j=-2;j<=2;++j) if (i||j) VicinityTraits<ReturnType>::insertInContainer(res,i+2,j+2,mat.get_unsafe(r+i,c+j));
01394                                 return res;
01395                         }
01396                 };
01397                 /**
01398                   * Template specialization for getVicinity.
01399                   *
01400                   * ( * * * * * )
01401                   * ( * * * * * )
01402                   * ( * * * * * )
01403                   * ( * * * * * )
01404                   * ( * * * * * )
01405                   *
01406                   */
01407                 template<typename MatrixType,typename T,typename ReturnType> struct getVicinity<MatrixType,T,ReturnType,25>     {
01408                 public:
01409                         static ReturnType get(size_t r,size_t c,const MatrixType &mat)  {
01410                                 mat.ASSERT_ENOUGHROOM<2>(r,c);
01411                                 ReturnType res;
01412                                 VicinityTraits<ReturnType>::initialize(res,5);
01413                                 for (int i=-2;i<=2;++i) for (int j=-2;j<=2;++j) VicinityTraits<ReturnType>::insertInContainer(res,i+2,j+2,mat.get_unsafe(r+i,c+j));
01414                                 return res;
01415                         }
01416                 };
01417 
01418                 template<typename MatrixType> size_t rank(const MatrixType &m,typename MatrixType::value_type eps)      {
01419                         size_t N=m.getRowCount();
01420                         ASSERT_(m.getColCount()==N);
01421                         MAT_TYPE_SAMESIZE_OF(MatrixType) tmp;
01422                         tmp.assignMatrix(m);
01423                         CArbitrarySubmatrixView<MatrixType> mat(tmp,0,N,0,N);
01424                         size_t res=0;
01425                         while (mat.getRowCount()!=0&&mat.getColCount()!=0)      {
01426                                 size_t row=0;
01427                                 if (std::abs(mat(0,0)<=eps))    {
01428                                         for (size_t i=1;i<mat.getColCount();++i) if (std::abs(mat(i,0))>eps)    {
01429                                                 row=i;
01430                                                 break;
01431                                         }
01432                                         if (row==0)     {
01433                                                 mat.deleteColumn(0);
01434                                                 continue;
01435                                         }
01436                                 }
01437                                 size_t pRow=mat.getProxyRow(row);
01438                                 for (size_t i=0;i<mat.getRowCount();++i)        {
01439                                         if (i==row) continue;
01440                                         size_t pRowAlt=mat.getProxyRow(i);
01441                                         typename MatrixType::value_type prop=mat.getWithRowProxied(pRowAlt,0)/mat.getWithRowProxied(pRow,0);
01442                                         for (size_t j=1;j<mat.getColCount();++j) mat.getWithRowProxied(pRowAlt,j)-=prop*mat.getWithRowProxied(pRow,j);
01443                                 }
01444                                 res++;
01445                                 mat.deleteColumn(0);
01446                                 mat.deleteRow(row);
01447                         }
01448                         return res;
01449                 }
01450 
01451                 template<typename JA> void pivotUntilIdentity(JointAccessor<JA> &joint) {
01452                         size_t N=joint.size();
01453                         //First step: the lower submatrix is erased ("downwards" movement).
01454                         for (size_t i=0;i<N-1;++i)      {
01455                                 //std::cout << "pivotUntilIdentity, i="<<i<< ", mat: "<< std::endl; joint.dumpToConsole(); std::cout << std::endl;
01456                                 joint.ensureSuitablePos(i);
01457                                 for (size_t j=i+1;j<N;++j) joint.substractRowAsNeeded(i,j);
01458                                 joint.unitarizeReducedRow(i);
01459                         }
01460                         joint.ensureAndUnitarizeLast();
01461                         //std::cout << "pivotUntilIdentity, after last, mat: "<< std::endl; joint.dumpToConsole(); std::cout << std::endl;
01462                         //Second step: the upper submatrix is erased ("upwards" movement, after the lower submatrix is cleared).
01463                         for (size_t i=N-1;i>0;--i) for (int j=i-1;j>=0;--j) joint.substractWhenReduced(i,j);
01464                 }
01465 
01466                 /*! Matrix left divide: RES = A<sup>-1</sup> · C  (A must be a square matrix). \sa rightDivideSquare,fastLeftDivideSquare */
01467                 template <typename MAT1,typename MAT2,typename MAT3> inline void leftDivideSquare(const MAT1 &C,const MAT2 &A,MAT3 &RES) {
01468                         MAT_TYPE_SAMESIZE_OF(MAT2) tmp;
01469                         tmp.assignMatrix(A);
01470                         RES.assignMatrix(C);
01471                         fastLeftDivideSquare(RES,tmp);
01472                 }
01473 
01474                 /*! Matrix right divide: RES = C · B<sup>-1</sup> (B must be a square matrix). \sa leftDivideSquare,fastRightDivideSquare */
01475                 template <typename MAT1,typename MAT2,typename MAT3> inline void rightDivideSquare(const MAT1 &C,const MAT2 &B, MAT3 &RES) {
01476                         MAT_TYPE_SAMESIZE_OF(MAT2) tmp;
01477                         tmp.assignMatrix(B);
01478                         RES.assignMatrix(C);
01479                         fastRightDivideSquare(RES,tmp);
01480                 }
01481 
01482                 /*! Matrix left divide: B=A<sup>-1</sup>·C. A must be a square matrix, which is destroyed. C is also destroyed, and assigned to the looked for quotient. */
01483                 template<typename MAT1,typename MAT2> inline void fastLeftDivideSquare(MAT1 &inout_CB,MAT2 &willBeDestroyed_A)  {
01484                         JointHorizontalAccessor<MAT2,MAT1> jha=JointHorizontalAccessor<MAT2,MAT1>(willBeDestroyed_A,inout_CB);  //I don't like temporary variables, but this needs a scope.
01485                         JointAccessor<JointHorizontalAccessor<MAT2,MAT1> > ja=JointAccessor<JointHorizontalAccessor<MAT2,MAT1> >(jha);
01486                         pivotUntilIdentity(ja);
01487                 }
01488 
01489                 /*! Matrix left divide: A=C·B<sup>-1</sup>. B must be a square matrix, which is destroyed. C is also destroyed, and assigned to the looked for quotient. */
01490                 template<typename MAT1,typename MAT2> inline void fastRightDivideSquare(MAT1 &inout_CA,MAT2 &willBeDestroyed_B) {
01491                         JointVerticalAccessor<MAT2,MAT1> jva=JointVerticalAccessor<MAT2,MAT1>(willBeDestroyed_B,inout_CA);      //I don't like temporary variables, but this needs a scope.
01492                         JointAccessor<JointVerticalAccessor<MAT2,MAT1> > ja=JointAccessor<JointVerticalAccessor<MAT2,MAT1> >(jva);
01493                         pivotUntilIdentity(ja);
01494                 }
01495 
01496         } // end of detail namespace
01497 
01498         } // End of math namespace
01499 } // End of mrpt namespace
01500 
01501 
01502 #endif