Add constructor tests and extend tests to Pose2
							parent
							
								
									ef2cd5dab5
								
							
						
					
					
						commit
						74b92efd89
					
				|  | @ -11,6 +11,7 @@ | |||
| 
 | ||||
| #include <gtsam_unstable/slam/PartialPriorFactor.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/geometry/Pose3.h> | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <gtsam/base/TestableAssertions.h> | ||||
|  | @ -19,8 +20,9 @@ | |||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| namespace NM = gtsam::noiseModel; | ||||
| 
 | ||||
| // Pose representation is [ Rx Ry Rz Tx Ty Tz ].
 | ||||
| // Pose3 tangent representation is [ Rx Ry Rz Tx Ty Tz ].
 | ||||
| static const int kIndexRx = 0; | ||||
| static const int kIndexRy = 1; | ||||
| static const int kIndexRz = 2; | ||||
|  | @ -29,30 +31,152 @@ static const int kIndexTy = 4; | |||
| static const int kIndexTz = 5; | ||||
| 
 | ||||
| 
 | ||||
| typedef PartialPriorFactor<Pose3> TestPartialPriorFactor; | ||||
| typedef PartialPriorFactor<Pose2> TestPartialPriorFactor2; | ||||
| typedef PartialPriorFactor<Pose3> TestPartialPriorFactor3; | ||||
| typedef std::vector<size_t> Indices; | ||||
| 
 | ||||
| /// traits
 | ||||
