// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void DerivativeHelpers::TriangleDeriv::operator()(TriangleGeom* triangles, int64_t triId, double values[3], double derivs[3])
{
float vert0_f[3] = { 0.0f, 0.0f, 0.0f };
float vert1_f[3] = { 0.0f, 0.0f, 0.0f };
float vert2_f[3] = { 0.0f, 0.0f, 0.0f };
double vert0[3] = { 0.0, 0.0, 0.0 };
double vert1[3] = { 0.0, 0.0, 0.0 };
double vert2[3] = { 0.0, 0.0, 0.0 };
double vert0_2d[2] = { 0.0, 0.0 };
double vert1_2d[2] = { 0.0, 0.0 };
double vert2_2d[2] = { 0.0, 0.0 };
double vector20[3] = { 0.0, 0.0, 0.0 };
double basis1[3] = { 0.0, 0.0, 0.0 };
double basis2[3] = { 0.0, 0.0, 0.0 };
double mag_basis1 = 0.0;
double mag_basis2 = 0.0;
double normal[3] = { 0.0, 0.0, 0.0 };
double shapeFunctions[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
double sum[2] = { 0.0, 0.0 };
double dBydx = 0.0;
double dBydy = 0.0;
int64_t verts[3] = { 0, 0, 0 };
triangles->getVertsAtTri(triId, verts);
triangles->getCoords(verts[0], vert0_f);
triangles->getCoords(verts[1], vert1_f);
triangles->getCoords(verts[2], vert2_f);
std::copy(vert0_f, vert0_f + 3, vert0);
std::copy(vert1_f, vert1_f + 3, vert1);
std::copy(vert2_f, vert2_f + 3, vert2);
GeometryMath::FindPlaneNormalVector(vert0, vert1, vert2, normal);
MatrixMath::Normalize3x1(normal);
for (size_t i = 0; i < 3; i++)
{
basis1[i] = vert1[i] - vert0[i];
vector20[i] = vert2[i] - vert0[i];
}
MatrixMath::CrossProduct(normal, basis1, basis2);
mag_basis1 = MatrixMath::Magnitude3x1(basis1);
mag_basis2 = MatrixMath::Magnitude3x1(basis2);
if ( mag_basis1 <= 0.0 || mag_basis2 <= 0.0 )
{
for (size_t i = 0; i < 3; i++ )
{
derivs[i] = 0.0;
}
return;
}
MatrixMath::Normalize3x1(basis1);
MatrixMath::Normalize3x1(basis2);
// Project the 3D triangle onto a 2D local system
// Basis vectors for 2D system are the vector between triangle vertex 1 and 0 (basis1)
// and the normal of the plane defined by the triangle normal and basis1 (basis2)
vert0_2d[0] = vert0_2d[1] = 0.0;
vert1_2d[0] = mag_basis1;
vert1_2d[1] = 0.0;
vert2_2d[0] = MatrixMath::DotProduct3x1(vector20, basis1);
vert2_2d[1] = MatrixMath::DotProduct3x1(vector20, basis2);
// Compute interpolation function derivatives
triangles->getShapeFunctions(NULL, shapeFunctions);
// Compute Jacobian and inverse Jacobian using Eigen
// Jacobian is constant for a triangle, so inverse must exist
double jPtr[4] = { 0.0, 0.0, 0.0, 0.0 };
jPtr[0] = vert1_2d[0] - vert0_2d[0];
jPtr[1] = vert1_2d[1] - vert0_2d[1];
jPtr[2] = vert2_2d[0] - vert0_2d[0];
jPtr[3] = vert2_2d[1] - vert0_2d[1];
Eigen::Map<TriangleJacobian> jMat(jPtr);
TriangleJacobian jMatI;
jMatI = jMat.inverse();
// Loop over derivative values. For each set of values, compute
// derivatives in local 2D system and then transform into original 3D system
for (size_t i = 0; i < 3; i++)
{
sum[0] += shapeFunctions[i] * values[i];
sum[1] += shapeFunctions[3 + i] * values[i];
}
dBydx = sum[0] * jMatI(0, 0) + sum[1] * jMatI(0, 1);
dBydy = sum[0] * jMatI(1, 0) + sum[1] * jMatI(1, 1);
derivs[0] = dBydx * basis1[0] + dBydy * basis2[0];
derivs[1] = dBydx * basis1[1] + dBydy * basis2[1];
derivs[2] = dBydx * basis1[2] + dBydy * basis2[2];
}
//*****************************************************************************
// 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;
}
}
// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void DerivativeHelpers::QuadDeriv::operator()(QuadGeom* quads, int64_t quadId, double values[4], double derivs[3])
{
float vert0_f[3] = { 0.0f, 0.0f, 0.0f };
float vert1_f[3] = { 0.0f, 0.0f, 0.0f };
float vert2_f[3] = { 0.0f, 0.0f, 0.0f };
float vert3_f[3] = { 0.0f, 0.0f, 0.0f };
double vert0[3] = { 0.0, 0.0, 0.0 };
double vert1[3] = { 0.0, 0.0, 0.0 };
double vert2[3] = { 0.0, 0.0, 0.0 };
double vert3[3] = { 0.0, 0.0, 0.0 };
double vert0_2d[2] = { 0.0, 0.0 };
double vert1_2d[2] = { 0.0, 0.0 };
double vert2_2d[2] = { 0.0, 0.0 };
double vert3_2d[2] = { 0.0, 0.0 };
double vector20[3] = { 0.0, 0.0, 0.0 };
double vector30[3] = { 0.0, 0.0, 0.0 };
double basis1[3] = { 0.0, 0.0, 0.0 };
double basis2[3] = { 0.0, 0.0, 0.0 };
double mag_basis1 = 0.0;
double mag_basis2 = 0.0;
double normal[3] = { 0.0, 0.0, 0.0 };
double shapeFunctions[8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
double pCoords[3] { 0.0, 0.0, 0.0 };
double sum[2] = { 0.0, 0.0 };
double dBydx = 0.0;
double dBydy = 0.0;
int64_t verts[4] = { 0, 0, 0 };
quads->getVertsAtQuad(quadId, verts);
quads->getCoords(verts[0], vert0_f);
quads->getCoords(verts[1], vert1_f);
quads->getCoords(verts[2], vert2_f);
quads->getCoords(verts[3], vert3_f);
std::copy(vert0_f, vert0_f + 3, vert0);
std::copy(vert1_f, vert1_f + 3, vert1);
std::copy(vert2_f, vert2_f + 3, vert2);
std::copy(vert3_f, vert3_f + 3, vert3);
GeometryMath::FindPlaneNormalVector(vert0, vert1, vert2, normal);
MatrixMath::Normalize3x1(normal);
// If vertices 0, 1, & 2 are co-linear, use vertex 3 to find the normal
if (normal[0] == 0.0 && normal[1] == 0.0 && normal[2] == 0.0)
{
GeometryMath::FindPlaneNormalVector(vert0, vert1, vert2, normal);
MatrixMath::Normalize3x1(normal);
}
for (size_t i = 0; i < 3; i++)
{
basis1[i] = vert1[i] - vert0[i];
vector20[i] = vert2[i] - vert0[i];
vector30[i] = vert3[i] - vert0[i];
}
MatrixMath::CrossProduct(normal, basis1, basis2);
mag_basis1 = MatrixMath::Magnitude3x1(basis1);
mag_basis2 = MatrixMath::Magnitude3x1(basis2);
if ( mag_basis1 <= 0.0 || mag_basis2 <= 0.0 )
{
for (size_t i = 0; i < 3; i++ )
{
derivs[i] = 0.0;
}
return;
}
MatrixMath::Normalize3x1(basis1);
MatrixMath::Normalize3x1(basis2);
// Project the 3D quad onto a 2D local system
// Basis vectors for 2D system are the vector between quad vertex 1 and 0 (basis1)
// and the normal of the plane defined by the quad normal and basis1 (basis2)
vert0_2d[0] = vert0_2d[1] = 0.0;
vert1_2d[0] = mag_basis1;
vert1_2d[1] = 0.0;
vert2_2d[0] = MatrixMath::DotProduct3x1(vector20, basis1);
vert2_2d[1] = MatrixMath::DotProduct3x1(vector20, basis2);
vert3_2d[0] = MatrixMath::DotProduct3x1(vector30, basis1);
vert3_2d[1] = MatrixMath::DotProduct3x1(vector30, basis2);
quads->getParametricCenter(pCoords);
quads->getShapeFunctions(pCoords, shapeFunctions);
// Compute Jacobian and inverse Jacobian using Eigen
double jPtr[4] = { 0.0, 0.0, 0.0, 0.0 };
jPtr[0] = vert0_2d[0] * shapeFunctions[0] + vert1_2d[0] * shapeFunctions[1] +
vert2_2d[0] * shapeFunctions[2] + vert3_2d[0] * shapeFunctions[3];
jPtr[1] = vert0_2d[1] * shapeFunctions[0] + vert1_2d[1] * shapeFunctions[1] +
vert2_2d[1] * shapeFunctions[2] + vert3_2d[1] * shapeFunctions[3];
jPtr[2] = vert0_2d[0] * shapeFunctions[4] + vert1_2d[0] * shapeFunctions[5] +
vert2_2d[0] * shapeFunctions[6] + vert3_2d[0] * shapeFunctions[7];
jPtr[3] = vert0_2d[1] * shapeFunctions[4] + vert1_2d[1] * shapeFunctions[5] +
vert2_2d[1] * shapeFunctions[6] + vert3_2d[1] * shapeFunctions[7];
Eigen::Map<QuadJacobian> jMat(jPtr);
QuadJacobian jMatI;
bool invertible = false;
jMat.computeInverseWithCheck(jMatI, invertible);
// Jacobian is non-constant for a quad, so must check if must exists
// If the Jacobian is not invertible, set derivatives to 0
if (!invertible)
{
for (size_t i = 0; i < 3; i++ )
{
derivs[i] = 0.0;
}
}
// Loop over derivative values. For each set of values, compute
// derivatives in local 2D system and then transform into original 3D system
for (size_t i = 0; i < 4; i++)
{
sum[0] += shapeFunctions[i] * values[i];
sum[1] += shapeFunctions[4 + i] * values[i];
}
dBydx = sum[0] * jMatI(0, 0) + sum[1] * jMatI(0, 1);
dBydy = sum[0] * jMatI(1, 0) + sum[1] * jMatI(1, 1);
derivs[0] = dBydx * basis1[0] + dBydy * basis2[0];
derivs[1] = dBydx * basis1[1] + dBydy * basis2[1];
derivs[2] = dBydx * basis1[2] + dBydy * basis2[2];
}
