Unverified Commit 9598b19b authored by Stuart Sides's avatar Stuart Sides Committed by GitHub
Browse files

Merge pull request #224 from jessemapel/partials 

Fixed ground partials in framer and added a test
parents 53a5edd6 65a2df47

src/UsgsAstroFrameSensorModel.cpp

+34 −44
Original line number Diff line number Diff line
@@ -556,50 +556,40 @@ std::vector<double> UsgsAstroFrameSensorModel::computeGroundPartials(const csm:: 
                                                                     &groundPt) const {

  MESSAGE_LOG(this->m_logger, "Computing ground partials for ground point {}, {}, {}",

                               groundPt.x, groundPt.y, groundPt.z);

  // Partials of line, sample wrt X, Y, Z

  // Uses collinearity equations

  std::vector<double> partials(6, 0.0);

  // Partial of line, sample wrt X, Y, Z

  double x = groundPt.x;

  double y = groundPt.y;

  double z = groundPt.z;



  double m[3][3];

  calcRotationMatrix(m, m_noAdjustments);



  double xo, yo, zo;

  xo = groundPt.x - m_currentParameterValue[0];

  yo = groundPt.y - m_currentParameterValue[1];

  zo = groundPt.z - m_currentParameterValue[2];



  double u, v, w;

  u = m[0][0] * xo + m[0][1] * yo + m[0][2] * zo;

  v = m[1][0] * xo + m[1][1] * yo + m[1][2] * zo;

  w = m[2][0] * xo + m[2][1] * yo + m[2][2] * zo;



  double fdw, udw, vdw;

  fdw = m_focalLength / w;

  udw = u / w;

  vdw = v / w;



  double upx, vpx, wpx;

  upx = m[0][0];

  vpx = m[1][0];

  wpx = m[2][0];

  partials[0] = -fdw * ( upx - udw * wpx );

  partials[3] = -fdw * ( vpx - vdw * wpx );



  double upy, vpy, wpy;

  upy = m[0][1];

  vpy = m[1][1];

  wpy = m[2][1];

  partials[1] = -fdw * ( upy - udw * wpy );

  partials[4] = -fdw * ( vpy - vdw * wpy );



  double upz, vpz, wpz;

  upz = m[0][2];

  vpz = m[1][2];

  wpz = m[2][2];

  partials[2] = -fdw * ( upz - udw * wpz );

  partials[5] = -fdw * ( vpz - vdw * wpz );



  MESSAGE_LOG(this->m_logger, "Computing ground partials result line: {}, sample: {}, wrt: {} and x: {}, y: {}, z: {}",

  csm::ImageCoord ipB = groundToImage(groundPt);

  csm::EcefCoord nextPoint = imageToGround(csm::ImageCoord(ipB.line + 1, ipB.samp + 1));

  double dx = nextPoint.x - x;

  double dy = nextPoint.y - y;

  double dz = nextPoint.z - z;

  double pixelGroundSize = sqrt((dx*dx + dy*dy + dz*dz) / 2.0);



  // If the ground size is too small, try the opposite direction

  if (pixelGroundSize < 1e-10) {

    nextPoint = imageToGround(csm::ImageCoord(ipB.line - 1, ipB.samp - 1));

    dx = nextPoint.x - x;

    dy = nextPoint.y - y;

    dz = nextPoint.z - z;

    pixelGroundSize = sqrt((dx*dx + dy*dy + dz*dz) / 2.0);

  }



  csm::ImageCoord ipX = groundToImage(csm::EcefCoord(x + pixelGroundSize, y, z));

  csm::ImageCoord ipY = groundToImage(csm::EcefCoord(x, y + pixelGroundSize, z));

  csm::ImageCoord ipZ = groundToImage(csm::EcefCoord(x, y, z + pixelGroundSize));



  std::vector<double> partials(6, 0.0);

  partials[0] = (ipX.line - ipB.line) / pixelGroundSize;

  partials[3] = (ipX.samp - ipB.samp) / pixelGroundSize;

  partials[1] = (ipY.line - ipB.line) / pixelGroundSize;

  partials[4] = (ipY.samp - ipB.samp) / pixelGroundSize;

  partials[2] = (ipZ.line - ipB.line) / pixelGroundSize;

  partials[5] = (ipZ.samp - ipB.samp) / pixelGroundSize;



  MESSAGE_LOG(this->m_logger, "Computing ground partials results:\nLine: {}, {}, {}\nSample: {}, {}, {}",

                               partials[0], partials[1], partials[2], partials[3], partials[4], partials[5]);



  return partials;

src/Utilities.cpp

+2 −0
Original line number Diff line number Diff line
@@ -810,6 +810,8 @@ std::vector<double> getDistortionCoeffs(json isd, csm::WarningList *list) { 
              "Utilities::getDistortion()"));

        }

        coefficients = std::vector<double>(20, 0.0);

        coefficients[1] = 1.0;

        coefficients[12] = 1.0;

      }

    }

    break;

tests/Fixtures.h

+27 −0
Original line number Diff line number Diff line
@@ -62,6 +62,33 @@ class FrameSensorModel : public ::testing::Test { 
      }

};



class OrbitalFrameSensorModel : public ::testing::Test {

   protected:

      csm::Isd isd;

      UsgsAstroFrameSensorModel *sensorModel;



      void SetUp() override {

         sensorModel = NULL;



         isd.setFilename("data/orbitalFramer.img");

         UsgsAstroPlugin frameCameraPlugin;



         csm::Model *model = frameCameraPlugin.constructModelFromISD(

               isd,

               "USGS_ASTRO_FRAME_SENSOR_MODEL");

         sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);



         ASSERT_NE(sensorModel, nullptr);

      }



      void TearDown() override {

         if (sensorModel) {

            delete sensorModel;

            sensorModel = NULL;

         }

      }

};



class FrameIsdTest : public ::testing::Test {

   protected:

      csm::Isd isd;

tests/FrameCameraTests.cpp

+31 −0
Original line number Diff line number Diff line
@@ -79,6 +79,37 @@ TEST_F(FrameSensorModel, getImageIdentifier) { 
  EXPECT_EQ("simpleFramerISD", sensorModel->getImageIdentifier());

}



TEST_F(FrameSensorModel, Inversion) {

   csm::ImageCoord imagePt1(9.0, 9.0);

   csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt1, 0.0);

   csm::ImageCoord imagePt2 = sensorModel->groundToImage(groundPt);

   EXPECT_DOUBLE_EQ(imagePt1.line, imagePt2.line);

   EXPECT_DOUBLE_EQ(imagePt1.samp, imagePt2.samp);

}



TEST_F(OrbitalFrameSensorModel, Center) {

   csm::ImageCoord imagePt(8.0, 8.0);

   csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);

   EXPECT_DOUBLE_EQ(groundPt.x, 1000000.0);

   EXPECT_DOUBLE_EQ(groundPt.y, 0);

   EXPECT_DOUBLE_EQ(groundPt.z, 0);

}



TEST_F(OrbitalFrameSensorModel, GroundPartials) {

   csm::EcefCoord groundPt(1000000.0, 0.0, 0.0);

   std::vector<double> partials = sensorModel->computeGroundPartials(groundPt);

   // Pixels are 100m

   // lines are increasing z and samples are increasing y in body fixed

   // lines partials should be 0 except for the z partial which should be 1/100

   // sample partials should be 0 except for the y partial which should be 1/100

   EXPECT_DOUBLE_EQ(partials[0], 0.0);

   EXPECT_DOUBLE_EQ(partials[1], 0.0);

   EXPECT_DOUBLE_EQ(partials[2], 1 / 100.0);

   EXPECT_DOUBLE_EQ(partials[3], 0.0);

   EXPECT_DOUBLE_EQ(partials[4], 1 / 100.0);

   EXPECT_DOUBLE_EQ(partials[5], 0.0);

}





// Focal Length Tests:

TEST_F(FrameStateTest, FL500_OffBody4) {

tests/UtilitiesTests.cpp

+15 −0
Original line number Diff line number Diff line
@@ -141,6 +141,21 @@ TEST(UtilitiesTests, computePixelStart) { 
   EXPECT_DOUBLE_EQ(sample, 4.0);

}



TEST(UtilitiesTests, computePixelStartSumming) {

   double iTransS[] = {0.0, 0.0, 10.0};

   double iTransL[] = {0.0, 10.0, 0.0};

   double line, sample;

   computePixel(

         -0.5, -0.2,

         8.0, 8.0,

         2.0, 4.0,

         2.0, 1.0,

         iTransS, iTransL,

         line, sample);

   EXPECT_DOUBLE_EQ(line, 0.5);

   EXPECT_DOUBLE_EQ(sample, 2.0);

}



TEST(UtilitiesTests, createCameraLookVector) {

  double cameraLook[3];

  createCameraLookVector(0, -0.4, 1, 50, cameraLook);