From 075293cf83d91e0a4391107c588765fdafdddfea Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Sat, 20 Mar 2021 17:44:03 -0400 Subject: [PATCH 01/11] Three examples of failing PartialPriorFactor Jacobians --- .../slam/tests/testPartialPriorFactor.cpp | 114 ++++++++++++++++++ 1 file changed, 114 insertions(+) create mode 100644 gtsam_unstable/slam/tests/testPartialPriorFactor.cpp diff --git a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp new file mode 100644 index 000000000..6cc70ef46 --- /dev/null +++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + + +// Pose representation is [ Rx Ry Rz Tx Ty Tz ]. +static const int kIndexRx = 0; +static const int kIndexRy = 1; +static const int kIndexRz = 2; +static const int kIndexTx = 3; +static const int kIndexTy = 4; +static const int kIndexTz = 5; + + +typedef PartialPriorFactor TestPartialPriorFactor; + +/// traits +namespace gtsam { +template<> +struct traits : public Testable { +}; +} + +/* ************************************************************************* */ +TEST(PartialPriorFactor, JacobianPartialTranslation) { + 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); + + TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().x(), 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)); + + // Calculate numerical derivatives. + Matrix expectedH1 = numericalDerivative11( + boost::bind(&TestPartialPriorFactor::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, JacobianFullTranslation) { + 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); + + std::vector translationIndices = { kIndexTx, kIndexTy, kIndexTz }; + TestPartialPriorFactor 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)); + + // Calculate numerical derivatives. + Matrix expectedH1 = numericalDerivative11( + boost::bind(&TestPartialPriorFactor::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, JacobianFullRotation) { + 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); + + std::vector rotationIndices = { kIndexRx, kIndexRy, kIndexRz }; + TestPartialPriorFactor factor(poseKey, rotationIndices, Rot3::Logmap(measurement.rotation()), 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)); + + // Calculate numerical derivatives. + Matrix expectedH1 = numericalDerivative11( + boost::bind(&TestPartialPriorFactor::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)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ From e6b7d9f13320ffc49372d50ba17f7fa77d1f2ce7 Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Sat, 20 Mar 2021 17:57:10 -0400 Subject: [PATCH 02/11] Add successful unit test for identity pose --- .../slam/tests/testPartialPriorFactor.cpp | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp index 6cc70ef46..8a77bc5b9 100644 --- a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp @@ -38,6 +38,32 @@ struct traits : public Testable }; } + +/* ************************************************************************* */ +TEST(PartialPriorFactor, JacobianAtIdentity) +{ + Key poseKey(1); + Pose3 measurement = Pose3::identity(); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25); + + TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().x(), model); + + // Create a linearization point at the zero-error point + Pose3 pose = Pose3::identity(); + + // Calculate numerical derivatives. + Matrix expectedH1 = numericalDerivative11( + boost::bind(&TestPartialPriorFactor::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, JacobianPartialTranslation) { Key poseKey(1); From 593e6e975d6fd6e1f7bfc9a19c1d4c84568a19ad Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Sun, 21 Mar 2021 17:10:00 -0400 Subject: [PATCH 03/11] Correct Jacobian in PartialPriorFactor, modify derived factors for compatibility --- gtsam_unstable/dynamics/DynamicsPriors.h | 32 ++--------- gtsam_unstable/slam/PartialPriorFactor.h | 68 ++++++++++++------------ 2 files changed, 38 insertions(+), 62 deletions(-) diff --git a/gtsam_unstable/dynamics/DynamicsPriors.h b/gtsam_unstable/dynamics/DynamicsPriors.h index 1e768ef2a..e286b5fdc 100644 --- a/gtsam_unstable/dynamics/DynamicsPriors.h +++ b/gtsam_unstable/dynamics/DynamicsPriors.h @@ -50,16 +50,7 @@ struct DRollPrior : public gtsam::PartialPriorFactor { struct VelocityPrior : public gtsam::PartialPriorFactor { typedef gtsam::PartialPriorFactor Base; VelocityPrior(Key key, const gtsam::Vector& vel, const gtsam::SharedNoiseModel& model) - : Base(key, model) { - this->prior_ = vel; - assert(vel.size() == 3); - this->mask_.resize(3); - this->mask_[0] = 6; - this->mask_[1] = 7; - this->mask_[2] = 8; - this->H_ = Matrix::Zero(3, 9); - this->fillH(); - } + : Base(key, {6, 7, 8}, vel, model) {} }; /** @@ -74,31 +65,14 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor { * Primary constructor allows for variable height of the "floor" */ DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel& model) - : Base(key, model) { - this->prior_ = Vector::Unit(4,0)*height; // [z, vz, roll, pitch] - this->mask_.resize(4); - this->mask_[0] = 5; // z = height - this->mask_[1] = 8; // vz - this->mask_[2] = 0; // roll - this->mask_[3] = 1; // pitch - this->H_ = Matrix::Zero(3, 9); - this->fillH(); - } + : Base(key, {5, 8, 0, 1}, Vector::Unit(4,0)*height, model) {} /** * Fully specify vector - use only for debugging */ DGroundConstraint(Key key, const Vector& constraint, const gtsam::SharedNoiseModel& model) - : Base(key, model) { + : Base(key, {5, 8, 0, 1}, constraint, model) { assert(constraint.size() == 4); - this->prior_ = constraint; // [z, vz, roll, pitch] - this->mask_.resize(4); - this->mask_[0] = 5; // z = height - this->mask_[1] = 8; // vz - this->mask_[2] = 0; // roll - this->mask_[3] = 1; // pitch - this->H_ = Matrix::Zero(3, 9); - this->fillH(); } }; diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index c1984992b..96b0d3147 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -17,6 +17,8 @@ #pragma once +#include + #include #include @@ -43,16 +45,14 @@ namespace gtsam { typedef VALUE T; protected: - // Concept checks on the variable type - currently requires Lie GTSAM_CONCEPT_LIE_TYPE(VALUE) typedef NoiseModelFactor1 Base; typedef PartialPriorFactor This; - Vector prior_; ///< measurement on tangent space parameters, in compressed form - std::vector mask_; ///< indices of values to constrain in compressed prior vector - Matrix H_; ///< Constant Jacobian - computed at creation + Vector prior_; ///< Measurement on tangent space parameters, in compressed form. + std::vector mask_; ///< Indices of the measured tangent space parameters. /** default constructor - only use for serialization */ PartialPriorFactor() {} @@ -68,20 +68,22 @@ namespace gtsam { ~PartialPriorFactor() override {} - /** Single Element Constructor: acts on a single parameter specified by idx */ + /** Single Element Constructor: Prior on a single parameter at index 'idx' in the tangent vector.*/ PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : - Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(Matrix::Zero(1, T::dimension)) { + Base(model, key), + prior_((Vector(1) << prior).finished()), + mask_(1, idx) { assert(model->dim() == 1); - this->fillH(); } - /** Indices Constructor: specify the mask with a set of indices */ - PartialPriorFactor(Key key, const std::vector& mask, const Vector& prior, + /** Indices Constructor: Specify the relevant measured indices in the tangent vector.*/ + PartialPriorFactor(Key key, const std::vector& indices, const Vector& prior, const SharedNoiseModel& model) : - Base(model, key), prior_(prior), mask_(mask), H_(Matrix::Zero(mask.size(), T::dimension)) { - assert((size_t)prior_.size() == mask.size()); - assert(model->dim() == (size_t) prior.size()); - this->fillH(); + Base(model, key), + prior_(prior), + mask_(indices) { + assert((size_t)prior_.size() == mask_.size()); + assert(model->dim() == (size_t)prior.size()); } /// @return a deep copy of this factor @@ -107,30 +109,30 @@ namespace gtsam { /** implement functions needed to derive from Factor */ - /** vector of errors */ + /** Returns a vector of errors for the measured tangent parameters. */ Vector evaluateError(const T& p, boost::optional H = boost::none) const override { - if (H) (*H) = H_; - // FIXME: this was originally the generic retraction - may not produce same results - Vector full_logmap = T::Logmap(p); -// Vector full_logmap = T::identity().localCoordinates(p); // Alternate implementation - Vector masked_logmap = Vector::Zero(this->dim()); - for (size_t i=0; i& mask() const { return mask_; } - const Matrix& H() const { return H_; } - - protected: - - /** Constructs the jacobian matrix in place */ - void fillH() { - for (size_t i=0; i& mask() const { return mask_; } private: /** Serialization function */ @@ -141,7 +143,7 @@ namespace gtsam { boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); ar & BOOST_SERIALIZATION_NVP(mask_); - ar & BOOST_SERIALIZATION_NVP(H_); + // ar & BOOST_SERIALIZATION_NVP(H_); } }; // \class PartialPriorFactor From 43c9f2ba268c964571b5a96320442c14688a9195 Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Sun, 21 Mar 2021 17:20:43 -0400 Subject: [PATCH 04/11] Change mask to indices and update factor docstring --- gtsam_unstable/slam/PartialPriorFactor.h | 34 +++++++++++------------- 1 file changed, 16 insertions(+), 18 deletions(-) diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 96b0d3147..da6e0d535 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -31,11 +31,9 @@ namespace gtsam { * * The prior vector used in this factor is stored in compressed form, such that * it only contains values for measurements that are to be compared, and they are in - * the same order as VALUE::Logmap(). The mask will determine which components to extract - * in the error function. + * the same order as VALUE::Logmap(). The provided indices will determine which components to + * extract in the error function. * - * For practical use, it would be good to subclass this factor and have the class type - * construct the mask. * @tparam VALUE is the type of variable the prior effects */ template @@ -51,8 +49,8 @@ namespace gtsam { typedef NoiseModelFactor1 Base; typedef PartialPriorFactor This; - Vector prior_; ///< Measurement on tangent space parameters, in compressed form. - std::vector mask_; ///< Indices of the measured tangent space parameters. + Vector prior_; ///< Measurement on tangent space parameters, in compressed form. + std::vector indices_; ///< Indices of the measured tangent space parameters. /** default constructor - only use for serialization */ PartialPriorFactor() {} @@ -72,7 +70,7 @@ namespace gtsam { PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : Base(model, key), prior_((Vector(1) << prior).finished()), - mask_(1, idx) { + indices_(1, idx) { assert(model->dim() == 1); } @@ -81,8 +79,8 @@ namespace gtsam { const SharedNoiseModel& model) : Base(model, key), prior_(prior), - mask_(indices) { - assert((size_t)prior_.size() == mask_.size()); + indices_(indices) { + assert((size_t)prior_.size() == indices_.size()); assert(model->dim() == (size_t)prior.size()); } @@ -104,7 +102,7 @@ namespace gtsam { const This *e = dynamic_cast (&expected); return e != nullptr && Base::equals(*e, tol) && gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) && - this->mask_ == e->mask_; + this->indices_ == e->indices_; } /** implement functions needed to derive from Factor */ @@ -114,17 +112,17 @@ namespace gtsam { if (H) { Matrix H_logmap; T::Logmap(p, H_logmap); - (*H) = Matrix::Zero(mask_.size(), T::dimension); - for (size_t i = 0; i < mask_.size(); ++i) { - (*H).row(i) = H_logmap.row(mask_.at(i)); + (*H) = Matrix::Zero(indices_.size(), T::dimension); + for (size_t i = 0; i < indices_.size(); ++i) { + (*H).row(i) = H_logmap.row(indices_.at(i)); } } // FIXME: this was originally the generic retraction - may not produce same results. - // Compute the tangent vector representation of T, and optionally get the Jacobian. + // Compute the tangent vector representation of T. const Vector& full_logmap = T::Logmap(p); Vector partial_logmap = Vector::Zero(T::dimension); - for (size_t i = 0; i < mask_.size(); ++i) { - partial_logmap(i) = full_logmap(mask_.at(i)); + for (size_t i = 0; i < indices_.size(); ++i) { + partial_logmap(i) = full_logmap(indices_.at(i)); } return partial_logmap - prior_; @@ -132,7 +130,7 @@ namespace gtsam { // access const Vector& prior() const { return prior_; } - const std::vector& mask() const { return mask_; } + const std::vector& indices() const { return indices_; } private: /** Serialization function */ @@ -142,7 +140,7 @@ namespace gtsam { ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); - ar & BOOST_SERIALIZATION_NVP(mask_); + ar & BOOST_SERIALIZATION_NVP(indices_); // ar & BOOST_SERIALIZATION_NVP(H_); } }; // \class PartialPriorFactor From 4b29c0370dacab9fc8cafe12e586a0f7d8715b5f Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Sun, 21 Mar 2021 20:46:26 -0400 Subject: [PATCH 05/11] Prefer localCoordinates over logmap --- gtsam_unstable/slam/PartialPriorFactor.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index da6e0d535..bc28a6830 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -111,15 +111,14 @@ namespace gtsam { Vector evaluateError(const T& p, boost::optional H = boost::none) const override { if (H) { Matrix H_logmap; - T::Logmap(p, H_logmap); + p.localCoordinates(T::identity(), H_logmap); (*H) = Matrix::Zero(indices_.size(), T::dimension); for (size_t i = 0; i < indices_.size(); ++i) { (*H).row(i) = H_logmap.row(indices_.at(i)); } } - // FIXME: this was originally the generic retraction - may not produce same results. - // Compute the tangent vector representation of T. - const Vector& full_logmap = T::Logmap(p); + // Compute the tangent vector representation of T and select relevant parameters. + const Vector& full_logmap = p.localCoordinates(T::identity()); Vector partial_logmap = Vector::Zero(T::dimension); for (size_t i = 0; i < indices_.size(); ++i) { partial_logmap(i) = full_logmap(indices_.at(i)); From ef2cd5dab54e87ef864b7fda2e29de47f6896c3a Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Fri, 26 Mar 2021 08:59:44 -0400 Subject: [PATCH 06/11] Fix x/y mismatch in unit tests --- gtsam_unstable/slam/tests/testPartialPriorFactor.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp index 8a77bc5b9..55be97d9d 100644 --- a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp @@ -38,7 +38,6 @@ struct traits : public Testable }; } - /* ************************************************************************* */ TEST(PartialPriorFactor, JacobianAtIdentity) { @@ -46,7 +45,7 @@ TEST(PartialPriorFactor, JacobianAtIdentity) Pose3 measurement = Pose3::identity(); SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25); - TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().x(), model); + TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().y(), model); // Create a linearization point at the zero-error point Pose3 pose = Pose3::identity(); @@ -63,14 +62,13 @@ TEST(PartialPriorFactor, JacobianAtIdentity) CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } - /* ************************************************************************* */ TEST(PartialPriorFactor, JacobianPartialTranslation) { 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); - TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().x(), model); + TestPartialPriorFactor 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)); From 74b92efd89655659c34f89749c11e861deefc10c Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Fri, 26 Mar 2021 09:55:25 -0400 Subject: [PATCH 07/11] Add constructor tests and extend tests to Pose2 --- .../slam/tests/testPartialPriorFactor.cpp | 206 +++++++++++++++--- 1 file changed, 176 insertions(+), 30 deletions(-) diff --git a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp index 55be97d9d..8a3a2a63b 100644 --- a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp @@ -11,6 +11,7 @@ #include #include +#include #include #include #include @@ -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 TestPartialPriorFactor; +typedef PartialPriorFactor TestPartialPriorFactor2; +typedef PartialPriorFactor TestPartialPriorFactor3; +typedef std::vector Indices; /// traits namespace gtsam { template<> -struct traits : public Testable { -}; +struct traits : public Testable {}; + +template<> +struct traits : public Testable {}; } /* ************************************************************************* */ -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({ 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(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({ 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( + 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( + 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( + 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({ 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(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(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( - 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( - 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 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( - 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 rotationIndices = { kIndexRx, kIndexRy, kIndexRz }; - TestPartialPriorFactor factor(poseKey, rotationIndices, Rot3::Logmap(measurement.rotation()), model); + std::vector 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( - 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 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( + boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; From 05fad78ce917a4024d6a1c48b4ba35b3bd997be7 Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Fri, 26 Mar 2021 10:09:05 -0400 Subject: [PATCH 08/11] Switch to cleaner T::Logmap --- gtsam_unstable/slam/PartialPriorFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index bc28a6830..7568a222f 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -111,14 +111,14 @@ namespace gtsam { Vector evaluateError(const T& p, boost::optional H = boost::none) const override { if (H) { Matrix H_logmap; - p.localCoordinates(T::identity(), H_logmap); + T::Logmap(p, H_logmap); (*H) = Matrix::Zero(indices_.size(), T::dimension); for (size_t i = 0; i < indices_.size(); ++i) { (*H).row(i) = H_logmap.row(indices_.at(i)); } } // Compute the tangent vector representation of T and select relevant parameters. - const Vector& full_logmap = p.localCoordinates(T::identity()); + const Vector& full_logmap = T::Logmap(p); Vector partial_logmap = Vector::Zero(T::dimension); for (size_t i = 0; i < indices_.size(); ++i) { partial_logmap(i) = full_logmap(indices_.at(i)); From 909b5500f8a43e4aa07c15086fd586069cd869c8 Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Fri, 26 Mar 2021 11:26:27 -0400 Subject: [PATCH 09/11] Fix incorrect Vector dimension that was causing CI failures --- gtsam_unstable/slam/PartialPriorFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 7568a222f..e92c23cb8 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -119,7 +119,7 @@ namespace gtsam { } // Compute the tangent vector representation of T and select relevant parameters. const Vector& full_logmap = T::Logmap(p); - Vector partial_logmap = Vector::Zero(T::dimension); + Vector partial_logmap = Vector::Zero(indices_.size()); for (size_t i = 0; i < indices_.size(); ++i) { partial_logmap(i) = full_logmap(indices_.at(i)); } From a0ff5e3886f751b6d2093e8f6d94efa869816510 Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Fri, 26 Mar 2021 14:36:43 -0400 Subject: [PATCH 10/11] Add LocalCoordinates() to ProductLieGroup and remove unnecessary include to reduce compile memory --- gtsam/base/ProductLieGroup.h | 3 ++ gtsam_unstable/dynamics/DynamicsPriors.h | 28 +++++++++++++------ gtsam_unstable/dynamics/PoseRTV.h | 1 + gtsam_unstable/slam/PartialPriorFactor.h | 16 +++++------ .../slam/tests/testPartialPriorFactor.cpp | 1 - 5 files changed, 30 insertions(+), 19 deletions(-) diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 458538003..a2dcf738e 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -161,6 +161,9 @@ public: } return v; } + static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { + return Logmap(p, Hp); + } ProductLieGroup expmap(const TangentVector& v) const { return compose(ProductLieGroup::Expmap(v)); } diff --git a/gtsam_unstable/dynamics/DynamicsPriors.h b/gtsam_unstable/dynamics/DynamicsPriors.h index e286b5fdc..d4ebcba19 100644 --- a/gtsam_unstable/dynamics/DynamicsPriors.h +++ b/gtsam_unstable/dynamics/DynamicsPriors.h @@ -9,20 +9,28 @@ #pragma once -#include - #include +#include namespace gtsam { +// Indices of relevant variables in the PoseRTV tangent vector: +// [ rx ry rz tx ty tz vx vy vz ] +static const size_t kRollIndex = 0; +static const size_t kPitchIndex = 1; +static const size_t kHeightIndex = 5; +static const size_t kVelocityZIndex = 8; +static const std::vector kVelocityIndices = { 6, 7, 8 }; + /** - * Forces the value of the height in a PoseRTV to a specific value + * Forces the value of the height (z) in a PoseRTV to a specific value. * Dim: 1 */ struct DHeightPrior : public gtsam::PartialPriorFactor { typedef gtsam::PartialPriorFactor Base; + DHeightPrior(Key key, double height, const gtsam::SharedNoiseModel& model) - : Base(key, 5, height, model) { } + : Base(key, kHeightIndex, height, model) {} }; /** @@ -35,11 +43,11 @@ struct DRollPrior : public gtsam::PartialPriorFactor { /** allows for explicit roll parameterization - uses canonical coordinate */ DRollPrior(Key key, double wx, const gtsam::SharedNoiseModel& model) - : Base(key, 0, wx, model) { } + : Base(key, kRollIndex, wx, model) { } /** Forces roll to zero */ DRollPrior(Key key, const gtsam::SharedNoiseModel& model) - : Base(key, 0, 0.0, model) { } + : Base(key, kRollIndex, 0.0, model) { } }; /** @@ -49,8 +57,9 @@ struct DRollPrior : public gtsam::PartialPriorFactor { */ struct VelocityPrior : public gtsam::PartialPriorFactor { typedef gtsam::PartialPriorFactor Base; + VelocityPrior(Key key, const gtsam::Vector& vel, const gtsam::SharedNoiseModel& model) - : Base(key, {6, 7, 8}, vel, model) {} + : Base(key, kVelocityIndices, vel, model) {} }; /** @@ -65,13 +74,14 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor { * Primary constructor allows for variable height of the "floor" */ DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel& model) - : Base(key, {5, 8, 0, 1}, Vector::Unit(4,0)*height, model) {} + : Base(key, { kHeightIndex, kVelocityZIndex, kRollIndex, kPitchIndex }, + Vector::Unit(4, 0)*height, model) {} /** * Fully specify vector - use only for debugging */ DGroundConstraint(Key key, const Vector& constraint, const gtsam::SharedNoiseModel& model) - : Base(key, {5, 8, 0, 1}, constraint, model) { + : Base(key, { kHeightIndex, kVelocityZIndex, kRollIndex, kPitchIndex }, constraint, model) { assert(constraint.size() == 4); } }; diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index b1cfb6f30..c573de2b6 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -80,6 +80,7 @@ public: using Base::Dim; using Base::retract; using Base::localCoordinates; + using Base::LocalCoordinates; /// @} /// @name measurement functions diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index e92c23cb8..910ee7bbd 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -17,8 +17,6 @@ #pragma once -#include - #include #include @@ -110,21 +108,21 @@ namespace gtsam { /** Returns a vector of errors for the measured tangent parameters. */ Vector evaluateError(const T& p, boost::optional H = boost::none) const override { if (H) { - Matrix H_logmap; - T::Logmap(p, H_logmap); + Matrix H_local; + T::LocalCoordinates(p, H_local); (*H) = Matrix::Zero(indices_.size(), T::dimension); for (size_t i = 0; i < indices_.size(); ++i) { - (*H).row(i) = H_logmap.row(indices_.at(i)); + (*H).row(i) = H_local.row(indices_.at(i)); } } // Compute the tangent vector representation of T and select relevant parameters. - const Vector& full_logmap = T::Logmap(p); - Vector partial_logmap = Vector::Zero(indices_.size()); + const Vector& full_tangent = T::LocalCoordinates(p); + Vector partial_tangent = Vector::Zero(indices_.size()); for (size_t i = 0; i < indices_.size(); ++i) { - partial_logmap(i) = full_logmap(indices_.at(i)); + partial_tangent(i) = full_tangent(indices_.at(i)); } - return partial_logmap - prior_; + return partial_tangent - prior_; } // access diff --git a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp index 8a3a2a63b..ae8074f43 100644 --- a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp @@ -30,7 +30,6 @@ static const int kIndexTx = 3; static const int kIndexTy = 4; static const int kIndexTz = 5; - typedef PartialPriorFactor TestPartialPriorFactor2; typedef PartialPriorFactor TestPartialPriorFactor3; typedef std::vector Indices; From 1f12f82e01506b4596c06b97292d405707aa5a66 Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Fri, 26 Mar 2021 17:29:47 -0400 Subject: [PATCH 11/11] Fix Rot3::LocalCoordinates runtime error when using Cayley map --- gtsam_unstable/slam/PartialPriorFactor.h | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 910ee7bbd..52a57b945 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -107,16 +107,23 @@ namespace gtsam { /** Returns a vector of errors for the measured tangent parameters. */ Vector evaluateError(const T& p, boost::optional H = boost::none) 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 + const Vector full_tangent = T::Logmap(p, H ? &H_local : nullptr); + #endif + if (H) { - Matrix H_local; - T::LocalCoordinates(p, H_local); (*H) = Matrix::Zero(indices_.size(), T::dimension); for (size_t i = 0; i < indices_.size(); ++i) { (*H).row(i) = H_local.row(indices_.at(i)); } } - // Compute the tangent vector representation of T and select relevant parameters. - const Vector& full_tangent = T::LocalCoordinates(p); + // Select relevant parameters from the tangent vector. Vector partial_tangent = Vector::Zero(indices_.size()); for (size_t i = 0; i < indices_.size(); ++i) { partial_tangent(i) = full_tangent(indices_.at(i));