//*****************************************************************************
// METHOD: ossimRpcModel::lineSampleToWorld()
//
// Overrides base class implementation. Performs DEM intersection.
//*****************************************************************************
void ossimRpcModel::lineSampleToWorld(const ossimDpt& imagePoint,
ossimGpt& worldPoint) const
{
//---
// Under debate... (drb 20130610)
// this seems to be more accurate for the round trip
//---
#if 0
if(!imagePoint.hasNans())
{
lineSampleHeightToWorld(imagePoint,
worldPoint.height(),
worldPoint);
}
else
{
worldPoint.makeNan();
}
#else
if(!imagePoint.hasNans())
{
ossimEcefRay ray;
imagingRay(imagePoint, ray);
ossimElevManager::instance()->intersectRay(ray, worldPoint);
}
else
{
worldPoint.makeNan();
}
#endif
}
void ossimBuckeyeSensor::lineSampleToWorld(const ossimDpt& image_point,
ossimGpt& gpt) const
{
if (traceDebug()) ossimNotify(ossimNotifyLevel_DEBUG) << "DEBUG ossimBuckeyeSensor::lineSampleToWorld:entering..." << std::endl;
if(image_point.hasNans())
{
gpt.makeNan();
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 ossimBuckeyeSensor::lineSampleToWorld: returning..." << std::endl;
return;
}
void ossimIvtGeomXform::imageToGround(const ossimDpt& ipt, ossimGpt& gpt)
{
gpt.makeNan();
if(m_geom.valid())
{
m_geom->localToWorld(ipt, gpt);
}
}
void ossimIvtGeomXform::viewToGround(const ossimDpt& viewPt, ossimGpt& gpt)
{
ossimDpt ipt;
gpt.makeNan();
viewToImage(viewPt, ipt);
if(!ipt.hasNans())
{
imageToGround(ipt, gpt);
}
}
void ossimRpcProjection::lineSampleToWorld(const ossimDpt& imagePoint,
ossimGpt& worldPoint) const
{
if(!imagePoint.hasNans())
{
lineSampleHeightToWorld(imagePoint,
worldPoint.height(),
worldPoint);
}
else
{
worldPoint.makeNan();
}
}
void ossimCsmSensorModel::lineSampleToWorld(const ossimDpt& image_point,
ossimGpt& gpt) const
{
if(image_point.hasNans())
{
gpt.makeNan();
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);
}
void ossimTileMapModel::lineSampleHeightToWorld(const ossimDpt& image_point,
const double& /* height */,
ossimGpt& gpt) const
{
if(!image_point.hasNans())
{
gpt.lon = static_cast<double>(image_point.samp)/(1 << qDepth)/256 *360.0-180.0;
double y = static_cast<double>(image_point.line)/(1 << qDepth)/256;
double ex = exp(4*M_PI*(y-0.5));
gpt.lat = -180.0/M_PI*asin((ex-1)/(ex+1));
}
else
{
gpt.makeNan();
}
return;
}
//*****************************************************************************
// METHOD: ossimRpcModel::lineSampleToWorld()
//
// Overrides base class implementation. Performs DEM intersection.
//*****************************************************************************
void ossimRpcModel::lineSampleToWorld(const ossimDpt& imagePoint,
ossimGpt& worldPoint) const
{
//lineSampleHeightToWorld(imagePoint, theHgtOffset, worldPoint);
//worldPoint.hgt = ossimElevManager::instance()->getHeightAboveEllipsoid(worldPoint);
//if(!worldPoint.hasNans()) return;
if(!imagePoint.hasNans())
{
ossimEcefRay ray;
imagingRay(imagePoint, ray);
if (m_proj) worldPoint.datum(m_proj->getDatum()); //loong
ossimElevManager::instance()->intersectRay(ray, worldPoint);
}
else
{
worldPoint.makeNan();
}
}
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 ossimSpectraboticsRedEdgeModel::lineSampleToWorld(const ossimDpt& image_point,
ossimGpt& gpt) const
{
if (traceDebug()) ossimNotify(ossimNotifyLevel_DEBUG) << "DEBUG ossimSpectraboticsRedEdgeModel::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: intersectRay()
//
// Service method for intersecting a ray with the elevation surface to
// arrive at a ground point. The ray is expected to originate ABOVE the
// surface and pointing down.
//
// NOTE: the gpt argument is expected to be initialized with the desired
// datum, including ellipsoid, for the proper intersection point to be
// computed.
//
// LIMITATION: This release supports only single valued solutions, i.e., it
// is possible a ray passing through one side of a mountain and out the other
// will return an intersection with the far side. Eventually, a more robust
// algorithm will be employed.
//
//*****************************************************************************
bool ossimElevSource::intersectRay(const ossimEcefRay& ray, ossimGpt& gpt, double defaultElevValue)
{
if (traceExec()) ossimNotify(ossimNotifyLevel_DEBUG) << "DEBUG ossimElevSource::intersectRay: entering..." << std::endl;
static const double CONVERGENCE_THRESHOLD = 0.001; // meters
static const int MAX_NUM_ITERATIONS = 50;
double h_ellips; // height above ellipsoid
bool intersected;
ossimEcefPoint prev_intersect_pt (ray.origin());
ossimEcefPoint new_intersect_pt;
double distance;
bool done = false;
int iteration_count = 0;
if(ray.hasNans())
{
gpt.makeNan();
return false;
}
//***
// Set the initial guess for horizontal intersect position as the ray's
// origin, and establish the datum and ellipsoid:
//***
const ossimDatum* datum = gpt.datum();
const ossimEllipsoid* ellipsoid = datum->ellipsoid();
// double lat, lon, h;
// ellipsoid->XYZToLatLonHeight(ray.origin().x(),
// ray.origin().y(),
// ray.origin().z(),
// lat, lon, h);
// ossimGpt nadirGpt(lat, lon, h);
// std::cout << "nadir pt = " << nadirGpt << std::endl;
gpt = ossimGpt(prev_intersect_pt, datum);
//
// Loop to iterate on ray intersection with variable elevation surface:
//
do
{
//
// Intersect ray with ellipsoid inflated by h_ellips:
//
h_ellips = getHeightAboveEllipsoid(gpt);
if ( ossim::isnan(h_ellips) ) h_ellips = defaultElevValue;
intersected = ellipsoid->nearestIntersection(ray,
h_ellips,
new_intersect_pt);
if (!intersected)
{
//
// No intersection (looking over the horizon), so set ground point
// to NaNs:
//
gpt.makeNan();
done = true;
}
else
{
//
// Assign the ground point to the latest iteration's intersection
// point:
//
gpt = ossimGpt(new_intersect_pt, datum);
//
// Determine if convergence achieved:
//
distance = (new_intersect_pt - prev_intersect_pt).magnitude();
if (distance < CONVERGENCE_THRESHOLD)
done = true;
else
{
prev_intersect_pt = new_intersect_pt;
}
}
iteration_count++;
} while ((!done) && (iteration_count < MAX_NUM_ITERATIONS));
if (iteration_count == MAX_NUM_ITERATIONS)
{
if(traceDebug())
{
ossimNotify(ossimNotifyLevel_WARN) << "WARNING ossimElevSource::intersectRay: Max number of iterations reached solving for ground "
<< "point. Result is probably inaccurate." << std::endl;
}
}
if (traceExec()) ossimNotify(ossimNotifyLevel_DEBUG) << "DEBUG ossimElevSource::intersectRay: returning..." << std::endl;
return intersected;
}
//*****************************************************************************
// 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;
}
void ossimEquDistCylProjection::lineSampleHeightToWorld(const ossimDpt &lineSample,
const double& hgtEllipsoid,
ossimGpt& gpt)const
{
//
// make sure that the passed in lineSample is good and
// check to make sure our easting northing is good so
// we can compute the line sample.
//
//
if(lineSample.hasNans())
{
gpt.makeNan();
return;
}
if(theModelTransformUnitType != OSSIM_UNIT_UNKNOWN)
{
ossimMapProjection::lineSampleHeightToWorld(lineSample, hgtEllipsoid, gpt);
return;
}
else
{
if(theUlEastingNorthing.hasNans())
{
gpt.makeNan();
return;
}
ossimDpt eastingNorthing;
eastingNorthing = (theUlEastingNorthing);
eastingNorthing.x += (lineSample.x*theMetersPerPixel.x);
//
// Note: the Northing is positive up. In image space
// the positive axis is down so we must multiply by
// -1
//
eastingNorthing.y += (-lineSample.y*theMetersPerPixel.y);
//
// now invert the meters into a ground point.
//
gpt = inverse(eastingNorthing);
gpt.datum(theDatum);
if(gpt.isLatNan() && gpt.isLonNan())
{
gpt.makeNan();
}
else
{
// Finally assign the specified height:
gpt.hgt = hgtEllipsoid;
}
}
if(theElevationLookupFlag)
{
gpt.hgt = ossimElevManager::instance()->getHeightAboveEllipsoid(gpt);
}
}
