void ossimAdjustableParameterInterface::eraseAdjustment(ossim_uint32 idx, bool notify)
{
if(!theAdjustmentList.size())
{
return;
}
if(theCurrentAdjustment == idx)
{
theAdjustmentList.erase(theAdjustmentList.begin() + theCurrentAdjustment);
if(theCurrentAdjustment >= theAdjustmentList.size())
{
if(theAdjustmentList.size() < 1)
{
theCurrentAdjustment = 0;
}
else
{
theCurrentAdjustment = (ossim_uint32)theAdjustmentList.size() - 1;
}
}
if(notify)
{
adjustableParametersChanged();
}
}
else if(idx < theAdjustmentList.size())
{
theAdjustmentList.erase(theAdjustmentList.begin() + idx);
if(theAdjustmentList.size() < 1)
{
theCurrentAdjustment = 0;
}
else
{
if(theCurrentAdjustment > idx)
{
--theCurrentAdjustment;
if(notify)
{
adjustableParametersChanged();
}
}
}
if(notify)
{
adjustableParametersChanged();
}
}
}
void ossimAdjustableParameterInterface::resetAdjustableParameters(bool notify)
{
if(!theAdjustmentList.size())
{
return;
}
ossim_uint32 saveCurrent = theCurrentAdjustment;
copyAdjustment();
initAdjustableParameters();
ossim_uint32 numberOfAdjustables = getNumberOfAdjustableParameters();
ossim_uint32 idx = 0;
for(idx = 0; idx < numberOfAdjustables; ++idx)
{
theAdjustmentList[saveCurrent].getParameterList()[idx].setParameter(theAdjustmentList[theAdjustmentList.size()-1].getParameterList()[idx].getParameter());
}
setCurrentAdjustment(saveCurrent);
eraseAdjustment((ossim_uint32)theAdjustmentList.size()-1, false);
if(notify)
{
adjustableParametersChanged();
}
}
void ossimAdjustableParameterInterface::addAdjustment(const ossimAdjustmentInfo& adj, bool notify)
{
theAdjustmentList.push_back(adj);
if(notify)
{
adjustableParametersChanged();
}
}
void rspfSFIMFusion::initialize()
{
rspfFusionCombiner::initialize();
if(!theIntensityConnection)
{
theLowPassFilter->disconnectAllInputs();
theHighPassFilter->disconnectAllInputs();
}
else
{
theLowPassFilter->connectMyInputTo(0, PTR_CAST(rspfConnectableObject,
theIntensityConnection->getObject()));
theHighPassFilter->connectMyInputTo(0, PTR_CAST(rspfConnectableObject,
theIntensityConnection->getObject()));
adjustableParametersChanged();
setFilters();
theLowPassFilter->initialize();
theHighPassFilter->initialize();
if(theAutoAdjustScales)
{
if(theInputConnection && theIntensityConnection)
{
rspfTypeNameVisitor visitor("rspfImageRenderer", true);
theInputConnection->accept(visitor);
rspfRefPtr<rspfConnectableObject> inputColor = visitor.getObjectAs<rspfConnectableObject>();
visitor.reset();
theIntensityConnection->accept(visitor);
rspfRefPtr<rspfConnectableObject> inputPan = visitor.getObjectAs<rspfConnectableObject>();
if(inputColor.valid()&&inputPan.valid())
{
rspfImageSource* inputColorSource = dynamic_cast<rspfImageSource*> (inputColor->getInput());
rspfImageSource* inputPanSource = dynamic_cast<rspfImageSource*> (inputPan->getInput());
if(inputColorSource&&inputPanSource)
{
rspfRefPtr<rspfImageGeometry> colorGeom = inputColorSource->getImageGeometry();
rspfRefPtr<rspfImageGeometry> intensityGeom = inputPanSource->getImageGeometry();
if(colorGeom.valid()&&intensityGeom.valid())
{
rspfDpt gsdIntensity = intensityGeom->getMetersPerPixel();
rspfDpt gsdColor = colorGeom->getMetersPerPixel();
if(!gsdColor.hasNans()&&!gsdIntensity.hasNans())
{
double scaleChange = gsdColor.length()/gsdIntensity.length();
if(scaleChange < 1.0) scaleChange = 1.0;
setParameterOffset(LOW_PASS_WIDTH_OFFSET,
scaleChange);
}
}
}
}
}
}
}
}
void ossimAdjustableParameterInterface::setAdjustment(ossim_uint32 idx, const ossimAdjustmentInfo& adj, bool notify)
{
if(idx < getNumberOfAdjustments())
{
theAdjustmentList[(int)idx] = adj;
if(notify)
{
adjustableParametersChanged();
}
}
}
void ossimAdjustableParameterInterface::setCurrentAdjustment(ossim_uint32 adjustmentIdx, bool notify)
{
if(adjustmentIdx < theAdjustmentList.size())
{
theCurrentAdjustment = adjustmentIdx;
if(notify)
{
adjustableParametersChanged();
}
}
}
bool rspfSFIMFusion::loadState(const rspfKeywordlist& kwl,
const char* prefix)
{
rspfFusionCombiner::loadState(kwl, prefix);
loadAdjustments(kwl, prefix);
adjustableParametersChanged();
rspfString autoAdjustScales = kwl.find(prefix, "auto_adjust_scales");
if(!autoAdjustScales.empty())
{
theAutoAdjustScales = autoAdjustScales.toBool();
}
return true;
}
void ossimAdjustableParameterInterface::setParameterSigma(ossim_uint32 idx, double value, bool notify)
{
if(!theAdjustmentList.size())
{
return;
}
if(idx < theAdjustmentList[theCurrentAdjustment].getNumberOfAdjustableParameters())
{
theAdjustmentList[theCurrentAdjustment].getParameterList()[idx].setSigma(value);
if(notify)
{
adjustableParametersChanged();
}
}
}
void ossimAdjustableParameterInterface::copyAdjustment(ossim_uint32 idx, bool notify)
{
if(!theAdjustmentList.size())
{
return;
}
if(idx < theAdjustmentList.size())
{
theAdjustmentList.push_back(theAdjustmentList[idx]);
if(idx == theCurrentAdjustment)
{
theCurrentAdjustment = (ossim_uint32)theAdjustmentList.size() - 1;
}
if(notify)
{
adjustableParametersChanged();
}
}
}
void ossimAdjustableParameterInterface::setParameterOffset(ossim_uint32 idx,
ossim_float64 value,
bool notify)
{
// double center = getParameterCenter(idx);
// double sigma = getParameterSigma(idx);
// double minValue = center - sigma;
// double maxValue = center + sigma;
// double x = 0.0;
// if(sigma != 0.0)
// {
// x = (value - center)/sigma;
// value = center + x*sigma;
// if(value < minValue)
// {
// x = -1;
// }
// else if(value >maxValue)
// {
// x = 1.0;
// }
// setAdjustableParameter(idx, x, false);
// }
if(!theAdjustmentList.size())
{
return;
}
if(idx < theAdjustmentList[theCurrentAdjustment].getNumberOfAdjustableParameters())
{
theAdjustmentList[theCurrentAdjustment].getParameterList()[idx].setOffset(value);
if(notify)
{
adjustableParametersChanged();
}
}
}
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
}
