12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Correct Jacobian in PartialPriorFactor, modify derived factors for compatibility

release/4.3a0
Milo Knowles 2021-03-21 17:10:00 -04:00
parent e6b7d9f133
commit 593e6e975d
2 changed files with 38 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
gtsam_unstable/dynamics/DynamicsPriors.h
View File

@ -50,16 +50,7 @@ struct DRollPrior : public gtsam::PartialPriorFactor<PoseRTV> {
struct VelocityPrior : public gtsam::PartialPriorFactor<PoseRTV> {
typedef gtsam::PartialPriorFactor<PoseRTV> 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<PoseRTV> {
* 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();
}
};

66
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -17,6 +17,8 @@
#pragma once
#include <Eigen/Core>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Lie.h>
@ -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<VALUE> Base;
typedef PartialPriorFactor<VALUE> This;
Vector prior_; ///< measurement on tangent space parameters, in compressed form
std::vector<size_t> 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<size_t> 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<size_t>& mask, const Vector& prior,
/** Indices Constructor: Specify the relevant measured indices in the tangent vector.*/
PartialPriorFactor(Key key, const std::vector<size_t>& 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<Matrix&> 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_.size(); ++i)
masked_logmap(i) = full_logmap(mask_[i]);
return masked_logmap - prior_;
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));
}
}
// FIXME: this was originally the generic retraction - may not produce same results.
// Compute the tangent vector representation of T, and optionally get the Jacobian.
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));
}
return partial_logmap - prior_;
}
// access
const Vector& prior() const { return prior_; }
const std::vector<size_t>& 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_.size(); ++i)
H_(i, mask_[i]) = 1.0;
}
private:
/** Serialization function */
@ -141,7 +143,7 @@ namespace gtsam {
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(prior_);
ar & BOOST_SERIALIZATION_NVP(mask_);
ar & BOOST_SERIALIZATION_NVP(H_);
// ar & BOOST_SERIALIZATION_NVP(H_);
}
}; // \class PartialPriorFactor