ossimGpt ossimNadconNarDatum::shift(const ossimGpt &aPt)const
{
const ossimDatum* datum = aPt.datum();
ossimString code = datum->code();
ossimString subCode(code.begin(),
code.begin() + 3);
if(subCode == "NAR")
{
return aPt;
}
else
{
if(subCode == "NAS")
{
checkGrid(aPt);
if(!theLatGrid.getFileOkFlag()||
!theLonGrid.getFileOkFlag())
{
return ossimThreeParamDatum::shift(aPt);
}
double shiftLat = theLatGrid.getShiftAtLatLon(aPt.latd(), aPt.lond());
double shiftLon = theLonGrid.getShiftAtLatLon(aPt.latd(), aPt.lond());
if( (ossim::isnan(shiftLat)) || (ossim::isnan(shiftLon)) )
{
return ossimThreeParamDatum::shift(aPt);
}
else
{
// Note the shifts are stored in the file
// as seconds.
//
// convert the seconds into decimal degrees.
//
shiftLat /= 3600.0;
shiftLon /= 3600.0;
return ossimGpt(aPt.latd() + shiftLat,
aPt.lond() - shiftLon,
aPt.height(),
this);
}
}
else
{
return ossimThreeParamDatum::shift(aPt);
}
}
return ossimThreeParamDatum::shift(aPt);
}
ossimGpt ossimThreeParamDatum::shift(const ossimGpt &aPt)const
{
const ossimDatum *aDatum = aPt.datum();
if( code() == aDatum->code())
{
return ossimGpt(aPt.latd(), aPt.lond(), aPt.height(), this);
}
if(aDatum)
{
return shiftFromWgs84(aDatum->shiftToWgs84(aPt));
}
return aPt;
}
//*****************************************************************************
// 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();
}
}
ossimDpt ossimVanDerGrintenProjection::forward(const ossimGpt &latLon)const
{
double easting = 0.0;
double northing = 0.0;
ossimGpt gpt = latLon;
if (theDatum)
{
if (theDatum->code() != latLon.datum()->code())
{
gpt.changeDatum(theDatum); // Shift to our datum.
}
}
Convert_Geodetic_To_Van_der_Grinten(gpt.latr(),
gpt.lonr(),
&easting,
&northing);
return ossimDpt(easting, northing);
}
ossimDpt ossimTransCylEquAreaProjection::forward(const ossimGpt &latLon)const
{
double easting = 0.0;
double northing = 0.0;
ossimGpt gpt = latLon;
if (theDatum)
{
if (theDatum->code() != latLon.datum()->code())
{
gpt.changeDatum(theDatum); // Shift to our datum.
}
}
Convert_Geodetic_To_Trans_Cyl_Eq_Area(gpt.latr(),
gpt.lonr(),
&easting,
&northing);
return ossimDpt(easting, northing);
}
//*****************************************************************************
// 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: ossimSarModel::getForwardDeriv()
//
// Compute partials of samp/line WRT to ground.
//
//*****************************************************************************
ossimDpt ossimRpcModel::getForwardDeriv(int derivMode,
const ossimGpt& pos,
double h)
{
// If derivMode (parmIdx) >= 0 call base class version
// for "adjustable parameters"
if (derivMode >= 0)
{
return ossimSensorModel::getForwardDeriv(derivMode, pos, h);
}
// Use alternative derivMode definitions
else
{
ossimDpt returnData;
//******************************************
// OBS_INIT mode
// [1]
// [2]
// Note: In this mode, pos is used to pass
// in the (s,l) observations.
//******************************************
if (derivMode==OBS_INIT)
{
// Image coordinates
ossimDpt obs;
obs.samp = pos.latd();
obs.line = pos.lond();
theObs = obs;
}
//******************************************
// EVALUATE mode
// [1] evaluate & save partials, residuals
// [2] return residuals
//******************************************
else if (derivMode==EVALUATE)
{
//***
// Normalize the lat, lon, hgt:
//***
double nlat = (pos.lat - theLatOffset) / theLatScale;
double nlon = (pos.lon - theLonOffset) / theLonScale;
double nhgt;
if( ossim::isnan(pos.hgt) )
{
nhgt = (theHgtScale - theHgtOffset) / theHgtScale;
}
else
{
nhgt = (pos.hgt - theHgtOffset) / theHgtScale;
}
//***
// Compute the normalized line (Un) and sample (Vn):
//***
double Pu = polynomial(nlat, nlon, nhgt, theLineNumCoef);
double Qu = polynomial(nlat, nlon, nhgt, theLineDenCoef);
double Pv = polynomial(nlat, nlon, nhgt, theSampNumCoef);
double Qv = polynomial(nlat, nlon, nhgt, theSampDenCoef);
double Un = Pu / Qu;
double Vn = Pv / Qv;
//***
// Compute the actual line (U) and sample (V):
//***
double U = Un*theLineScale + theLineOffset;
double V = Vn*theSampScale + theSampOffset;
//***
// Compute the partials of each polynomial wrt lat, lon, hgt
//***
double dPu_dLat, dQu_dLat, dPv_dLat, dQv_dLat;
double dPu_dLon, dQu_dLon, dPv_dLon, dQv_dLon;
double dPu_dHgt, dQu_dHgt, dPv_dHgt, dQv_dHgt;
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);
dPu_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theLineNumCoef);
dQu_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theLineDenCoef);
dPv_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theSampNumCoef);
dQv_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theSampDenCoef);
//***
// Compute partials of quotients U and V wrt lat, lon, hgt
//***
double dU_dLat, dU_dLon, dU_dHgt, dV_dLat, dV_dLon, dV_dHgt;
dU_dLat = (Qu*dPu_dLat - Pu*dQu_dLat)/(Qu*Qu);
dU_dLon = (Qu*dPu_dLon - Pu*dQu_dLon)/(Qu*Qu);
dU_dHgt = (Qu*dPu_dHgt - Pu*dQu_dHgt)/(Qu*Qu);
dV_dLat = (Qv*dPv_dLat - Pv*dQv_dLat)/(Qv*Qv);
dV_dLon = (Qv*dPv_dLon - Pv*dQv_dLon)/(Qv*Qv);
dV_dHgt = (Qv*dPv_dHgt - Pv*dQv_dHgt)/(Qv*Qv);
//***
// Apply necessary scale factors
//***
dU_dLat *= theLineScale/theLatScale;
dU_dLon *= theLineScale/theLonScale;
dU_dHgt *= theLineScale/theHgtScale;
dV_dLat *= theSampScale/theLatScale;
dV_dLon *= theSampScale/theLonScale;
dV_dHgt *= theSampScale/theHgtScale;
dU_dLat *= DEG_PER_RAD;
dU_dLon *= DEG_PER_RAD;
dV_dLat *= DEG_PER_RAD;
dV_dLon *= DEG_PER_RAD;
// Save the partials referenced to ECF
ossimEcefPoint location(pos);
NEWMAT::Matrix jMat(3,3);
pos.datum()->ellipsoid()->jacobianWrtEcef(location, jMat);
// Line
theParWRTx.u = dU_dLat*jMat(1,1)+dU_dLon*jMat(2,1)+dU_dHgt*jMat(3,1);
theParWRTy.u = dU_dLat*jMat(1,2)+dU_dLon*jMat(2,2)+dU_dHgt*jMat(3,2);
theParWRTz.u = dU_dLat*jMat(1,3)+dU_dLon*jMat(2,3)+dU_dHgt*jMat(3,3);
// Samp
theParWRTx.v = dV_dLat*jMat(1,1)+dV_dLon*jMat(2,1)+dV_dHgt*jMat(3,1);
theParWRTy.v = dV_dLat*jMat(1,2)+dV_dLon*jMat(2,2)+dV_dHgt*jMat(3,2);
theParWRTz.v = dV_dLat*jMat(1,3)+dV_dLon*jMat(2,3)+dV_dHgt*jMat(3,3);
// Residuals
ossimDpt resid(theObs.samp-V, theObs.line-U);
returnData = resid;
}
//******************************************
// P_WRT_X, P_WRT_Y, P_WRT_Z modes
// [1] 3 separate calls required
// [2] return 3 sets of partials
//******************************************
else if (derivMode==P_WRT_X)
{
returnData = theParWRTx;
}
else if (derivMode==P_WRT_Y)
{
returnData = theParWRTy;
}
else
{
returnData = theParWRTz;
}
return returnData;
}
}
ossimDpt ossimRpcModel::getForwardDeriv(int derivMode,
const ossimGpt& pos,
double h)
{
if (derivMode >= 0)
{
return ossimSensorModel::getForwardDeriv(derivMode, pos, h);
}
else
{
ossimDpt returnData;
if (derivMode==OBS_INIT)
{
ossimDpt obs;
obs.samp = pos.latd();
obs.line = pos.lond();
theObs = obs;
}
else if (derivMode==EVALUATE)
{
double nlat = (pos.lat - theLatOffset) / theLatScale;
double nlon = (pos.lon - theLonOffset) / theLonScale;
double nhgt;
if( ossim::isnan(pos.hgt) )
{
nhgt = (theHgtScale - theHgtOffset) / theHgtScale;
}
else
{
nhgt = (pos.hgt - theHgtOffset) / theHgtScale;
}
double Pu = polynomial(nlat, nlon, nhgt, theLineNumCoef);
double Qu = polynomial(nlat, nlon, nhgt, theLineDenCoef);
double Pv = polynomial(nlat, nlon, nhgt, theSampNumCoef);
double Qv = polynomial(nlat, nlon, nhgt, theSampDenCoef);
double Un = Pu / Qu;
double Vn = Pv / Qv;
double U = Un*theLineScale + theLineOffset;
double V = Vn*theSampScale + theSampOffset;
double dPu_dLat, dQu_dLat, dPv_dLat, dQv_dLat;
double dPu_dLon, dQu_dLon, dPv_dLon, dQv_dLon;
double dPu_dHgt, dQu_dHgt, dPv_dHgt, dQv_dHgt;
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);
dPu_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theLineNumCoef);
dQu_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theLineDenCoef);
dPv_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theSampNumCoef);
dQv_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theSampDenCoef);
double dU_dLat, dU_dLon, dU_dHgt, dV_dLat, dV_dLon, dV_dHgt;
dU_dLat = (Qu*dPu_dLat - Pu*dQu_dLat)/(Qu*Qu);
dU_dLon = (Qu*dPu_dLon - Pu*dQu_dLon)/(Qu*Qu);
dU_dHgt = (Qu*dPu_dHgt - Pu*dQu_dHgt)/(Qu*Qu);
dV_dLat = (Qv*dPv_dLat - Pv*dQv_dLat)/(Qv*Qv);
dV_dLon = (Qv*dPv_dLon - Pv*dQv_dLon)/(Qv*Qv);
dV_dHgt = (Qv*dPv_dHgt - Pv*dQv_dHgt)/(Qv*Qv);
dU_dLat *= theLineScale/theLatScale;
dU_dLon *= theLineScale/theLonScale;
dU_dHgt *= theLineScale/theHgtScale;
dV_dLat *= theSampScale/theLatScale;
dV_dLon *= theSampScale/theLonScale;
dV_dHgt *= theSampScale/theHgtScale;
dU_dLat *= DEG_PER_RAD;
dU_dLon *= DEG_PER_RAD;
dV_dLat *= DEG_PER_RAD;
dV_dLon *= DEG_PER_RAD;
ossimEcefPoint location(pos);
NEWMAT::Matrix jMat(3,3);
pos.datum()->ellipsoid()->jacobianWrtEcef(location, jMat);
theParWRTx.u = dU_dLat*jMat(1,1)+dU_dLon*jMat(2,1)+dU_dHgt*jMat(3,1);
theParWRTy.u = dU_dLat*jMat(1,2)+dU_dLon*jMat(2,2)+dU_dHgt*jMat(3,2);
theParWRTz.u = dU_dLat*jMat(1,3)+dU_dLon*jMat(2,3)+dU_dHgt*jMat(3,3);
theParWRTx.v = dV_dLat*jMat(1,1)+dV_dLon*jMat(2,1)+dV_dHgt*jMat(3,1);
theParWRTy.v = dV_dLat*jMat(1,2)+dV_dLon*jMat(2,2)+dV_dHgt*jMat(3,2);
theParWRTz.v = dV_dLat*jMat(1,3)+dV_dLon*jMat(2,3)+dV_dHgt*jMat(3,3);
ossimDpt resid(theObs.samp-V, theObs.line-U);
returnData = resid;
}
else if (derivMode==P_WRT_X)
{
returnData = theParWRTx;
}
else if (derivMode==P_WRT_Y)
{
returnData = theParWRTy;
}
else
{
returnData = theParWRTz;
}
return returnData;
}
}
ossimUsgsQuad::ossimUsgsQuad(const ossimGpt& lrGpt)
:
theQuadLowerRightCorner(0.0, 0.0, 0.0, lrGpt.datum()),
theQuarterQuadLowerRightCorner(0.0, 0.0, 0.0, lrGpt.datum())
// thePaddingInDegrees(0.0, 0.0),
{
const char* MODULE = "ImgOutput::quadBasename";
//***
// NOTE:
// This method computes a file name from the lower right corner of the
// image. The format is derived from USGS Digital Raster Graphic(DRG)
// program for standardized data set names for DRG products. Due to
// customer requirements there is one deviation: The first digit of the
// name is converted to a letter with 1 being = A, 2 being = B,
// 3 being = C, ....
// This was done to allow the name to be used on a pc.
//***
const double QUAD_SIZE_IN_DEGREES = 0.125;
ossimString baseString;
int tmpInt;
double tmpDbl;
char quadChar = 'A';
char quadNum = '1';
ostringstream os;
os << setiosflags(ios::right)
<< setiosflags(ios::fixed)
<< setfill('0');
tmpInt = static_cast<int>(abs(lrGpt.lat)); // Cast away the fractional part.
os << tmpInt; // latitude
//***
// Get the quadrant charactor in the latitude direction. (A - H)
//***
tmpDbl = fabs(lrGpt.lat) - (double)tmpInt;
quadChar += static_cast<int>(tmpDbl / QUAD_SIZE_IN_DEGREES);
tmpInt = static_cast<int>(abs(lrGpt.lon)); // longitude
os << setw(3) << tmpInt;
tmpDbl = fabs(lrGpt.lon) - (double)tmpInt;
quadNum += static_cast<char>(tmpDbl / QUAD_SIZE_IN_DEGREES);
os << quadChar << quadNum;
double latFraction = (lrGpt.lat / QUAD_SIZE_IN_DEGREES) -
ossim::round<int>((lrGpt.lat) / QUAD_SIZE_IN_DEGREES);
double lonFraction = (lrGpt.lon / QUAD_SIZE_IN_DEGREES) -
ossim::round<int>((lrGpt.lon) / QUAD_SIZE_IN_DEGREES);
// Black & White
// if(theRectsStandardFlag &&
// theChipSource->radiometry().numberOfBands() == 1)
// {
// char quarterQuadChar = '4';
// if (latFraction && lonFraction)
// {
// quarterQuadChar = '1';
// }
// else if (latFraction && !lonFraction)
// {
// quarterQuadChar = '2';
// }
// else if (!latFraction && lonFraction)
// {
// quarterQuadChar = '3';
// }
// os << quarterQuadChar << ends;
// }
// // Color
// else if(theRectsStandardFlag &&
// theChipSource->radiometry().numberOfBands() > 1)
// {
// char quarterQuadChar = '8';
// if (latFraction && lonFraction)
// {
// quarterQuadChar = '5';
// }
// else if (latFraction && !lonFraction)
// {
// quarterQuadChar = '6';
// }
// else if (!latFraction && lonFraction)
// {
// quarterQuadChar = '7';
// }
// os << quarterQuadChar << ends;
// }
// else
// {
char quarterQuadChar = 'D';
if (latFraction && lonFraction)
{
quarterQuadChar = 'A';
}
else if (latFraction && !lonFraction)
{
quarterQuadChar = 'B';
}
else if (!latFraction && lonFraction)
{
quarterQuadChar = 'C';
}
os << quarterQuadChar << ends;
// }
baseString = os.str().c_str();
if (traceDebug())
{
ossimNotify(ossimNotifyLevel_DEBUG)
<< " DEBUG: " << MODULE
<< "\nbaseString: " << baseString << std::endl;
}
setQuadName(baseString);
}
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);
}
}
