NEWMAT::ColumnVector
ossimRpcProjection::getResidue(const ossimTieGptSet& tieSet)
{
//init
NEWMAT::ColumnVector residue;
int dimObs;
bool useImageObs = useForward(); //caching
if (useImageObs)
{
dimObs = 2; //image observation
} else {
dimObs = 3; //ground observations
}
int no = dimObs * tieSet.size(); //number of observations
residue.ReSize(no);
const vector<ossimRefPtr<ossimTieGpt> >& theTPV = tieSet.getTiePoints();
vector<ossimRefPtr<ossimTieGpt> >::const_iterator tit;
unsigned long c=1;
if (useImageObs)
{
//image observations
ossimDpt resIm;
// loop on tie points
for (tit = theTPV.begin() ; tit != theTPV.end() ; ++tit)
{
//compute residue
resIm = (*tit)->tie - forward(**tit);
residue(c++) = resIm.x;
residue(c++) = resIm.y;
}
} else {
// ground observations
ossimGpt gd;
// loop on tie points
for (tit = theTPV.begin() ; tit != theTPV.end() ; ++tit)
{
//compute residue
gd = inverse((*tit)->tie);
residue(c++) = ((*tit)->lon - gd.lon) * 100000.0; //approx meters //TBC TBD
residue(c++) = ((*tit)->lat - gd.lat) * 100000.0 * cos(gd.lat / 180.0 * M_PI);
residue(c++) = (*tit)->hgt - gd.hgt; //meters
}
} //end of if (useImageObs)
return residue;
}
double ossimRpcModel::optimizeFit(const ossimTieGptSet& tieSet, double* targetVariance)
{
int nGpt = static_cast<int>(tieSet.size());
if(nGpt < 1)
{
return 0.0;
}
else if(nGpt < 2)
{
m_modelOptimizeType = OptImageTranslation;
}
else if(nGpt < 3)
{
//m_modelOptimizeType = OptImageTrans_scale;
m_modelOptimizeType = OptImageTranslation;
}
else
{
m_modelOptimizeType = OptImageAffine;
}
return ossimSensorModel::optimizeFit(tieSet, targetVariance);
}
void
ossimRpcProjection::buildNormalEquation(const ossimTieGptSet& tieSet,
NEWMAT::SymmetricMatrix& A,
NEWMAT::ColumnVector& residue,
NEWMAT::ColumnVector& projResidue,
double pstep_scale)
{
//goal: build Least Squares system
//constraint: never store full Jacobian matrix in memory (can be huge)
// so we build the matrices incrementally
// the system can be built using forward() or inverse() depending on the projection capabilities : useForward()
//
//TBD : add covariance matrix for each tie point
//init
int np = getNumberOfAdjustableParameters();
int dimObs;
bool useImageObs = useForward(); //caching
if (useImageObs)
{
dimObs = 2; //image observation
} else {
dimObs = 3; //ground observations
}
int no = dimObs * tieSet.size(); //number of observations
A.ReSize(np);
residue.ReSize(no);
projResidue.ReSize(np);
//Zeroify matrices that will be accumulated
A = 0.0;
projResidue = 0.0;
const vector<ossimRefPtr<ossimTieGpt> >& theTPV = tieSet.getTiePoints();
vector<ossimRefPtr<ossimTieGpt> >::const_iterator tit;
unsigned long c=1;
if (useImageObs)
{
//image observations
ossimDpt* imDerp = new ossimDpt[np];
ossimDpt resIm;
// loop on tie points
for (tit = theTPV.begin() ; tit != theTPV.end() ; ++tit)
{
//compute residue
resIm = (*tit)->tie - forward(*(*tit));
residue(c++) = resIm.x;
residue(c++) = resIm.y;
//compute all image derivatives regarding parametres for the tie point position
for(int p=0;p<np;++p)
{
imDerp[p] = getForwardDeriv( p , *(*tit) , pstep_scale);
}
//compute influence of tie point on all sytem elements
for(int p1=0;p1<np;++p1)
{
//proj residue: J * residue
projResidue.element(p1) += imDerp[p1].x * resIm.x + imDerp[p1].y * resIm.y;
//normal matrix A = transpose(J)*J
for(int p2=p1;p2<np;++p2)
{
A.element(p1,p2) += imDerp[p1].x * imDerp[p2].x + imDerp[p1].y * imDerp[p2].y;
}
}
}
delete []imDerp;
}
else
{
// ground observations
std::vector<ossimGpt> gdDerp(np);
ossimGpt gd, resGd;
// loop on tie points
for (tit = theTPV.begin() ; tit != theTPV.end() ; ++tit)
{
//compute residue
gd = inverse((*tit)->tie);
residue(c++) = resGd.lon = ((*tit)->lon - gd.lon) * 100000.0;
residue(c++) = resGd.lat = ((*tit)->lat - gd.lat) * 100000.0 * cos(gd.lat / 180.0 * M_PI);
residue(c++) = resGd.hgt = (*tit)->hgt - gd.hgt; //TBD : normalize to meters?
//compute all image derivatives regarding parametres for the tie point position
for(int p=0;p<np;++p)
{
gdDerp[p] = getInverseDeriv( p , (*tit)->tie, pstep_scale);
}
//compute influence of tie point on all sytem elements
for(int p1=0;p1<np;++p1)
{
//proj residue: J * residue
projResidue.element(p1) += gdDerp[p1].lon * resGd.lon + gdDerp[p1].lat * resGd.lat + gdDerp[p1].hgt * resGd.hgt; //TBC
//normal matrix A = transpose(J)*J
for(int p2=p1;p2<np;++p2)
{
A.element(p1,p2) += gdDerp[p1].lon * gdDerp[p2].lon + gdDerp[p1].lat * gdDerp[p2].lat + gdDerp[p1].hgt * gdDerp[p2].hgt;
}
}
}
} //end of if (useImageObs)
}
double
ossimRpcProjection::optimizeFit(const ossimTieGptSet& tieSet, double* /* targetVariance */)
{
#if 1
//NOTE : ignore targetVariance
ossimRefPtr<ossimRpcSolver> solver = new ossimRpcSolver(false, false); //TBD : choices should be part of setupFromString
std::vector<ossimDpt> imagePoints;
std::vector<ossimGpt> groundPoints;
tieSet.getSlaveMasterPoints(imagePoints, groundPoints);
solver->solveCoefficients(imagePoints, groundPoints);
ossimRefPtr< ossimImageGeometry > optProj = solver->createRpcProjection();
if (!optProj)
{
ossimNotify(ossimNotifyLevel_FATAL) << "FATAL ossimRpcProjection::optimizeFit(): error when optimizing the RPC with given tie points"
<< std::endl;
return -1.0;
}
if(optProj->hasProjection())
{
ossimKeywordlist kwl;
optProj->getProjection()->saveState(kwl);
this->loadState(kwl);
}
return std::pow(solver->getRmsError(), 2); //variance in pixel^2
#else
// COPIED from ossimRpcProjection
//
//
//use a simple Levenberg-Marquardt non-linear optimization
//note : please limit the number of tie points
//
//INPUTS: requires Jacobian matrix (partial derivatives with regards to parameters)
//OUTPUTS: will also compute parameter covariance matrix
//
//TBD: use targetVariance!
int np = getNumberOfAdjustableParameters();
int nobs = tieSet.size();
//setup initail values
int iter=0;
int iter_max = 200;
double minResidue = 1e-10; //TBC
double minDelta = 1e-10; //TBC
//build Least Squares initial normal equation
// don't waste memory, add samples one at a time
NEWMAT::SymmetricMatrix A;
NEWMAT::ColumnVector residue;
NEWMAT::ColumnVector projResidue;
double deltap_scale = 1e-4; //step_Scale is 1.0 because we expect parameters to be between -1 and 1
buildNormalEquation(tieSet, A, residue, projResidue, deltap_scale);
double ki2=residue.SumSquare();
//get current adjustment (between -1 and 1 normally) and convert to ColumnVector
ossimAdjustmentInfo cadj;
getAdjustment(cadj);
std::vector< ossimAdjustableParameterInfo >& parmlist = cadj.getParameterList();
NEWMAT::ColumnVector cparm(np), nparm(np);
for(int n=0;n<np;++n)
{
cparm(n+1) = parmlist[n].getParameter();
}
double damping_speed = 2.0;
//find max diag element for A
double maxdiag=0.0;
for(int d=1;d<=np;++d) {
if (maxdiag < A(d,d)) maxdiag=A(d,d);
}
double damping = 1e-3 * maxdiag;
double olddamping = 0.0;
bool found = false;
//DEBUG TBR
cout<<"rms="<<sqrt(ki2/nobs)<<" ";
cout.flush();
while ( (!found) && (iter < iter_max) ) //non linear optimization loop
{
bool decrease = false;
do
{
//add damping update to normal matrix
for(int d=1;d<=np;++d) A(d,d) += damping - olddamping;
olddamping = damping;
NEWMAT::ColumnVector deltap = solveLeastSquares(A, projResidue);
if (deltap.NormFrobenius() <= minDelta)
{
found = true;
} else {
//update adjustment
nparm = cparm + deltap;
for(int n=0;n<np;++n)
{
setAdjustableParameter(n, nparm(n+1), false); //do not update now, wait
}
adjustableParametersChanged();
//check residue is reduced
NEWMAT::ColumnVector newresidue = getResidue(tieSet);
double newki2=newresidue.SumSquare();
double res_reduction = (ki2 - newki2) / (deltap.t()*(deltap*damping + projResidue)).AsScalar();
//DEBUG TBR
cout<<sqrt(newki2/nobs)<<" ";
cout.flush();
if (res_reduction > 0)
{
//accept new parms
cparm = nparm;
ki2=newki2;
deltap_scale = max(1e-15, deltap.NormInfinity()*1e-4);
buildNormalEquation(tieSet, A, residue, projResidue, deltap_scale);
olddamping = 0.0;
found = ( projResidue.NormInfinity() <= minResidue );
//update damping factor
damping *= std::max( 1.0/3.0, 1.0-std::pow((2.0*res_reduction-1.0),3));
damping_speed = 2.0;
decrease = true;
} else {
//cancel parameter update
for(int n=0;n<np;++n)
{
setAdjustableParameter(n, nparm(n+1), false); //do not update right now
}
adjustableParametersChanged();
damping *= damping_speed;
damping_speed *= 2.0;
}
}
} while (!decrease && !found);
++iter;
}
//DEBUG TBR
cout<<endl;
//compute parameter correlation
// use normal matrix inverse
//TBD
return ki2/nobs;
#endif
}
