void ossimSpectraboticsRedEdgeModel::imagingRay(const ossimDpt& image_point,
ossimEcefRay& image_ray) const
{
if(traceDebug())
{
ossimNotify(ossimNotifyLevel_DEBUG) << "ossimSpectraboticsRedEdgeModel::imagingRay: ..... entered" << std::endl;
}
ossimDpt film (image_point.x-m_calibratedCenter.x,
m_calibratedCenter.y - image_point.y); //- theRefImgPt);
// ossimDpt film (image_point-m_calibratedCenter); //- theRefImgPt);
if(m_lensDistortion.valid())
{
ossimDpt tempFilm(film.x/m_norm, film.y/m_norm);
ossimDpt filmOut;
m_lensDistortion->undistort(tempFilm, filmOut);
film.x = filmOut.x*m_norm;
film.y = filmOut.y*m_norm;
}
film.x *= m_pixelSize.x; // pixel size on the film
film.y *= m_pixelSize.y; // pixel size on the film
ossimColumnVector3d cam_ray_dir (film.x,
film.y,
-m_focalLength);
ossimEcefVector ecf_ray_dir (m_compositeMatrix*cam_ray_dir);
ecf_ray_dir = ecf_ray_dir*(1.0/ecf_ray_dir.magnitude());
image_ray.setOrigin(m_adjEcefPlatformPosition);
image_ray.setDirection(ecf_ray_dir);
}
void ossimPpjFrameSensor::imagingRay(const ossimDpt& imagePoint,
ossimEcefRay& imageRay) const
{
// Form camera frame LOS vector
ossimColumnVector3d camLOS(imagePoint.x - m_principalPoint.x,
imagePoint.y - m_principalPoint.y,
m_adjustedFocalLength);
// Rotate to ECF
ossimColumnVector3d ecfLOS = m_ecef2CamInverse * camLOS;
imageRay.setOrigin(m_adjustedCameraPosition);
ossimEcefVector ecfRayDir(ecfLOS);
imageRay.setDirection(ecfRayDir);
if (traceDebug())
{
ossimNotify(ossimNotifyLevel_DEBUG)
<< "ossimPpjFrameSensor::imagingRay DEBUG:\n"
<< " camLOS = " << camLOS << "\n"
<< " m_adjustedPlatformPosition = " << m_adjustedCameraPosition << "\n"
<< " imageRay = " << imageRay << "\n"
<< std::endl;
}
}
void ossimBuckeyeSensor::imagingRay(const ossimDpt& image_point,
ossimEcefRay& image_ray) const
{
if(traceDebug()) ossimNotify(ossimNotifyLevel_DEBUG) << "ossimBuckeyeSensor::imagingRay: ..... entered" << std::endl;
ossimDpt f1 ((image_point) - theRefImgPt);
f1.x *= thePixelSize.x;
f1.y *= -thePixelSize.y;
ossimDpt film (f1 - thePrincipalPoint);
if(traceDebug())
{
ossimNotify(ossimNotifyLevel_DEBUG) << "pixel size = " << thePixelSize << std::endl;
ossimNotify(ossimNotifyLevel_DEBUG) << "principal pt = " << thePrincipalPoint << std::endl;
ossimNotify(ossimNotifyLevel_DEBUG) << "film pt = " << film << std::endl;
}
if (theLensDistortion.valid())
{
ossimDpt filmOut;
theLensDistortion->undistort(film, filmOut);
film = filmOut;
}
ossimColumnVector3d cam_ray_dir (film.x,
film.y,
-theFocalLength);
ossimEcefVector ecf_ray_dir (theCompositeMatrix*cam_ray_dir);
ecf_ray_dir = ecf_ray_dir*(1.0/ecf_ray_dir.magnitude());
image_ray.setOrigin(theAdjEcefPlatformPosition);
image_ray.setDirection(ecf_ray_dir);
if(traceDebug()) ossimNotify(ossimNotifyLevel_DEBUG) << "ossimBuckeyeSensor::imagingRay: ..... leaving" << std::endl;
}
void ossimCsmSensorModel::imagingRay(const ossimDpt& image_point,
ossimEcefRay& image_ray) const
{
if(m_model)
{
double locus[6] = {0.0, 0.0, 0.0,
0.0, 0.0, 0.0};
double AP = 0.0;
m_model->imageToRemoteImagingLocus(image_point.y, image_point.x, locus, AP);
ossimEcefVector v(locus[3], locus[4], locus[5]);
image_ray.setOrigin(ossimEcefPoint(locus[0], locus[1], locus[2]));
image_ray.setDirection(v);
}
}
void ossimAlphaSensorHSI::imagingRay(const ossimDpt& imagePoint,
ossimEcefRay& imageRay) const
{
ossim_float64 line = imagePoint.y;
// Form camera frame LOS vector
ossim_float64 scanAngle = getScanAngle(line);
ossimColumnVector3d camLOS(imagePoint.x - theImageSize.x/2,
m_adjustedFocalLength * tan(scanAngle),
m_adjustedFocalLength);
// Compute camera position & orientation matrix
ossimEcefPoint platPos;
NEWMAT::Matrix cam2EcfRot;
getPositionOrientation(line, platPos, cam2EcfRot);
// Rotate camera vector to ECF
ossimColumnVector3d ecfLOS = cam2EcfRot * camLOS.unit();
// Construct ECF image ray
imageRay.setOrigin(platPos);
ossimEcefVector ecfRayDir(ecfLOS);
imageRay.setDirection(ecfRayDir);
if (traceDebug())
{
ossimNotify(ossimNotifyLevel_DEBUG)
<< "ossimAlphaSensorHSI::imagingRay DEBUG:\n"
<< " imagePoint = " << imagePoint << "\n"
<< " imageRay = " << imageRay << "\n"
<< " camLOS = " << camLOS << "\n"
<< " platPos = " << platPos << "\n"
<< std::endl;
}
}
//*****************************************************************************
// 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;
}
