From 593e6e975d6fd6e1f7bfc9a19c1d4c84568a19ad Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Sun, 21 Mar 2021 17:10:00 -0400 Subject: [PATCH] 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