/**
* stable invert stolen from ossimRpcSolver
*/
NEWMAT::Matrix
ossimRpcProjection::invert(const NEWMAT::Matrix& m)const
{
ossim_uint32 idx = 0;
NEWMAT::DiagonalMatrix d;
NEWMAT::Matrix u;
NEWMAT::Matrix v;
// decompose m.t*m which is stored in Temp into the singular values and vectors.
//
NEWMAT::SVD(m, d, u, v, true, true);
// invert the diagonal
// this is just doing the reciprical fo all diagonal components and store back int
// d. ths compute d inverse.
//
for(idx=0; idx < (ossim_uint32)d.Ncols(); ++idx)
{
if(d[idx] > 1e-14) //TBC : use DBL_EPSILON ?
{
d[idx] = 1.0/d[idx];
}
else
{
d[idx] = 0.0;
//DEBUG TBR
cout<<"warning: singular matrix in SVD"<<endl;
}
}
//compute inverse of decomposed m;
return v*d*u.t();
}
NEWMAT::Matrix rspfSensorModelTuple::invert(const NEWMAT::Matrix& m) const
{
rspf_uint32 idx = 0;
NEWMAT::DiagonalMatrix d;
NEWMAT::Matrix u;
NEWMAT::Matrix v;
NEWMAT::SVD(m, d, u, v, true, true);
for(idx=0; idx < (rspf_uint32)d.Ncols(); ++idx)
{
if(d[idx] > 1e-14) //TBC : use DBL_EPSILON ?
{
d[idx] = 1.0/d[idx];
}
else
{
d[idx] = 0.0;
if (traceDebug())
{
rspfNotify(rspfNotifyLevel_WARN)
<< "DEBUG: rspfSensorModelTuple::invert(): "
<< "\nsingular matrix in SVD..."
<< std::endl;
}
}
}
return v*d*u.t();
}