matrix.cpp

摄影测量专业。实现单像后方交会以及立体像对的前方交会。以文件形式读取控制点和像点坐标。

/**
\file     Matrix.h
\brief    The Zenautics Matrix Class
\author   Glenn D. MacGougan (GDM)
\date     2008-09-22
\version  0.06 Beta

\b Version \b Information \n
This is the open source version (BSD license). The Professional Version
is avaiable via http://www.zenautics.com. The Professional Version
is highly optimized using SIMD and includes optimization for multi-core
processors.

\b License \b Information \n
Copyright (c) 2008, Glenn D. MacGougan, Zenautics Technologies Inc. \n

Redistribution and use in source and binary forms, with or without
modification, of the specified files is permitted provided the following 
conditions are met: \n

- Redistributions of source code must retain the above copyright
  notice, this list of conditions and the following disclaimer. \n
- Redistributions in binary form must reproduce the above copyright
  notice, this list of conditions and the following disclaimer in the
  documentation and/or other materials provided with the distribution. \n
- The name(s) of the contributor(s) may not be used to endorse or promote 
  products derived from this software without specific prior written 
  permission. \n

THIS SOFTWARE IS PROVIDED BY THE CONTRIBUTORS ``AS IS'' AND ANY EXPRESS 
OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 
OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 
SUCH DAMAGE.

\b NOTES: \n
This code was developed using rigourous unit testing for every function 
and operation. Despite any rigorous development process, bugs are
inevitable. Please report bugs and suggested fixes to glenn @ zenautics.com.\n

\b Preprocessor Defines \n
The implementation of exception handling and the use of namespace is
not standardized among older compilers, the following deines may be 
necessary. \n
#define _MATRIX_NO_NAMESPACE // removes namespace support. \n
#define _MATRIX_NO_EXCEPTION // removes exception handling support. \n
*/

#include <stdlib.h>
#include <string.h>

#include "Matrix.h"
#include "cmatrix.h"

// deal with msvc empty projects
#ifndef WIN32
  #ifdef _WIN32
    #define WIN32
  #endif
#endif

#ifndef WIN32
#define _CRT_SECURE_NO_DEPRECATE
#endif


#ifndef DEG2RAD
#define DEG2RAD   (0.017453292519943295769236907684886)  //!< PI/180.0
#endif

#ifndef RAD2DEG
#define RAD2DEG   (57.295779513082320876798154814105)    //!< 180.0/PI
#endif

namespace Zenautics
{
  /// A static double  value used for bad referencing. i.e. give me element 10 of 1x9 vector.
  static double staticglobal_BadDouble = 0.0;

  // A boolean used to ensure initialization of the mtx engine.
  bool Matrix::m_IsMTXInitialized = false;


#ifndef _MATRIX_NO_EXCEPTION

  MatrixException::MatrixException( const char* msg )
  {
    unsigned msgLength = 0;
    if( msg == NULL )
    {
#ifndef _CRT_SECURE_NO_DEPRECATE
      strcpy_s( m_msg, 256, "Unknown Matrix Exception" );
#else
      strcpy( m_msg, "Unknown Matrix Exception" );
#endif
    }
    else
    {
      msgLength = (unsigned)strlen( msg );
      // note 255 here, not 256, 
      // just in case msgLength is 255 and we add '\0' to the end of m_msg.
#ifndef _CRT_SECURE_NO_DEPRECATE
      if( msgLength < 255 )
      {
        strncpy_s( m_msg, 256, msg, msgLength );
        m_msg[msgLength] = '\0';
      }
      else
      {
        strncpy_s( m_msg, 256, msg, 255 );
        m_msg[255] = '\0';
      }
#else
      if( msgLength < 255 )
      {
        strncpy( m_msg, msg, msgLength );
        m_msg[msgLength] = '\0';
      }
      else
      {
        strncpy( m_msg, msg, 255 );
        m_msg[255] = '\0';
      }
#endif
    }
    m_ExceptionString = m_msg;
  }

  MatrixException::MatrixException(const MatrixException& matrix_exception)
  {
    // This will copy only the matrix exception string.
    m_ExceptionString = matrix_exception.m_ExceptionString;
  }

  std::string MatrixException::GetExceptionMessage()
  {
    return m_ExceptionString;
  }

  MatrixException::operator const char*()
  {
    return m_ExceptionString.c_str();
  }
#endif


  // default constructor
  Matrix::Matrix()
    :m_MatrixElement(m_Matrix)
  {
    if( !m_IsMTXInitialized )
    {
      if( !MTX_Initialize_MTXEngine() )
      {
        m_IsMTXInitialized = false;
        MatrixError( "Matrix", "Failed to initialize the MTX Engine." );
      }
      else
      {
        m_IsMTXInitialized = true;
      }
    }

    MTX_Init( &m_Matrix );
  }


  // destructor
  Matrix::~Matrix()
  { 
    if( !MTX_Free( &m_Matrix ) )
    {
      MatrixError( "~Matrix", "Unable to free memory properly" );
    }
  }


  // vector style constructor
  Matrix::Matrix( const unsigned nrows )
    :m_MatrixElement(m_Matrix)
  { 
    if( !m_IsMTXInitialized )
    {
      if( !MTX_Initialize_MTXEngine() )
      {
        m_IsMTXInitialized = false;
        MatrixError( "Matrix", "Failed to initialize the MTX Engine." );
      }
      else
      {
        m_IsMTXInitialized = true;
      }
    }

    MTX_Init( &m_Matrix );
    if( !MTX_Calloc( &m_Matrix, nrows, 1, true ) )
    {
      char msg[128];
#ifndef _CRT_SECURE_NO_DEPRECATE
      sprintf_s( msg, 128, "Unable to allocate enough memory for Matrix as vector(%d)", nrows );
#else
      sprintf( msg, "Unable to allocate enough memory for Matrix as vector(%d)", nrows );
#endif
      MatrixError( "Matrix", msg );
    }
  }


  // matrix style constructor
  Matrix::Matrix( const unsigned nrows, const unsigned ncols, const bool isReal )
    :m_MatrixElement(m_Matrix)
  { 
    if( !m_IsMTXInitialized )
    {
      if( !MTX_Initialize_MTXEngine() )
      {
        m_IsMTXInitialized = false;
        MatrixError( "Matrix", "Failed to initialize the MTX Engine." );
      }
      else
      {
        m_IsMTXInitialized = true;
      }
    }

    MTX_Init( &m_Matrix );
    if( !MTX_Calloc( &m_Matrix, nrows, ncols, isReal ) )
    {
      char msg[128];
#ifndef _CRT_SECURE_NO_DEPRECATE
      if( isReal )
        sprintf_s( msg, 128, "Unable to allocate enough memory for Matrix(%d,%d)", nrows, ncols );
      else
        sprintf_s( msg, 128, "Unable to allocate enough memory for complex Matrix(%d,%d)", nrows, ncols );
#else
      if( isReal )
        sprintf( msg, "Unable to allocate enough memory for Matrix(%d,%d)", nrows, ncols );
      else
        sprintf( msg, "Unable to allocate enough memory for complex Matrix(%d,%d)", nrows, ncols );
#endif

      MatrixError( "Matrix", msg );
    }
  }


  // constructor reading data from file   
  Matrix::Matrix( const char* path, bool& itWorked )
    :m_MatrixElement(m_Matrix)
  {
    if( !m_IsMTXInitialized )
    {
      if( !MTX_Initialize_MTXEngine() )
      {
        m_IsMTXInitialized = false;
        MatrixError( "Matrix", "Failed to initialize the MTX Engine." );
      }
      else
      {
        m_IsMTXInitialized = true;
      }
    }

    MTX_Init( &m_Matrix );

    if( MTX_ReadFromFile( &m_Matrix, path ) )
      itWorked = true;
    else
      itWorked = false;  
  }

  // copy constructor
  Matrix::Matrix( const Matrix& mat )
    :m_MatrixElement(m_Matrix)
  {
    MTX_Init( &m_Matrix );
    if( !MTX_Copy( &(mat.m_Matrix), &m_Matrix ) )
    {
      MatrixError( "Matrix", "Copy constructor failed to copy input matrix." );
    }
  }

  // copy from a static matrix
  Matrix::Matrix(const double mat[], const unsigned nrows, const unsigned ncols )
    :m_MatrixElement(m_Matrix)
  {
    MTX_Init( &m_Matrix );
    if( mat == NULL )
    {
      MatrixError( "Matrix", "Input static double array(matrix) pointer is NULL" );
    }
    if( !MTX_SetFromStaticMatrix( &m_Matrix, mat, nrows, ncols ) )
    {
      MatrixError( "Matrix", "Failed to set the matrix from a static double array(matrix)" );
    }
  }

  // copy from a matrix string 
  Matrix::Matrix(const char* strMatrix)
    :m_MatrixElement(m_Matrix)
  {
    MTX_Init( &m_Matrix );
    if( !MTX_SetFromMatrixString( &m_Matrix, strMatrix ) )
    {
      MatrixError( "Matrix = \"string matrix\"", "Unable to set matrix from the string specified." );
    }    
  }


  // assignment operator (constructor)
  Matrix& Matrix::operator= (const Matrix& mat)
  {
    // trap assignment to self
    if( this == &mat )
      return *this;

    if( !MTX_Copy( &mat.m_Matrix, &m_Matrix ) )
    {
      MatrixError( "operator=", "Failed to copy input matrix" );
    }

    return *this;
  }


  Matrix& Matrix::operator= (const double value)
  {
    if( !MTX_Malloc( &m_Matrix, 1, 1, true ) )
    {
      MatrixError( "operator=double", "Unable to redimension to 1x1." );
    }

    if( !MTX_SetValue( &m_Matrix, 0, 0, value ) )
    {
      MatrixError( "operator=double", "Unable to set double value." );
    }

    return *this;
  }

  Matrix& Matrix::operator= (const std::complex<double> value)
  {
    if( !MTX_Malloc( &m_Matrix, 1, 1, false ) )
    {
      MatrixError( "operator=std::complex<double>", "Unable to redimension to 1x1." );
    }

    if( !MTX_SetComplexValue( &m_Matrix, 0, 0, value.real(), value.imag() ) )
    {
      MatrixError( "operator=std::complex<double>", "Unable to set the value." );
    }

    return *this;
  }

  Matrix& Matrix::operator=(const char* strMatrix)
  {
    if( !MTX_SetFromMatrixString( &m_Matrix, strMatrix ) )
    {
      MatrixError( "operator=string", "Unable to set matrix from the string specified." );
    }
    return *this;
  }

  bool Matrix::Clear()
  {
    if( MTX_Free( &m_Matrix ) )
    {
      return true;
    }
    else
    {
      MTX_ERROR_MSG( "MTX_FREE returned false." );
      return false;    
    }
  }

  void Matrix::MatrixError( const char* error )
  {
    Clear();
    StaticMatrixError( error );
  }

  void Matrix::MatrixError( const char* function, const char* error )
  {
    Clear();
    StaticMatrixError( function, error );
  }


  // static
  void Matrix::StaticMatrixError( const char* error )
  {
    StaticMatrixError( "", error );
  }

  // static
  void Matrix::StaticMatrixError( const char* function, const char* error )
  {
    char msg[256];
#ifndef _CRT_SECURE_NO_DEPRECATE
    if( strstr(function,"operator") != NULL )
      sprintf_s( msg, 256, "\nMatrix::%s, Error:\n%s\n", function, error );   
    else
      sprintf_s( msg, 256, "\nMatrix::%s(), Error:\n%s\n", function, error );
#else
    if( strstr(function,"operator") != NULL )
      sprintf( msg, "\nMatrix::%s, Error:\n%s\n", function, error );   
    else
      sprintf( msg, "\nMatrix::%s(), Error:\n%s\n", function, error );
#endif

#ifndef _MATRIX_NO_EXCEPTION

    throw MatrixException(msg);
    return;

#else

    printf( "%s\r\n", msg );   

    // no choice but to call exit!
    exit(1);

#endif    
  }


  bool Matrix::isEmpty() const
  {
    if( MTX_isNull( &m_Matrix ) )
    {
      return true;
    }
    else
    {
      return false;
    }
  }

  bool Matrix::isConformal(const Matrix& mat) const
  {
    if( MTX_isConformalForMultiplication( &m_Matrix, &mat.m_Matrix ) )
    {
      return true;
    }
    else
    {
      MTX_ERROR_MSG( "MTX_isConformalForMultiplication returned false." );
      return false;
    }
  }

  bool Matrix::isSameSize(const Matrix& mat) const
  {
    if( MTX_isSameSize( &m_Matrix, &mat.m_Matrix ) )
    {
      return true;
    }
    else 
    {
      MTX_ERROR_MSG( "MTX_isSameSize returned false." );
      return false;
    }
  }

  bool Matrix::isSquare() const
  {
    if( MTX_isSquare( &m_Matrix ) )
    {
      return true;
    }
    else 
    {
      MTX_ERROR_MSG( "MTX_isSquare returned false." );
      return false;
    }
  }

  unsigned  Matrix::GetNrCols() const
  {
    return m_Matrix.ncols;
  }

  unsigned  Matrix::ncols() const
  {
    return m_Matrix.ncols;
  }

  unsigned Matrix::GetNrElems() const
  {
    return m_Matrix.ncols*m_Matrix.nrows;
  }

  unsigned Matrix::nelems() const
  {
    return m_Matrix.ncols*m_Matrix.nrows;
  }

  unsigned Matrix::GetNrRows() const
  {
    return m_Matrix.nrows;
  }

  unsigned Matrix::nrows() const
  {
    return m_Matrix.nrows;
  }

  unsigned Matrix::GetLength() const
  {
    if( m_Matrix.nrows > m_Matrix.ncols )
      return m_Matrix.nrows;
    else
      return m_Matrix.ncols;
  }

  double Matrix::real(const unsigned row, const unsigned col)
  {
    if( IndexCheck(row,col) )
    {
      if( m_Matrix.isReal )
      {
        return m_Matrix.data[col][row];
      }
      else
      {
        return m_Matrix.cplx[col][row].re;
      }
    }
    else
    {
      return 0.0;
    }
  }

  double Matrix::real(const unsigned index)
  {
    unsigned row = 0;
    unsigned col = 0;

    if( IndexCheck(index) )
    {
      if( m_Matrix.ncols == 1 )
      {
        row = index;
      }
      else if( m_Matrix.nrows == 1 )
      {
        col = index;
      }
      else
      {
        // access the matrix as a singular column array
        col = index / m_Matrix.nrows;
        row = index - col*m_Matrix.nrows;
      }
      if( m_Matrix.isReal )
      {
        return m_Matrix.data[col][row];
      }
      else
      {
        return m_Matrix.cplx[col][row].re;
      }
    }
    else
    {
      return 0.0;
    }
  }

  double Matrix::imag(const unsigned row, const unsigned col)
  {
    if( IndexCheck(row,col) )
    {
      if( m_Matrix.isReal )
      {
        return 0.0;
      }
      else
      {
        return m_Matrix.cplx[col][row].im;
      }
    }
    else
    {
      return 0.0;
    }
  }

  double Matrix::imag(const unsigned index)
  {
    unsigned row = 0;
    unsigned col = 0;

    if( IndexCheck(index) )
    {
      if( m_Matrix.ncols == 1 )
      {
        row = index;
      }
      else if( m_Matrix.nrows == 1 )
      {
        col = index;
      }
      else
      {
        // access the matrix as a singular column array
        col = index / m_Matrix.nrows;
        row = index - col*m_Matrix.nrows;
      }
      if( m_Matrix.isReal )