void rspfLandSatModel::lineSampleHeightToWorld(const rspfDpt& image_point,
const double& height,
rspfGpt& gpt) const
{
if (traceExec()) rspfNotify(rspfNotifyLevel_DEBUG) << "DEBUG rspfLandSatModel::lineSampleHeightToWorld: entering..." << std::endl;
#if 0
if (!insideImage(image_point))
{
gpt = extrapolate(image_point, height);
if (traceExec()) rspfNotify(rspfNotifyLevel_DEBUG) << "DEBUG rspfLandSatModel::lineSampleHeightToWorld: returning..." << std::endl;
return;
}
#endif
rspfEcefRay imaging_ray;
imagingRay(image_point, imaging_ray);
//rspfEcefPoint Pecf (imaging_ray.intersectAboveEarthEllipsoid(height));
//gpt = rspfGpt(Pecf);
if (m_proj==NULL) {
rspfEcefPoint Pecf (imaging_ray.intersectAboveEarthEllipsoid(height));
gpt = rspfGpt(Pecf);
}
else
{
rspfEcefPoint Pecf (imaging_ray.intersectAboveEarthEllipsoid(height,m_proj->getDatum()));
gpt = rspfGpt(Pecf,m_proj->getDatum());
}
}
void rspfBuckeyeSensor::lineSampleHeightToWorld(const rspfDpt& image_point,
const double& heightEllipsoid,
rspfGpt& worldPoint) const
{
if (!insideImage(image_point))
{
worldPoint.makeNan();
worldPoint = extrapolate(image_point, heightEllipsoid);
}
else
{
rspfEcefRay ray;
imagingRay(image_point, ray);
rspfEcefPoint Pecf (ray.intersectAboveEarthEllipsoid(heightEllipsoid));
worldPoint = rspfGpt(Pecf);
}
}
void ossimApplanixUtmModel::lineSampleHeightToWorld(const ossimDpt& image_point,
const double& heightEllipsoid,
ossimGpt& worldPoint) const
{
if (!insideImage(image_point))
{
worldPoint.makeNan();
// worldPoint = extrapolate(image_point, heightEllipsoid);
}
else
{
//***
// First establish imaging ray from image point:
//***
ossimEcefRay ray;
imagingRay(image_point, ray);
ossimEcefPoint Pecf (ray.intersectAboveEarthEllipsoid(heightEllipsoid));
worldPoint = ossimGpt(Pecf);
}
}
void ossimBuckeyeSensor::lineSampleHeightToWorld(const ossimDpt& image_point,
const double& heightEllipsoid,
ossimGpt& worldPoint) const
{
if (traceDebug()) ossimNotify(ossimNotifyLevel_DEBUG) << "DEBUG ossimBuckeyeSensor::lineSampleHeightToWorld: entering..." << std::endl;
if (!insideImage(image_point))
{
worldPoint.makeNan();
}
else
{
//***
// First establish imaging ray from image point:
//***
ossimEcefRay ray;
imagingRay(image_point, ray);
ossimEcefPoint Pecf (ray.intersectAboveEarthEllipsoid(heightEllipsoid));
worldPoint = ossimGpt(Pecf);
}
if (traceDebug()) ossimNotify(ossimNotifyLevel_DEBUG) << "DEBUG ossimBuckeyeSensor::lineSampleHeightToWorld: returning..." << std::endl;
}
void ossimApplanixUtmModel::lineSampleToWorld(const ossimDpt& image_point,
ossimGpt& gpt) const
{
if (traceDebug()) ossimNotify(ossimNotifyLevel_DEBUG) << "DEBUG ossimApplanixEcefModel::lineSampleToWorld:entering..." << std::endl;
if(image_point.hasNans())
{
gpt.makeNan();
return;
}
//***
// Extrapolate if image point is outside image:
//***
if (!insideImage(image_point))
{
gpt.makeNan();
// gpt = extrapolate(image_point);
return;
}
//***
// Determine imaging ray and invoke elevation source object's services to
// intersect ray with terrain model:
//***
ossimEcefRay ray;
imagingRay(image_point, ray);
ossimElevManager::instance()->intersectRay(ray, gpt);
if (traceDebug())
{
ossimNotify(ossimNotifyLevel_DEBUG) << "image_point = " << image_point << std::endl;
ossimNotify(ossimNotifyLevel_DEBUG) << "ray = " << ray << std::endl;
ossimNotify(ossimNotifyLevel_DEBUG) << "gpt = " << gpt << std::endl;
}
if (traceDebug()) ossimNotify(ossimNotifyLevel_DEBUG) << "DEBUG ossimSensorModel::lineSampleToWorld: returning..." << std::endl;
return;
}
//*****************************************************************************
// METHOD: ossimRpcModel::lineSampleHeightToWorld()
//
// Performs reverse projection of image line/sample to ground point.
// The imaging ray is intersected with a level plane at height = elev.
//
// NOTE: U = line, V = sample -- this differs from the convention.
//
//*****************************************************************************
void ossimRpcModel::lineSampleHeightToWorld(const ossimDpt& image_point,
const double& ellHeight,
ossimGpt& gpt) const
{
// if (traceExec()) ossimNotify(ossimNotifyLevel_DEBUG) << "DEBUG ossimRpcModel::lineSampleHeightToWorld: entering..." << std::endl;
//***
// Extrapolate if point is outside image:
//***
if (!insideImage(image_point))
{
// gpt = extrapolate(image_point, ellHeight);
// if (traceExec()) CLOG << "returning..." << endl;
gpt.makeNan();
return;
}
//***
// Constants for convergence tests:
//***
static const int MAX_NUM_ITERATIONS = 10;
static const double CONVERGENCE_EPSILON = 0.1; // pixels
//***
// The image point must be adjusted by the adjustable parameters as well
// as the scale and offsets given as part of the RPC param normalization.
//
// NOTE: U = line, V = sample
//***
double U = (image_point.y-theLineOffset - theIntrackOffset) / (theLineScale+theIntrackScale);
double V = (image_point.x-theSampOffset - theCrtrackOffset) / (theSampScale+theCrtrackScale);
//***
// Rotate the normalized U, V by the map rotation error (adjustable param):
//***
double U_rot = theCosMapRot*U - theSinMapRot*V;
double V_rot = theSinMapRot*U + theCosMapRot*V;
U = U_rot;
V = V_rot;
// now apply adjust intrack and cross track
//***
// Initialize quantities to be used in the iteration for ground point:
//***
double nlat = 0.0; // normalized latitude
double nlon = 0.0; // normalized longitude
double nhgt;
if(ossim::isnan(ellHeight))
{
nhgt = (theHgtScale - theHgtOffset) / theHgtScale; // norm height
}
else
{
nhgt = (ellHeight - theHgtOffset) / theHgtScale; // norm height
}
double epsilonU = CONVERGENCE_EPSILON/(theLineScale+theIntrackScale);
double epsilonV = CONVERGENCE_EPSILON/(theSampScale+theCrtrackScale);
int iteration = 0;
//***
// Declare variables only once outside the loop. These include:
// * polynomials (numerators Pu, Pv, and denominators Qu, Qv),
// * partial derivatives of polynomials wrt X, Y,
// * computed normalized image point: Uc, Vc,
// * residuals of normalized image point: deltaU, deltaV,
// * partial derivatives of Uc and Vc wrt X, Y,
// * corrections to normalized lat, lon: deltaLat, deltaLon.
//***
double Pu, Qu, Pv, Qv;
double dPu_dLat, dQu_dLat, dPv_dLat, dQv_dLat;
double dPu_dLon, dQu_dLon, dPv_dLon, dQv_dLon;
double Uc, Vc;
double deltaU, deltaV;
double dU_dLat, dU_dLon, dV_dLat, dV_dLon, W;
double deltaLat, deltaLon;
//***
// Now iterate until the computed Uc, Vc is within epsilon of the desired
// image point U, V:
//***
do
{
//***
// Calculate the normalized line and sample Uc, Vc as ratio of
// polynomials Pu, Qu and Pv, Qv:
//***
Pu = polynomial(nlat, nlon, nhgt, theLineNumCoef);
Qu = polynomial(nlat, nlon, nhgt, theLineDenCoef);
Pv = polynomial(nlat, nlon, nhgt, theSampNumCoef);
Qv = polynomial(nlat, nlon, nhgt, theSampDenCoef);
Uc = Pu/Qu;
Vc = Pv/Qv;
//***
// Compute residuals between desired and computed line, sample:
//***
deltaU = U - Uc;
deltaV = V - Vc;
//***
// Check for convergence and skip re-linearization if converged:
//***
if ((fabs(deltaU) > epsilonU) || (fabs(deltaV) > epsilonV))
{
//***
// Analytically compute the partials of each polynomial wrt lat, lon:
//***
dPu_dLat = dPoly_dLat(nlat, nlon, nhgt, theLineNumCoef);
dQu_dLat = dPoly_dLat(nlat, nlon, nhgt, theLineDenCoef);
dPv_dLat = dPoly_dLat(nlat, nlon, nhgt, theSampNumCoef);
dQv_dLat = dPoly_dLat(nlat, nlon, nhgt, theSampDenCoef);
dPu_dLon = dPoly_dLon(nlat, nlon, nhgt, theLineNumCoef);
dQu_dLon = dPoly_dLon(nlat, nlon, nhgt, theLineDenCoef);
dPv_dLon = dPoly_dLon(nlat, nlon, nhgt, theSampNumCoef);
dQv_dLon = dPoly_dLon(nlat, nlon, nhgt, theSampDenCoef);
//***
// Analytically compute partials of quotients U and V wrt lat, lon:
//***
dU_dLat = (Qu*dPu_dLat - Pu*dQu_dLat)/(Qu*Qu);
dU_dLon = (Qu*dPu_dLon - Pu*dQu_dLon)/(Qu*Qu);
dV_dLat = (Qv*dPv_dLat - Pv*dQv_dLat)/(Qv*Qv);
dV_dLon = (Qv*dPv_dLon - Pv*dQv_dLon)/(Qv*Qv);
W = dU_dLon*dV_dLat - dU_dLat*dV_dLon;
//***
// Now compute the corrections to normalized lat, lon:
//***
deltaLat = (dU_dLon*deltaV - dV_dLon*deltaU) / W;
deltaLon = (dV_dLat*deltaU - dU_dLat*deltaV) / W;
nlat += deltaLat;
nlon += deltaLon;
}
//double h = ossimElevManager::instance()->getHeightAboveEllipsoid(ossimGpt(nlat, nlon));
// if(!ossim::isnan(h))
// {
// nhgt = h;
// }
iteration++;
} while (((fabs(deltaU)>epsilonU) || (fabs(deltaV)>epsilonV))
&& (iteration < MAX_NUM_ITERATIONS));
//***
// Test for exceeding allowed number of iterations. Flag error if so:
//***
if (iteration == MAX_NUM_ITERATIONS)
{
ossimNotify(ossimNotifyLevel_WARN) << "WARNING ossimRpcModel::lineSampleHeightToWorld: \nMax number of iterations reached in ground point "
<< "solution. Results are inaccurate." << endl;
}
//***
// Now un-normalize the ground point lat, lon and establish return quantity:
//***
gpt.lat = nlat*theLatScale + theLatOffset;
gpt.lon = nlon*theLonScale + theLonOffset;
gpt.hgt = ellHeight;
}
