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 1e768ef2a..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,17 +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, 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, kVelocityIndices, vel, model) {} }; /** @@ -74,31 +74,15 @@ 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, { 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, model) { + : Base(key, { kHeightIndex, kVelocityZIndex, kRollIndex, kPitchIndex }, 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/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 c1984992b..52a57b945 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -29,11 +29,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 @@ -43,16 +41,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 indices_; ///< Indices of the measured tangent space parameters. /** default constructor - only use for serialization */ PartialPriorFactor() {} @@ -68,20 +64,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()), + indices_(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), + indices_(indices) { + assert((size_t)prior_.size() == indices_.size()); + assert(model->dim() == (size_t)prior.size()); } /// @return a deep copy of this factor @@ -102,35 +100,41 @@ 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 */ - /** 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 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) { + (*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)); + } + } + // 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)); + } + + return partial_tangent - prior_; } // access const Vector& prior() const { return prior_; } - const std::vector& 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& indices() const { return indices_; } private: /** Serialization function */ @@ -140,8 +144,8 @@ 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(H_); + ar & BOOST_SERIALIZATION_NVP(indices_); + // ar & BOOST_SERIALIZATION_NVP(H_); } }; // \class PartialPriorFactor diff --git a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp new file mode 100644 index 000000000..ae8074f43 --- /dev/null +++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp @@ -0,0 +1,283 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include + +using namespace std; +using namespace gtsam; + +namespace NM = gtsam::noiseModel; + +// 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; +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; + +/// traits +namespace gtsam { +template<> +struct traits : public Testable {}; + +template<> +struct traits : public Testable {}; +} + +/* ************************************************************************* */ +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 = NM::Isotropic::Sigma(1, 0.25); + + TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), 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; + factor.evaluateError(pose, actualH1); + + // Verify we get the expected error. + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); +} + +/* ************************************************************************* */ +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 = NM::Isotropic::Sigma(1, 0.25); + + TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), 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; + factor.evaluateError(pose, actualH1); + + // Verify we get the expected error. + 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)); + SharedNoiseModel model = NM::Isotropic::Sigma(3, 0.25); + + std::vector translationIndices = { kIndexTx, kIndexTy, kIndexTz }; + TestPartialPriorFactor3 factor(poseKey, translationIndices, measurement.translation(), model); + + // Create a linearization point at the zero-error point + 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; + factor.evaluateError(pose, actualH1); + + // Verify we get the expected error. + 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)); + 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); + + 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; + 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; + 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); } +/* ************************************************************************* */