Commit b885f175 authored by Kristin Berry's avatar Kristin Berry Committed by Jesse Mapel
Browse files

Add latitudinal partials for CSM (#4468) 



* Add latitudinal partials for CSM

* Added test

Co-authored-by: default avatarJesse Mapel <jmapel@usgs.gov>
parent a80265a4

isis/src/control/objs/CsmBundleObservation/CsmBundleObservation.cpp

+35 −15
Original line number Diff line number Diff line
@@ -748,6 +748,7 @@ QString CsmBundleObservation::formatBundleOutputString(bool errorPropagation, bo 
    SurfacePoint groundPoint = measure.parentControlPoint()->adjustedSurfacePoint();

    vector<double> groundPartials = measureCamera->GroundPartials(groundPoint);



    if (coordType == SurfacePoint::Rectangular) {

      // groundPartials is:

      // line WRT x

      // line WRT y
@@ -762,7 +763,26 @@ QString CsmBundleObservation::formatBundleOutputString(bool errorPropagation, bo 
      coeffPoint3D(0,0) = groundPartials[3] * 1000;

      coeffPoint3D(0,1) = groundPartials[4] * 1000;

      coeffPoint3D(0,2) = groundPartials[5] * 1000;

    }

    else if (coordType == SurfacePoint::Latitudinal) {

      std::vector<double> latDerivative = groundPoint.LatitudinalDerivative(SurfacePoint::One);

      std::vector<double> lonDerivative = groundPoint.LatitudinalDerivative(SurfacePoint::Two);

      std::vector<double> radDerivative = groundPoint.LatitudinalDerivative(SurfacePoint::Three);



      // Line w.r.t (lat, lon, radius)

      coeffPoint3D(1,0) = 1000 * (groundPartials[0]*latDerivative[0] + groundPartials[1]*latDerivative[1] + groundPartials[2]*latDerivative[2]);

      coeffPoint3D(1,1) = 1000 * (groundPartials[0]*lonDerivative[0] + groundPartials[1]*lonDerivative[1] + groundPartials[2]*lonDerivative[2]);

      coeffPoint3D(1,2) = 1000 * (groundPartials[0]*radDerivative[0] + groundPartials[1]*radDerivative[1] + groundPartials[2]*radDerivative[2]);



      // Sample w.r.t (lat, lon, radius)

      coeffPoint3D(0,0) = 1000 * (groundPartials[3]*latDerivative[0] + groundPartials[4]*latDerivative[1] + groundPartials[5]*latDerivative[2]);

      coeffPoint3D(0,1) = 1000 * (groundPartials[3]*lonDerivative[0] + groundPartials[4]*lonDerivative[1] + groundPartials[5]*lonDerivative[2]);

      coeffPoint3D(0,2) = 1000 * (groundPartials[3]*radDerivative[0] + groundPartials[4]*radDerivative[1] + groundPartials[5]*radDerivative[2]);

    }

    else {

      IString msg ="Unknown surface point coordinate type enum [" + toString(coordType) + "]." ;

      throw IException(IException::Programmer, msg, _FILEINFO_);

    }

    return true;

  }

isis/tests/CsmBundleObservationTests.cpp

+82 −1
Original line number Diff line number Diff line
@@ -9,8 +9,9 @@ 
#include "Mocks.h"

#include "TestUtilities.h"

#include "SerialNumber.h"

#include "BundleTargetBody.h"

#include "BundleControlPoint.h"

#include "BundleImage.h"

#include "BundleTargetBody.h"





#include "gmock/gmock.h"
@@ -258,3 +259,83 @@ TEST_F(CSMCameraFixture, CsmBundleApplyParameterCorrections) { 


  ASSERT_TRUE(observation.applyParameterCorrections(corrections));

}





TEST_F(CSMCameraFixture, CsmBundleComputePoint3DPartials) {

  EXPECT_CALL(mockModel, getNumParameters())

      .WillRepeatedly(::testing::Return(3));

  EXPECT_CALL(mockModel, getParameterType(0))

      .WillRepeatedly(::testing::Return(csm::param::FICTITIOUS));

  EXPECT_CALL(mockModel, getParameterType(1))

      .WillRepeatedly(::testing::Return(csm::param::FIXED));

  EXPECT_CALL(mockModel, getParameterType(2))

      .WillRepeatedly(::testing::Return(csm::param::REAL));

  EXPECT_CALL(mockModel, getParameterName(0))

      .WillRepeatedly(::testing::Return("Parameter 1"));

  EXPECT_CALL(mockModel, getParameterName(1))

      .WillRepeatedly(::testing::Return("Parameter 2"));

  EXPECT_CALL(mockModel, getParameterName(2))

      .WillRepeatedly(::testing::Return("Parameter 3"));

  EXPECT_CALL(mockModel, getParameterCovariance(0, 0))

      .WillRepeatedly(::testing::Return(0.112));

  EXPECT_CALL(mockModel, getParameterCovariance(1, 1))

      .WillRepeatedly(::testing::Return(0.0123));

  EXPECT_CALL(mockModel, getParameterCovariance(2, 2))

      .WillRepeatedly(::testing::Return(0.342));

  EXPECT_CALL(mockModel, computeGroundPartials)

      .WillRepeatedly(::testing::Return(std::vector<double>{1, 2, 3, 4, 5, 6}));





  QString sn = SerialNumber::Compose(*testCube);



  BundleImageQsp bi = BundleImageQsp(new BundleImage(testCam, sn, testCube->fileName()));

  BundleObservationSolveSettings bundleSolSetting;



  CsmBundleObservation observation(bi,

                                   "ObservationNumber",

                                   "InstrumentId",

                                   nullptr);



  QStringList paramList;



  bundleSolSetting.setCSMSolveSet(csm::param::ADJUSTABLE);



  ASSERT_TRUE(observation.setSolveSettings(bundleSolSetting));



  BundleSettingsQsp testBundleSettings(new BundleSettings());



  SurfacePoint testSurfacePoint(Displacement(1000.0, Displacement::Kilometers),

                                Displacement(0.0, Displacement::Kilometers),

                                Displacement(0.0, Displacement::Kilometers));



  ControlPoint testPoint("testPoint");

  testPoint.SetAdjustedSurfacePoint(testSurfacePoint);



  ControlMeasure *testMeasure = new ControlMeasure();

  testMeasure->SetCubeSerialNumber(sn);

  testMeasure->SetCamera(testCam);

  testPoint.Add(testMeasure);



  BundleControlPointQsp testBundlePoint(new BundleControlPoint(testBundleSettings, &testPoint));

  BundleMeasureQsp testBundleMeasure = testBundlePoint->front();



  LinearAlgebra::Matrix coeffPoint3D(2, 3);



  ASSERT_TRUE(observation.computePoint3DPartials(coeffPoint3D, *testBundleMeasure, SurfacePoint::Rectangular));



  EXPECT_EQ(coeffPoint3D(0,0), 4000);

  EXPECT_EQ(coeffPoint3D(0,1), 5000);

  EXPECT_EQ(coeffPoint3D(0,2), 6000);

  EXPECT_EQ(coeffPoint3D(1,0), 1000);

  EXPECT_EQ(coeffPoint3D(1,1), 2000);

  EXPECT_EQ(coeffPoint3D(1,2), 3000);



  ASSERT_TRUE(observation.computePoint3DPartials(coeffPoint3D, *testBundleMeasure, SurfacePoint::Latitudinal));



  EXPECT_EQ(coeffPoint3D(0,0), 6000000);

  EXPECT_EQ(coeffPoint3D(0,1), 5000000);

  EXPECT_EQ(coeffPoint3D(0,2), 4000);

  EXPECT_EQ(coeffPoint3D(1,0), 3000000);

  EXPECT_EQ(coeffPoint3D(1,1), 2000000);

  EXPECT_EQ(coeffPoint3D(1,2), 1000);

}
 
 No newline at end of file