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;