LeastSquares.cpp

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS 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
(at your option) any later version.

BIAS 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 BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "LeastSquares.hh"
#include "Lapack.hh"
#include <Base/Common/BIASpragma.hh>

using namespace BIAS;
using namespace std;

/////////////////////////////////////////////////////////////////////////
///           LeastSquaresBase
/////////////////////////////////////////////////////////////////////////

int LeastSquaresBase::Init(unsigned SolutionSize, bool ReduceToATA)
{
  _ReduceToATA=ReduceToATA;
  _SolutionSize=SolutionSize;
  _ATA.newsize(_SolutionSize, _SolutionSize);
  _Weight.newsize(_SolutionSize, _SolutionSize);
  _ATb.newsize(_SolutionSize);
  return 0;
}

int LeastSquaresBase::Solve(Matrix<double> &A, Vector<double> &b, 
                            Vector<double> &x)
{
  BIASERR("only the overloaded function in the derived class can be used");
  return -1;
}

int LeastSquaresBase::Solve(Matrix<double> &A, Vector<double> &x)
{
  BIASERR("only the overloaded function in the derived class can be used");
  return -1;
}

int LeastSquaresBase::WeightedSolve(Matrix<double> &A, Vector<double> &b, 
                                    Vector<double> &weights, Vector<double> &x)
{
  BIASERR("only the overloaded function in the derived class can be used");
  return -1;
}

int LeastSquaresBase::WeightedSolve(Matrix<double> &A, Vector<double> &weights,
                                    Vector<double> &x)
{
  BIASERR("only the overloaded function in the derived class can be used");
  return -1;
}


////////////////////////////////////////////////////////////////////////////
///             LeastSquaresLapack
///////////////////////////////////////////////////////////////////////////


int LeastSquaresLapack::Solve(Matrix<double> &A, Vector<double> &b, 
                              Vector<double> &x)
{
  BIASDOUT(D_LS_MATRIXES, "A: "<<A<<"\nb: "<<b);
  int res=0;
  if (_ReduceToATA){
    _AT=A.Transpose();
    _AT.Mult(A, _ATA);
    _AT.Mult(b, _ATb);
    BIASDOUT(D_LS_RED_MATRIXES, "_ATA: "<<_ATA<<"\n_ATb: "<<_ATb);
    x=Lapack_LLS_QR_linear_solve(_ATA, _ATb, res);
  } else {
    x=Lapack_LLS_QR_linear_solve(A, b, res);
  }

  return res;
}

int LeastSquaresLapack::Solve(Matrix<double> &A, Vector<double> &x)
{
  BIASERR("unfinished");
  return -1;
}

int LeastSquaresLapack::WeightedSolve(Matrix<double> &A, Vector<double> &b, 
                                      Vector<double> &weights, 
                                      Vector<double> &x)
{
  BIASDOUT(D_LS_MATRIXES, "A: "<<A<<"\nb: "<<b);
  int res=0;
  if (_ReduceToATA){
    Matrix<double> wA(A);
    Vector<double> wb(b);
    for (unsigned i=0; i<_SolutionSize; i++){
      wA.ScaleRow(i, weights[i]);
      wb[i]*=weights[i];
    }
    _AT=wA.Transpose();
    _AT.Mult(wA, _ATA);
    _AT.Mult(wb, _ATb);
    BIASDOUT(D_LS_RED_MATRIXES, "_ATA: "<<_ATA<<"\n_ATb: "<<_ATb);
    x=Lapack_LLS_QR_linear_solve(_ATA, _ATb, res);
  } else {
    _Weight.newsize(A.num_rows(), A.num_rows());
    for (int i=0; i<A.num_rows(); i++){
      _Weight[i][i]=1.0/weights[i];
    }
    x=Lapack_WLLS_solve(A, b, _Weight, res);
  }
  return res;
}

int LeastSquaresLapack::WeightedSolve(Matrix<double> &A, 
                                      Vector<double> &weights,
                                      Vector<double> &x)
{
  BIASERR("unfinished");
  return -1;
}


////////////////////////////////////////////////////////////////////////////
///             LeastSquaresSVD
///////////////////////////////////////////////////////////////////////////


/** solve |Ax-b|=min using svd with mxn Matrix A

    |r|^2 = |Ax-b|^2 = (Ax-b)^T(Ax-b) 
    = x^T A^T A x - x^T A^T b - b^T A x + b^T b =
       use x^T A^T b = b^T A x (both are real)
    = x^T A^T A x - 2 x^T A^T b + b^T b =
       use SVD(A^T A) := W S W^T since A^T A is symmetric
    = x^T W S W^T x - 2 x^T A^T b + b^T b =
       set z:=W^T x -> x=Wz or x^T = z^T W^T
    = z^T S z - 2 z^T W^T A^T b + b^T b =
       set d:= W^T A^T b
    = z^T S z - 2 z^T d + b^T b =
    = sum_i(S_i*z_i^2 - 2 z_i d_i + b_i^2)

    d |r|^2 / dx = d|r|^2/dz * dz/dx == 0
    -> d|r|^2/dz=0
    -> z_i = d_i / S_i 

    x = W z

    solve least squares using svd:
    1. calculate SVD(A^T A) = W S W^T
    2. calculate d = W^T A^T b
    3. calculate z: z_i = d_i/S_i
    4. calculate x = W z

    @author woelk 04 2003 */
int LeastSquaresSVD::Solve(Matrix<double> &A, Vector<double> &b, 
                           Vector<double> &x)
{
  BIASDOUT(D_LS_MATRIXES, "A: "<<A<<"\nb: "<<b);
  _AT=A.Transpose();
  _AT.Mult(A, _ATA);
  _AT.Mult(b, _ATb);
  BIASDOUT(D_LS_RED_MATRIXES, "_ATA: "<<_ATA<<"\n_ATb: "<<_ATb);
  if (_svd.Compute(_ATA)!=0){
     return -2;
  }
  Vector<double> d(A.num_rows()), z(A.num_cols());
  Vector<double> S=_svd.GetS();
  d=_svd.GetVT() * _ATb;
  BIASDOUT(D_LS_SVD, "_ATA: "<<_ATA<<"\nd: "<<d<<"\nS: "<<S);
  for (int i=0; i<A.num_cols(); i++)
    z[i]=(fabs(S[i])>DEFAULT_DOUBLE_ZERO_THRESHOLD)?d[i]/S[i]:0.0;
  
  x = _svd.GetU() * z;
  return 0;
}


int LeastSquaresSVD::Solve(Matrix<double> &A, Vector<double> &x)
{
  BIASDOUT(D_LS_MATRIXES, "A: "<<A);
  if (x.size()!=(int)_SolutionSize)
    x.newsize(_SolutionSize);

  if (_ReduceToATA){
    _AT=A.Transpose();
    _AT.Mult(A, _ATA);
    BIASDOUT(D_LS_RED_MATRIXES, "_ATA: "<<_ATA);
     if (General_singular_value_decomposition(_ATA, _S, _VT)!=0){
      BIASERR("error in svd"); 
      return -2;
    }
    for (unsigned i=0; i<_SolutionSize; i++){
      x[i]=_VT[_SolutionSize-1][i];
    }
  } else {
    if (General_singular_value_decomposition(A, _S, _VT)!=0){
      BIASERR("error in svd"); 
      return -2;
    }
    for (unsigned i=0; i<_SolutionSize; i++){
      x[i]=_VT[_SolutionSize-1][i];
    }
  }
  return 0;
}


int LeastSquaresSVD::WeightedSolve(Matrix<double> &A, Vector<double> &b, 
                                   Vector<double> &weights, Vector<double> &x)
{
  BIASERR("unfinished");
  return -1;
}

int LeastSquaresSVD::WeightedSolve(Matrix<double> &A, Vector<double> &weights, 
                                   Vector<double> &x)
{
  BIASDOUT(D_LS_MATRIXES, "A: "<<A<<"\nweights: "<<weights);
  if (x.size()!=(int)_SolutionSize)
    x.newsize(_SolutionSize);

  Matrix<double> wA(A);
  for (unsigned i=0; i<_SolutionSize; i++){
    wA.ScaleRow(i, weights[i]);
  }

  if (_ReduceToATA){
    _AT=wA.Transpose();
    _AT.Mult(wA, _ATA);
    BIASDOUT(D_LS_RED_MATRIXES, "_ATA: "<<_ATA);
    if (General_singular_value_decomposition(_ATA, _S, _VT)!=0){
      BIASERR("error in svd"); 
      return -2;
    }
    for (unsigned i=0; i<_SolutionSize; i++){
      x[i]=_VT[_SolutionSize-1][i];
    }
  } else {
    if (General_singular_value_decomposition(wA, _S, _VT)!=0){
      BIASERR("error in svd"); 
      return -2;
    }
    for (unsigned i=0; i<_SolutionSize; i++){
      x[i]=_VT[_SolutionSize-1][i];
    }
  }
  return 0;
}