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;
}
void
nlf1(int mode, int ndim, NEWMAT::ColumnVector const & x, OPTPP::real & fx, NEWMAT::ColumnVector & gx, int & result, void * data)
{
result = OPTPP::NLPNoOp;
if (mode & OPTPP::NLPFunction)
nlf0(ndim, x, fx, result, data);
if (mode & OPTPP::NLPGradient)
{
ScalarFunction const * func = static_cast<ScalarFunction *>(data);
AnalyticD1ScalarFunction const * func1 = dynamic_cast<AnalyticD1ScalarFunction const *>(func);
alwaysAssertM(func1, "OPTPPNumericalOptimizer: Function does not have analytical first derivative");
gx.ReSize(ndim);
func1->gradientAt(x.data(), gx.data());
result |= OPTPP::NLPGradient;
}
}
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)
}