| namespace gtsam { | ||||
| template<> | ||||
| struct traits<TestPartialPriorFactor> : public Testable<TestPartialPriorFactor> { | ||||
| }; | ||||
| struct traits<TestPartialPriorFactor2> : public Testable<TestPartialPriorFactor2> {}; | ||||
| 
 | ||||
| template<> | ||||
| struct traits<TestPartialPriorFactor3> : public Testable<TestPartialPriorFactor3> {}; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(PartialPriorFactor, JacobianAtIdentity) | ||||
| { | ||||
| TEST(PartialPriorFactor, Constructors2) { | ||||
|   Key poseKey(1); | ||||
|   Pose2 measurement(-13.1, 3.14, -0.73); | ||||
| 
 | ||||
|   // Prior on x component of translation.
 | ||||
|   TestPartialPriorFactor2 factor1(poseKey, 0, measurement.x(), NM::Isotropic::Sigma(1, 0.25)); | ||||
|   CHECK(assert_equal(1, factor1.prior().rows())); | ||||
|   CHECK(assert_equal(measurement.x(), factor1.prior()(0))); | ||||
|   CHECK(assert_container_equality<Indices>({ 0 }, factor1.indices())); | ||||
| 
 | ||||
|   // Prior on full translation vector.
 | ||||
|   const Indices t_indices = { 0, 1 }; | ||||
|   TestPartialPriorFactor2 factor2(poseKey, t_indices, measurement.translation(), NM::Isotropic::Sigma(2, 0.25)); | ||||
|   CHECK(assert_equal(2, factor2.prior().rows())); | ||||
|   CHECK(assert_equal(measurement.translation(), factor2.prior())); | ||||
|   CHECK(assert_container_equality<Indices>(t_indices, factor2.indices())); | ||||
| 
 | ||||
|   // Prior on theta.
 | ||||
|   TestPartialPriorFactor2 factor3(poseKey, 2, measurement.theta(), NM::Isotropic::Sigma(1, 0.1)); | ||||
|   CHECK(assert_equal(1, factor3.prior().rows())); | ||||
|   CHECK(assert_equal(measurement.theta(), factor3.prior()(0))); | ||||
|   CHECK(assert_container_equality<Indices>({ 2 }, factor3.indices())); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(PartialPriorFactor, JacobianPartialTranslation2) { | ||||
|   Key poseKey(1); | ||||
|   Pose2 measurement(-13.1, 3.14, -0.73); | ||||
| 
 | ||||
|   // Prior on x component of translation.
 | ||||
|   TestPartialPriorFactor2 factor(poseKey, 0, measurement.x(), NM::Isotropic::Sigma(1, 0.25)); | ||||
| 
 | ||||
|   Pose2 pose = measurement; // Zero-error linearization point.
 | ||||
| 
 | ||||
|   // Calculate numerical derivatives.
 | ||||
|   Matrix expectedH1 = numericalDerivative11<Vector, Pose2>( | ||||
|       boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose); | ||||
| 
 | ||||
|   // Use the factor to calculate the derivative.
 | ||||
|   Matrix actualH1; | ||||
|   factor.evaluateError(pose, actualH1); | ||||
| 
 | ||||
|   // Verify we get the expected error.
 | ||||
|   CHECK(assert_equal(expectedH1, actualH1, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(PartialPriorFactor, JacobianFullTranslation2) { | ||||
|   Key poseKey(1); | ||||
|   Pose2 measurement(-6.0, 3.5, 0.123); | ||||
| 
 | ||||
|   // Prior on x component of translation.
 | ||||
|   TestPartialPriorFactor2 factor(poseKey, { 0, 1 }, measurement.translation(), NM::Isotropic::Sigma(2, 0.25)); | ||||
| 
 | ||||
|   Pose2 pose = measurement; // Zero-error linearization point.
 | ||||
| 
 | ||||
|   // Calculate numerical derivatives.
 | ||||
|   Matrix expectedH1 = numericalDerivative11<Vector, Pose2>( | ||||
|       boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose); | ||||
| 
 | ||||
|   // Use the factor to calculate the derivative.
 | ||||
|   Matrix actualH1; | ||||
|   factor.evaluateError(pose, actualH1); | ||||
| 
 | ||||
|   // Verify we get the expected error.
 | ||||
|   CHECK(assert_equal(expectedH1, actualH1, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(PartialPriorFactor, JacobianTheta) { | ||||
|   Key poseKey(1); | ||||
|   Pose2 measurement(-1.0, 0.4, -2.5); | ||||
| 
 | ||||
|   // Prior on x component of translation.
 | ||||
|   TestPartialPriorFactor2 factor(poseKey, 2, measurement.theta(), NM::Isotropic::Sigma(1, 0.25)); | ||||
| 
 | ||||
|   Pose2 pose = measurement; // Zero-error linearization point.
 | ||||
| 
 | ||||
|   // Calculate numerical derivatives.
 | ||||
|   Matrix expectedH1 = numericalDerivative11<Vector, Pose2>( | ||||
|       boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose); | ||||
| 
 | ||||
|   // Use the factor to calculate the derivative.
 | ||||
|   Matrix actualH1; | ||||
|   factor.evaluateError(pose, actualH1); | ||||
| 
 | ||||
|   // Verify we get the expected error.
 | ||||
|   CHECK(assert_equal(expectedH1, actualH1, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(PartialPriorFactor, Constructors3) { | ||||
|   Key poseKey(1); | ||||
|   Pose3 measurement(Rot3::RzRyRx(-0.17, 0.567, M_PI), Point3(10.0, -2.3, 3.14)); | ||||
| 
 | ||||
|   // Single component of translation.
 | ||||
|   TestPartialPriorFactor3 factor1(poseKey, kIndexTy, measurement.y(), | ||||
|       NM::Isotropic::Sigma(1, 0.25)); | ||||
|   CHECK(assert_equal(1, factor1.prior().rows())); | ||||
|   CHECK(assert_equal(measurement.y(), factor1.prior()(0))); | ||||
|   CHECK(assert_container_equality<Indices>({ kIndexTy }, factor1.indices())); | ||||
| 
 | ||||
|   // Full translation vector.
 | ||||
|   const Indices t_indices = { kIndexTx, kIndexTy, kIndexTz }; | ||||
|   TestPartialPriorFactor3 factor2(poseKey, t_indices, measurement.translation(), | ||||
|       NM::Isotropic::Sigma(3, 0.25)); | ||||
|   CHECK(assert_equal(3, factor2.prior().rows())); | ||||
|   CHECK(assert_equal(measurement.translation(), factor2.prior())); | ||||
|   CHECK(assert_container_equality<Indices>(t_indices, factor2.indices())); | ||||
| 
 | ||||
|   // Full tangent vector of rotation.
 | ||||
|   const Indices r_indices = { kIndexRx, kIndexRy, kIndexRz }; | ||||
|   TestPartialPriorFactor3 factor3(poseKey, r_indices, Rot3::Logmap(measurement.rotation()), | ||||
|       NM::Isotropic::Sigma(3, 0.1)); | ||||
|   CHECK(assert_equal(3, factor3.prior().rows())); | ||||
|   CHECK(assert_equal(Rot3::Logmap(measurement.rotation()), factor3.prior())); | ||||
|   CHECK(assert_container_equality<Indices>(r_indices, factor3.indices())); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(PartialPriorFactor, JacobianAtIdentity3) { | ||||
|   Key poseKey(1); | ||||
|   Pose3 measurement = Pose3::identity(); | ||||
|   SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25); | ||||
|   SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25); | ||||
| 
 | ||||
|   TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().y(), model); | ||||
|   TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model); | ||||
| 
 | ||||
|   // Create a linearization point at the zero-error point
 | ||||
|   Pose3 pose = Pose3::identity(); | ||||
|   Pose3 pose = measurement; // Zero-error linearization point.
 | ||||
| 
 | ||||
|   // Calculate numerical derivatives.
 | ||||
|   Matrix expectedH1 = numericalDerivative11<Vector, Pose3>( | ||||
|       boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose); | ||||
|       boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); | ||||
| 
 | ||||
|   // Use the factor to calculate the derivative.
 | ||||
|   Matrix actualH1; | ||||
|  | @ -63,19 +187,18 @@ TEST(PartialPriorFactor, JacobianAtIdentity) | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(PartialPriorFactor, JacobianPartialTranslation) { | ||||
| TEST(PartialPriorFactor, JacobianPartialTranslation3) { | ||||
|   Key poseKey(1); | ||||
|   Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); | ||||
|   SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25); | ||||
|   SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25); | ||||
| 
 | ||||
|   TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().y(), model); | ||||
|   TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model); | ||||
| 
 | ||||
|   // Create a linearization point at the zero-error point
 | ||||
|   Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); | ||||
|   Pose3 pose = measurement; // Zero-error linearization point.
 | ||||
| 
 | ||||
|   // Calculate numerical derivatives.
 | ||||
|   Matrix expectedH1 = numericalDerivative11<Vector, Pose3>( | ||||
|       boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose); | ||||
|       boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); | ||||
| 
 | ||||
|   // Use the factor to calculate the derivative.
 | ||||
|   Matrix actualH1; | ||||
|  | @ -86,20 +209,20 @@ TEST(PartialPriorFactor, JacobianPartialTranslation) { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(PartialPriorFactor, JacobianFullTranslation) { | ||||
| TEST(PartialPriorFactor, JacobianFullTranslation3) { | ||||
|   Key poseKey(1); | ||||
|   Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); | ||||
|   SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); | ||||
|   SharedNoiseModel model = NM::Isotropic::Sigma(3, 0.25); | ||||
| 
 | ||||
|   std::vector<size_t> translationIndices = { kIndexTx, kIndexTy, kIndexTz }; | ||||
|   TestPartialPriorFactor factor(poseKey, translationIndices, measurement.translation(), model); | ||||
|   TestPartialPriorFactor3 factor(poseKey, translationIndices, measurement.translation(), model); | ||||
| 
 | ||||
|   // Create a linearization point at the zero-error point
 | ||||
|   Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); | ||||
|   Pose3 pose = measurement; // Zero-error linearization point.
 | ||||
| 
 | ||||
|   // Calculate numerical derivatives.
 | ||||
|   Matrix expectedH1 = numericalDerivative11<Vector, Pose3>( | ||||
|       boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose); | ||||
|       boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); | ||||
| 
 | ||||
|   // Use the factor to calculate the derivative.
 | ||||
|   Matrix actualH1; | ||||
|  | @ -110,20 +233,43 @@ TEST(PartialPriorFactor, JacobianFullTranslation) { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(PartialPriorFactor, JacobianFullRotation) { | ||||
| TEST(PartialPriorFactor, JacobianTxTz3) { | ||||
|   Key poseKey(1); | ||||
|   Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); | ||||
|   SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); | ||||
|   Pose3 measurement(Rot3::RzRyRx(-0.17, 0.567, M_PI), Point3(10.0, -2.3, 3.14)); | ||||
|   SharedNoiseModel model = NM::Isotropic::Sigma(2, 0.25); | ||||
| 
 | ||||
|   std::vector<size_t> rotationIndices = { kIndexRx, kIndexRy, kIndexRz }; | ||||
|   TestPartialPriorFactor factor(poseKey, rotationIndices, Rot3::Logmap(measurement.rotation()), model); | ||||
|   std::vector<size_t> translationIndices = { kIndexTx, kIndexTz }; | ||||
|   TestPartialPriorFactor3 factor(poseKey, translationIndices, | ||||
|       (Vector(2) << measurement.x(), measurement.z()).finished(), model); | ||||
| 
 | ||||
|   // Create a linearization point at the zero-error point
 | ||||
|   Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); | ||||
|   Pose3 pose = measurement; // Zero-error linearization point.
 | ||||
| 
 | ||||
|   // Calculate numerical derivatives.
 | ||||
|   Matrix expectedH1 = numericalDerivative11<Vector, Pose3>( | ||||
|       boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose); | ||||
|       boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); | ||||
| 
 | ||||
|   // Use the factor to calculate the derivative.
 | ||||
|   Matrix actualH1; | ||||
|   factor.evaluateError(pose, actualH1); | ||||
| 
 | ||||
|   // Verify we get the expected error.
 | ||||
|   CHECK(assert_equal(expectedH1, actualH1, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(PartialPriorFactor, JacobianFullRotation3) { | ||||
|   Key poseKey(1); | ||||
|   Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); | ||||
|   SharedNoiseModel model = NM::Isotropic::Sigma(3, 0.25); | ||||
| 
 | ||||
|   std::vector<size_t> rotationIndices = { kIndexRx, kIndexRy, kIndexRz }; | ||||
|   TestPartialPriorFactor3 factor(poseKey, rotationIndices, Rot3::Logmap(measurement.rotation()), model); | ||||
| 
 | ||||
|   Pose3 pose = measurement; // Zero-error linearization point.
 | ||||
| 
 | ||||
|   // Calculate numerical derivatives.
 | ||||
|   Matrix expectedH1 = numericalDerivative11<Vector, Pose3>( | ||||
|       boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); | ||||
| 
 | ||||
|   // Use the factor to calculate the derivative.
 | ||||
|   Matrix actualH1; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue