matrixoperators.hpp

一个gps小工具包

#pragma ident "$Id: MatrixOperators.hpp 222 2006-10-12 20:39:39Z btolman $"

//============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 2.1 of the License, or
//  any later version.
//
//  The GPSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GPSTk; if not, write to the Free Software Foundation,
//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//  
//  Copyright 2004, The University of Texas at Austin
//
//============================================================================

/**
 * @file MatrixOperators.hpp
 * Matrix operators (arithmetic, transpose(), inverse(), etc)
 */

#ifndef GPSTK_MATRIX_OPERATORS_HPP
#define GPSTK_MATRIX_OPERATORS_HPP

#include "MiscMath.hpp"
#include "MatrixFunctors.hpp"

namespace gpstk
{
 /** @addtogroup VectorGroup */
   //@{
 
/** 
 * Returns the top to bottom concatenation of Matrices l and r only if they have the
 * same number of columns.
 */
   template <class T, class BaseClass1, class BaseClass2>
   inline Matrix<T> operator&&(const ConstMatrixBase<T, BaseClass1>& l, 
                            const ConstMatrixBase<T, BaseClass2>& r) 
   throw(MatrixException)
   {
      if (l.cols() != r.cols())
      {
         MatrixException e("Incompatible dimensions for Matrix && Matrix");
         GPSTK_THROW(e);
      }

      size_t rows = l.rows() + r.rows();
      size_t cols = l.cols();
      Matrix<T> toReturn(rows, cols);

      for (rows = 0; rows < l.rows(); rows++)
         for (cols = 0; cols < l.cols(); cols++)
            toReturn(rows, cols) = l(rows, cols);

      for (rows = 0; rows < r.rows(); rows++)
         for (cols = 0; cols < l.cols(); cols++)
            toReturn(rows + l.rows(), cols) = r(rows, cols);

      return toReturn;
   }

/** 
 * Returns the top to bottom concatenation of Matrix t and Vector b
 * only if they have the same number of columns.
 */
   template <class T, class BaseClass1, class BaseClass2>
   inline Matrix<T> operator&&(const ConstMatrixBase<T, BaseClass1>& t, 
                            const ConstVectorBase<T, BaseClass2>& b) 
   throw(MatrixException)
   {
      if (t.cols() != b.size())
      {
         MatrixException e("Incompatible dimensions for Matrix && Vector");
         GPSTK_THROW(e);
      }

      size_t rows = t.rows() + 1;
      size_t cols = t.cols();
      Matrix<T> toReturn(rows, cols);

      for (rows = 0; rows < t.rows(); rows++)
         for (cols = 0; cols < t.cols(); cols++)
            toReturn(rows, cols) = t(rows, cols);

      for (cols = 0; cols < t.cols(); cols++)
         toReturn(t.rows(), cols) = b(cols);

      return toReturn;
   }

/** 
 * Returns the top to bottom concatenation of Vector t and Matrix b
 * only if they have the same number of columns.
 */
   template <class T, class BaseClass1, class BaseClass2>
   inline Matrix<T> operator&&(const ConstVectorBase<T, BaseClass1>& t, 
                            const ConstMatrixBase<T, BaseClass2>& b) 
   throw(MatrixException)
   {
      if (t.size() != b.cols())
      {
         MatrixException e("Incompatible dimensions for Vector && Matrix");
         GPSTK_THROW(e);
      }

      size_t rows = 1 + b.rows();
      size_t cols = b.cols();
      Matrix<T> toReturn(rows, cols);

      for (cols = 0; cols < b.cols(); cols++)
         toReturn(0, cols) = t(cols);

      for (rows = 1; rows < b.rows()+1; rows++)
         for (cols = 0; cols < b.cols(); cols++)
            toReturn(rows, cols) = b(rows, cols);

      return toReturn;
   }

/** 
 * Returns the left to right concatenation of l and r only if they have the
 * same number of rows.
 */
   template <class T, class BaseClass1, class BaseClass2>
   inline Matrix<T> operator||(const ConstMatrixBase<T, BaseClass1>& l,
                            const ConstMatrixBase<T, BaseClass2>& r)  
      throw(MatrixException)
   {
      if (l.rows() != r.rows())
      {
         MatrixException e("Incompatible dimensions for Matrix || Matrix");
         GPSTK_THROW(e);
      }

      size_t rows = l.rows();
      size_t cols = l.cols() + r.cols();
      Matrix<T> toReturn(rows, cols);

      for (cols = 0; cols < l.cols(); cols++)
         for (rows = 0; rows < l.rows(); rows++)
            toReturn(rows, cols) = l(rows, cols);

      for (cols = 0; cols < r.cols(); cols++)
         for (rows = 0; rows < l.rows(); rows++)
            toReturn(rows, cols + l.cols()) = r(rows,cols);

      return toReturn;
   }

/** 
 * Returns the left to right concatenation of Matrix l and Vector r
 * only if they have the same number of rows.
 */
   template <class T, class BaseClass1, class BaseClass2>
   inline Matrix<T> operator||(const ConstMatrixBase<T, BaseClass1>& l,
                            const ConstVectorBase<T, BaseClass2>& r)
      throw(MatrixException)
   {
      if (l.rows() != r.size())
      {
         MatrixException e("Incompatible dimensions for Matrix || Vector");
         GPSTK_THROW(e);
      }

      size_t rows = l.rows();
      size_t cols = l.cols() + 1;
      Matrix<T> toReturn(rows, cols);

      for (cols = 0; cols < l.cols(); cols++)
         for (rows = 0; rows < l.rows(); rows++)
            toReturn(rows, cols) = l(rows, cols);

      for (rows = 0; rows < l.rows(); rows++)
         toReturn(rows, l.cols()) = r(rows);

      return toReturn;
   }

/** 
 * Returns the left to right concatenation of Vector l and Matrix r
 * only if they have the same number of rows.
 */
   template <class T, class BaseClass1, class BaseClass2>
   inline Matrix<T> operator||(const ConstVectorBase<T, BaseClass1>& l,
                            const ConstMatrixBase<T, BaseClass2>& r)
      throw(MatrixException)
   {
      if (l.size() != r.rows())
      {
         MatrixException e("Incompatible dimensions for Vector || Matrix");
         GPSTK_THROW(e);
      }

      size_t rows = r.rows();
      size_t cols = r.cols() + 1;
      Matrix<T> toReturn(rows, cols);

      for (rows = 0; rows < r.rows(); rows++)
         toReturn(rows, 0) = l(rows);

      for (cols = 1; cols < r.cols()+1; cols++)
         for (rows = 0; rows < r.rows(); rows++)
            toReturn(rows, cols) = r(rows, cols);

      return toReturn;
   }

/** 
 * Returns the minor matrix of l at element (row, col).  A minor matrix is the
 * same matrix as \c l but with row \c row and col \c col removed.
 */
   template <class T, class BaseClass>
   inline Matrix<T> minorMatrix(const ConstMatrixBase<T, BaseClass>& l,
                          size_t row, size_t col) 
      throw(MatrixException)
   {
      if ((row >= l.rows()) || (col >= l.cols()))
      {
         MatrixException e("Invalid row or column for minorMatrix()");
         GPSTK_THROW(e);
      }
         // handle special cases
      if (row == 0)
      {
         if (col == 0)
         {
            return Matrix<T>(l,1,1,l.rows()-1,l.cols()-1);  
         }
         else if (col == (l.cols() - 1))
         {
            return Matrix<T>(l,1,0,l.rows()-1,l.cols()-1);
         }
         else
         {
            return Matrix<T>(l,1,0,l.rows()-1,col) ||
               Matrix<T>(l,1,col+1,l.rows()-1,l.cols()-col-1);
         }
      }
      else if (row == (l.rows() - 1))
      {
         if (col == 0)
         {
            return Matrix<T>(l,0,1,l.rows()-1,l.cols()-1);
         }
         else if (col == (l.cols() - 1))
         {
            return Matrix<T>(l,0,0,l.rows()-1,l.cols()-1);
         }
         else
         {
            return Matrix<T>(l,0,0,l.rows()-1,col) ||
               Matrix<T>(l,0,col+1,l.rows()-1,l.cols()-col-1);
         }
      }
      else if (col == 0)
      {
         return Matrix<T>(l,0,1,row,l.cols()-1) &&
            Matrix<T>(l,row+1,1,l.rows()-row-1,l.cols()-1);
      }
      else if (col == (l.cols() - 1))
      {
         return Matrix<T>(l,0,0,row,l.cols()-1) &&
            Matrix<T>(l,row+1,0,l.rows()-row-1,l.cols()-1);
      }
      else
      {
         return (Matrix<T>(l, 0, 0, row, col) || 
                 Matrix<T>(l, 0, col + 1, row, l.cols()-col-1)) &&
            (Matrix<T>(l, row + 1, 0, l.rows()-row-1, col) ||
             Matrix<T>(l, row + 1, col + 1, l.rows()-row-1, l.cols()-col-1));
      }
   }

/**
 * Returns a matrix that is \c m transposed.
 */
   template <class T, class BaseClass>
   inline Matrix<T> transpose(const ConstMatrixBase<T, BaseClass>& m)
   {
      Matrix<T> temp(m.cols(), m.rows());
      size_t i, j;
      for (i = 0; i < m.rows(); i++)
         for (j = 0; j < m.cols(); j++)
            temp(j,i) = m(i,j);
      return temp;
   }
 
/**
 * Uses an LU Decomposition to calculate the determinate of m. This is
 * faster than longDet() for large matricies.
 */
   template <class T, class BaseClass>
   inline T det(const ConstMatrixBase<T, BaseClass>& m) 
      throw(MatrixException)
   {
      try
      {
         LUDecomp<T> lud;
         lud(m);
         T det = 1;
         size_t i;
            // now just multiply down the main diagonal and then by the LUD sign.
         for(i = 0; i < m.rows(); i++)
            det *= lud.U(i,i);
         return det * lud.sign;
      }
      catch(SingularMatrixException &e)
      {
         return 0;
      }
      catch(MatrixException& e)
      {
         e.addText("in det()");
         GPSTK_RETHROW(e);
      }
   }

/**
 * returns the condition number of the matrix
 */
   template <class T, class BaseClass>
   inline T condNum(const ConstMatrixBase<T, BaseClass>& m, T& big, T& small) 
      throw ()
   {
      SVD<T> svd;
      svd(m);
      // SVD will sort singular values in descending order
      big = svd.S(0);
      small = svd.S(svd.S.size()-1);
      if(fabs(small) <= T(1.e-15)) return T(0);// TD replace with ~ machine precision
      return big/small;
   }

/**
 * returns the condition number of the matrix, doesnt require big or small..
 */
   template <class T, class BaseClass>
   inline T condNum(const ConstMatrixBase<T, BaseClass>& m) 
      throw ()
   {
      T big, small;
      return condNum(m, big, small);
   }

/**
 * Returns a new \c dim * \c dim matrix that's an identity matrix.
 */
   template <class T>
   inline Matrix<T> ident(size_t dim)
      throw(MatrixException)
   {
      if (dim == 0)
      {
         MatrixException e("Invalid (0) dimension for ident()");
         GPSTK_THROW(e);
      }
      Matrix<T> toReturn(dim, dim, T(0));
      size_t i;
      for (i = 0; i < toReturn.rows(); i++)
         toReturn(i,i) = T(1);
      return toReturn;
   }

/**
 * Return a rotation matrix [dimensioned 3x3, inverse() = transpose()]
 * for the rotation through \c angle radians about \c axis number (= 1, 2 or 3).
 */
   template <class T>
   inline Matrix<T> rotation(T angle, int axis)
      throw(MatrixException)
   {
      if (axis < 1 || axis > 3)
      {
         MatrixException e("Invalid axis (must be 1,2, or 3)");
         GPSTK_THROW(e);
      }
      Matrix<T> toReturn(3,3,T(0));
      int i1 = axis-1;
      int i2 = (i1+1) % 3;