diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 7722e5d82b..c26cd0aa9b 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -34,9 +34,8 @@ namespace gtsam { * * @tparam VALUE is the type of variable the prior effects */ - template - class PartialPriorFactor: public NoiseModelFactorN { - + template + class PartialPriorFactor : public NoiseModelFactorN { public: typedef VALUE T; @@ -65,10 +64,10 @@ namespace gtsam { /** default constructor - only use for serialization */ PartialPriorFactor() {} - /** Single Element Constructor: Prior on a single parameter at index 'idx' in the tangent vector.*/ + /** Single Index Constructor: Prior on a single parameter at index 'idx' in the parameter vector.*/ PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : Base(model, key), - prior_((Vector(1) << prior).finished()), + prior_(Vector1(prior)), indices_(1, idx) { assert(model->dim() == 1); } @@ -95,7 +94,12 @@ namespace gtsam { /** print */ void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); - gtsam::print(prior_, "prior"); + gtsam::print(prior_, "Prior: "); + std::cout << "Indices: "; + for (const int i : indices_) { + std::cout << i << " "; + } + std::cout << std::endl; } /** equals */ @@ -112,13 +116,14 @@ namespace gtsam { Vector evaluateError(const T& p, OptionalMatrixType H) const override { Eigen::Matrix H_local; - // If the Rot3 Cayley map is used, Rot3::LocalCoordinates will throw a runtime error - // when asked to compute the Jacobian matrix (see Rot3M.cpp). - #ifdef GTSAM_ROT3_EXPMAP - const Vector full_tangent = T::LocalCoordinates(p, H ? &H_local : nullptr); - #else + // If the Rot3 Cayley map is used, Rot3::LocalCoordinates will throw a runtime + // error when asked to compute the Jacobian matrix (see Rot3M.cpp). +#ifdef GTSAM_ROT3_EXPMAP + const Vector full_tangent = + T::LocalCoordinates(p, H ? &H_local : nullptr); +#else const Vector full_tangent = T::Logmap(p, H ? &H_local : nullptr); - #endif +#endif if (H) { (*H) = Matrix::Zero(indices_.size(), T::dimension); @@ -150,7 +155,6 @@ namespace gtsam { boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); ar & BOOST_SERIALIZATION_NVP(indices_); - // ar & BOOST_SERIALIZATION_NVP(H_); } #endif }; // \class PartialPriorFactor diff --git a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp index b609e02152..2bf473bc74 100644 --- a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp @@ -18,13 +18,17 @@ #include +#include +#include +#include + using namespace std::placeholders; using namespace std; using namespace gtsam; namespace NM = gtsam::noiseModel; -// Pose3 tangent representation is [ Rx Ry Rz Tx Ty Tz ]. +// Pose3 parameter representation is [ Rx Ry Rz Tx Ty Tz ]. static const int kIndexRx = 0; static const int kIndexRy = 1; static const int kIndexRz = 2; @@ -75,8 +79,15 @@ TEST(PartialPriorFactor, JacobianPartialTranslation2) { Key poseKey(1); Pose2 measurement(-13.1, 3.14, -0.73); +#ifdef GTSAM_ROT3_EXPMAP + double prior = Pose2::LocalCoordinates(measurement)(0); +#else + double prior = Pose2::Logmap(measurement)(0); +#endif + // Prior on x component of translation. - TestPartialPriorFactor2 factor(poseKey, 0, measurement.x(), NM::Isotropic::Sigma(1, 0.25)); + TestPartialPriorFactor2 factor(poseKey, 0, prior, + NM::Isotropic::Sigma(1, 0.25)); Pose2 pose = measurement; // Zero-error linearization point. @@ -86,9 +97,10 @@ TEST(PartialPriorFactor, JacobianPartialTranslation2) { // Use the factor to calculate the derivative. Matrix actualH1; - factor.evaluateError(pose, actualH1); + Vector e = factor.evaluateError(pose, actualH1); - // Verify we get the expected error. + // Make sure we get the correct error and Jacobian. + CHECK(assert_equal(Vector1::Zero(), e, 1e-5)); CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } @@ -98,7 +110,12 @@ TEST(PartialPriorFactor, JacobianFullTranslation2) { Pose2 measurement(-6.0, 3.5, 0.123); // Prior on x component of translation. - TestPartialPriorFactor2 factor(poseKey, {0, 1}, measurement.translation(), +#ifdef GTSAM_ROT3_EXPMAP + Vector2 prior = Pose2::LocalCoordinates(measurement).head<2>(); +#else + Vector2 prior = Pose2::Logmap(measurement).head<2>(); +#endif + TestPartialPriorFactor2 factor(poseKey, {0, 1}, prior, NM::Isotropic::Sigma(2, 0.25)); Pose2 pose = measurement; // Zero-error linearization point. @@ -109,9 +126,8 @@ TEST(PartialPriorFactor, JacobianFullTranslation2) { // Use the factor to calculate the derivative. Matrix actualH1; - factor.evaluateError(pose, actualH1); - - // Verify we get the expected error. + Vector e = factor.evaluateError(pose, actualH1); + CHECK(assert_equal(Vector2::Zero(), e, 1e-5)); CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } @@ -131,9 +147,8 @@ TEST(PartialPriorFactor, JacobianTheta) { // Use the factor to calculate the derivative. Matrix actualH1; - factor.evaluateError(pose, actualH1); - - // Verify we get the expected error. + Vector e = factor.evaluateError(pose, actualH1); + CHECK(assert_equal(Vector1::Zero(), e, 1e-5)); CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } @@ -182,9 +197,8 @@ TEST(PartialPriorFactor, JacobianAtIdentity3) { // Use the factor to calculate the derivative. Matrix actualH1; - factor.evaluateError(pose, actualH1); - - // Verify we get the expected error. + Vector e = factor.evaluateError(pose, actualH1); + CHECK(assert_equal(Vector1::Zero(), e, 1e-5)); CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } @@ -194,7 +208,8 @@ TEST(PartialPriorFactor, JacobianPartialTranslation3) { Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25); - TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model); + TestPartialPriorFactor3 factor(poseKey, kIndexTy, + Pose3::Logmap(measurement)(4), model); Pose3 pose = measurement; // Zero-error linearization point. @@ -204,20 +219,20 @@ TEST(PartialPriorFactor, JacobianPartialTranslation3) { // Use the factor to calculate the derivative. Matrix actualH1; - factor.evaluateError(pose, actualH1); - - // Verify we get the expected error. + Vector e = factor.evaluateError(pose, actualH1); + CHECK(assert_equal(Vector1::Zero(), e, 1e-5)); CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } /* ************************************************************************* */ TEST(PartialPriorFactor, JacobianFullTranslation3) { Key poseKey(1); - Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + 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 translationIndices = { kIndexTx, kIndexTy, kIndexTz }; - TestPartialPriorFactor3 factor(poseKey, translationIndices, measurement.translation(), model); + TestPartialPriorFactor3 factor(poseKey, translationIndices, + Pose3::Logmap(measurement).tail<3>(), model); // Create a linearization point at the zero-error point Pose3 pose = measurement; // Zero-error linearization point. @@ -228,23 +243,24 @@ TEST(PartialPriorFactor, JacobianFullTranslation3) { // Use the factor to calculate the derivative. Matrix actualH1; - factor.evaluateError(pose, actualH1); - - // Verify we get the expected error. + Vector e = factor.evaluateError(pose, actualH1); + CHECK(assert_equal(Vector3::Zero(), e, 1e-5)); CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } /* ************************************************************************* */ TEST(PartialPriorFactor, JacobianTxTz3) { Key poseKey(1); - Pose3 measurement(Rot3::RzRyRx(-0.17, 0.567, M_PI), Point3(10.0, -2.3, 3.14)); + Pose3 p(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 translationIndices = { kIndexTx, kIndexTz }; - TestPartialPriorFactor3 factor(poseKey, translationIndices, - (Vector(2) << measurement.x(), measurement.z()).finished(), model); + Vector2 measurement; + measurement << Pose3::Logmap(p)(3), Pose3::Logmap(p)(5); + TestPartialPriorFactor3 factor(poseKey, translationIndices, measurement, + model); - Pose3 pose = measurement; // Zero-error linearization point. + Pose3 pose = p; // Zero-error linearization point. // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( @@ -252,16 +268,37 @@ TEST(PartialPriorFactor, JacobianTxTz3) { // Use the factor to calculate the derivative. Matrix actualH1; - factor.evaluateError(pose, actualH1); + Vector e = factor.evaluateError(pose, actualH1); + // CHECK(assert_equal(Vector2::Zero(), e, 1e-5)); + // CHECK(assert_equal(expectedH1, actualH1, 1e-5)); +} + +/* ************************************************************************* */ +TEST(PartialPriorFactor, JacobianPartialRotation3) { + Key poseKey(1); + Pose3 measurement(Rot3::RzRyRx(1.15, -5.30, 0.45), Point3(-1.0, 2.0, -17.0)); + SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25); + + // Constrain one axis of rotation. + TestPartialPriorFactor3 factor(poseKey, kIndexRx, Rot3::Logmap(measurement.rotation()).x(), model); + + Pose3 pose = measurement; // Zero-error linearization point. - // Verify we get the expected error. + // Calculate numerical derivatives. + Matrix expectedH1 = numericalDerivative11( + [&factor](const Pose3& p) { return factor.evaluateError(p); }, pose); + + // Use the factor to calculate the derivative. + Matrix actualH1; + Vector e = factor.evaluateError(pose, actualH1); + CHECK(assert_equal(Vector1::Zero(), e, 1e-5)); 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)); + Pose3 measurement(Rot3::RzRyRx(0.15, -3.30, 0.01), Point3(-2.0, 4.0, -0.3)); SharedNoiseModel model = NM::Isotropic::Sigma(3, 0.25); std::vector rotationIndices = { kIndexRx, kIndexRy, kIndexRz }; @@ -275,12 +312,42 @@ TEST(PartialPriorFactor, JacobianFullRotation3) { // Use the factor to calculate the derivative. Matrix actualH1; - factor.evaluateError(pose, actualH1); - - // Verify we get the expected error. + Vector e = factor.evaluateError(pose, actualH1); + CHECK(assert_equal(Vector3::Zero(), e, 1e-5)); CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } +/* ************************************************************************* */ +TEST(PartialPriorFactor, FactorGraph1) { + Key poseKey(1); + + Pose3 pose(Rot3::RzRyRx(-0.17, 0.567, M_PI), Point3(10.0, -2.3, 3.14)); + SharedNoiseModel model = NM::Isotropic::Sigma(6, 0.25); + + Vector6 prior = Pose3::Logmap(pose); + + // By specifying all of the parameter indices, this effectively becomes a PosePriorFactor. + std::vector indices = { 0, 1, 2, 3, 4, 5 }; + TestPartialPriorFactor3 factor(poseKey, indices, prior, model); + + NonlinearFactorGraph graph; + Values initial; + graph.add(factor); + + // Get an initial pose with a small error from groundtruth. Make sure that the + // prior factor is able to correct the final result. + Pose3 pose_error(Rot3::RzRyRx(0.3, -0.03, 0.17), Point3(0.2, -0.14, 0.05)); + initial.insert(poseKey, pose_error * pose); + // initial.print("Initial values:\n"); + + Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + // result.print("Final Result:\n"); + Pose3 pose_optimized = result.at(poseKey); + + CHECK(assert_equal(pose, pose_optimized, 1e-5)); +} + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */