Skip to content
Snippets Groups Projects
Unverified Commit 0f6e6712 authored by Jesse Mapel's avatar Jesse Mapel Committed by GitHub
Browse files

Implemented cross covariance method (#271)

* Added proper covariance and reverted fl adjustment

* Added tests
parent 1bb01ae6
No related branches found
No related tags found
No related merge requests found
......@@ -620,17 +620,36 @@ void UsgsAstroLsSensorModel::updateState()
m_halfTime)
// Parameter covariance, hardcoded accuracy values
// hardcoded ~1 pixel accuracy values
int num_params = NUM_PARAMETERS;
int num_paramsSquare = num_params * num_params;
double variance = m_gsd * m_gsd;
for (int i = 0; i < num_paramsSquare; i++)
{
m_covariance[i] = 0.0;
}
for (int i = 0; i < num_params; i++)
{
m_covariance[i * num_params + i] = variance;
}
double positionVariance = m_gsd * m_gsd;
// parameter time is scaled to [0, 2]
// so divide by 2 for velocities and 4 for accelerations
double velocityVariance = positionVariance / 2.0;
double accelerationVariance = positionVariance / 4.0;
m_covariance.assign(num_params * num_params, 0.0);
// Set position variances
m_covariance[0] = positionVariance;
m_covariance[num_params + 1] = positionVariance;
m_covariance[2 * num_params + 2] = positionVariance;
m_covariance[3 * num_params + 3] = velocityVariance;
m_covariance[4 * num_params + 4] = velocityVariance;
m_covariance[5 * num_params + 5] = velocityVariance;
// Set orientation variances
m_covariance[6 * num_params + 6] = positionVariance;
m_covariance[7 * num_params + 7] = positionVariance;
m_covariance[8 * num_params + 8] = positionVariance;
m_covariance[9 * num_params + 9] = velocityVariance;
m_covariance[10 * num_params + 10] = velocityVariance;
m_covariance[11 * num_params + 11] = velocityVariance;
m_covariance[12 * num_params + 12] = accelerationVariance;
m_covariance[13 * num_params + 13] = accelerationVariance;
m_covariance[14 * num_params + 14] = accelerationVariance;
// Set focal length variance
m_covariance[15 * num_params + 15] = positionVariance;
}
......@@ -1695,6 +1714,18 @@ std::vector<double> UsgsAstroLsSensorModel::getCrossCovarianceMatrix(
csm::param::Set pSet,
const csm::GeometricModel::GeometricModelList& otherModels) const
{
// Return covariance matrix
if (&comparisonModel == this) {
std::vector<int> paramIndices = getParameterSetIndices(pSet);
int numParams = paramIndices.size();
std::vector<double> covariances(numParams * numParams, 0.0);
for (int i = 0; i < numParams; i++) {
for (int j = 0; j < numParams; j++) {
covariances[i * numParams + j] = getParameterCovariance(paramIndices[i], paramIndices[j]);
}
}
return covariances;
}
// No correlation between models.
const std::vector<int>& indices = getParameterSetIndices(pSet);
size_t num_rows = indices.size();
......@@ -1918,7 +1949,8 @@ void UsgsAstroLsSensorModel::losToEcf(
// Define imaging ray (look vector) in camera space
double cameraLook[3];
createCameraLookVector(undistortedFocalPlaneX, undistortedFocalPlaneY,
m_zDirection, m_focalLength + getValue(15, adj), cameraLook);
m_zDirection, m_focalLength * (1 - getValue(15, adj) / m_halfSwath),
cameraLook);
MESSAGE_LOG(m_logger, "losToEcf: uncorrected camera look vector {} {} {}",
cameraLook[0], cameraLook[1], cameraLook[2])
......
......@@ -267,6 +267,44 @@ class OrbitalLineScanSensorModel : public ::testing::Test {
}
};
class TwoLineScanSensorModels : public ::testing::Test {
protected:
csm::Isd isd;
UsgsAstroLsSensorModel *sensorModel1;
UsgsAstroLsSensorModel *sensorModel2;
void SetUp() override {
sensorModel1 = nullptr;
sensorModel2 = nullptr;
isd.setFilename("data/orbitalLineScan.img");
UsgsAstroPlugin cameraPlugin;
csm::Model *model1 = cameraPlugin.constructModelFromISD(
isd,
"USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL");
sensorModel1 = dynamic_cast<UsgsAstroLsSensorModel *>(model1);
csm::Model *model2 = cameraPlugin.constructModelFromISD(
isd,
"USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL");
sensorModel2 = dynamic_cast<UsgsAstroLsSensorModel *>(model2);
ASSERT_NE(sensorModel1, nullptr);
ASSERT_NE(sensorModel2, nullptr);
}
void TearDown() override {
if (sensorModel1) {
delete sensorModel1;
sensorModel1 = nullptr;
}
if (sensorModel2) {
delete sensorModel2;
sensorModel2 = nullptr;
}
}
};
#endif
......@@ -241,9 +241,36 @@ TEST_F(OrbitalLineScanSensorModel, ReferenceDateTime) {
EXPECT_EQ(date, "20000101T001639");
}
TEST_F(TwoLineScanSensorModels, CrossCovariance) {
std::vector<double> crossCovars = sensorModel1->getCrossCovarianceMatrix(*sensorModel2);
ASSERT_EQ(crossCovars.size(), sensorModel1->getNumParameters() * sensorModel2->getNumParameters());
for (int i = 0; i < sensorModel1->getNumParameters(); i++) {
for (int j = 0; j < sensorModel2->getNumParameters(); j++) {
EXPECT_EQ(crossCovars[i * sensorModel2->getNumParameters() + j], 0.0)
<< "Value at row " << i << " column " << j;
}
}
std::vector<double> covars = sensorModel1->getCrossCovarianceMatrix(*sensorModel1);
ASSERT_EQ(covars.size(), 16*16);
for (int i = 0; i < 16; i++) {
for (int j = 0; j < 16; j++) {
if (i == j) {
EXPECT_GT(covars[i * 16 + j], 0.0) << "Value at row " << i << " column " << j;
}
else {
EXPECT_EQ(covars[i * 16 + j], 0.0) << "Value at row " << i << " column " << j;
}
}
}
std::vector<double> fixedCovars = sensorModel1->getCrossCovarianceMatrix(*sensorModel1, csm::param::NON_ADJUSTABLE);
EXPECT_EQ(fixedCovars.size(), 0);
}
TEST_F(ConstVelocityLineScanSensorModel, FocalLengthAdjustment) {
csm::ImageCoord imagePt(8.5, 4.0);
sensorModel->setParameterValue(15, -45);
sensorModel->setParameterValue(15, 0.9 * sensorModel->m_halfSwath);
csm::EcefLocus locus = sensorModel->imageToRemoteImagingLocus(imagePt);
double scale = sqrt(5 * 5 + 0.4 * 0.4 + 0.05 * 0.05);
EXPECT_DOUBLE_EQ(locus.direction.x, -5.0 / scale);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please to comment