Change mask to indices and update factor docstring

release/4.3a0
Milo Knowles 2021-03-21 17:20:43 -04:00
parent 593e6e975d
commit 43c9f2ba26
1 changed files with 16 additions and 18 deletions

View File

@ -31,11 +31,9 @@ namespace gtsam {
* *
* The prior vector used in this factor is stored in compressed form, such that * 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 * 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 * the same order as VALUE::Logmap(). The provided indices will determine which components to
* in the error function. * 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 * @tparam VALUE is the type of variable the prior effects
*/ */
template<class VALUE> template<class VALUE>
@ -52,7 +50,7 @@ namespace gtsam {
typedef PartialPriorFactor<VALUE> This; typedef PartialPriorFactor<VALUE> This;
Vector prior_; ///< Measurement on tangent space parameters, in compressed form. Vector prior_; ///< Measurement on tangent space parameters, in compressed form.
std::vector<size_t> mask_; ///< Indices of the measured tangent space parameters. std::vector<size_t> indices_; ///< Indices of the measured tangent space parameters.
/** default constructor - only use for serialization */ /** default constructor - only use for serialization */
PartialPriorFactor() {} PartialPriorFactor() {}
@ -72,7 +70,7 @@ namespace gtsam {
PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
Base(model, key), Base(model, key),
prior_((Vector(1) << prior).finished()), prior_((Vector(1) << prior).finished()),
mask_(1, idx) { indices_(1, idx) {
assert(model->dim() == 1); assert(model->dim() == 1);
} }
@ -81,8 +79,8 @@ namespace gtsam {
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
Base(model, key), Base(model, key),
prior_(prior), prior_(prior),
mask_(indices) { indices_(indices) {
assert((size_t)prior_.size() == mask_.size()); assert((size_t)prior_.size() == indices_.size());
assert(model->dim() == (size_t)prior.size()); assert(model->dim() == (size_t)prior.size());
} }
@ -104,7 +102,7 @@ namespace gtsam {
const This *e = dynamic_cast<const This*> (&expected); const This *e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) && return e != nullptr && Base::equals(*e, tol) &&
gtsam::equal_with_abs_tol(this->prior_, e->prior_, 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 */ /** implement functions needed to derive from Factor */
@ -114,17 +112,17 @@ namespace gtsam {
if (H) { if (H) {
Matrix H_logmap; Matrix H_logmap;
T::Logmap(p, H_logmap); T::Logmap(p, H_logmap);
(*H) = Matrix::Zero(mask_.size(), T::dimension); (*H) = Matrix::Zero(indices_.size(), T::dimension);
for (size_t i = 0; i < mask_.size(); ++i) { for (size_t i = 0; i < indices_.size(); ++i) {
(*H).row(i) = H_logmap.row(mask_.at(i)); (*H).row(i) = H_logmap.row(indices_.at(i));
} }
} }
// FIXME: this was originally the generic retraction - may not produce same results. // 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); const Vector& full_logmap = T::Logmap(p);
Vector partial_logmap = Vector::Zero(T::dimension); Vector partial_logmap = Vector::Zero(T::dimension);
for (size_t i = 0; i < mask_.size(); ++i) { for (size_t i = 0; i < indices_.size(); ++i) {
partial_logmap(i) = full_logmap(mask_.at(i)); partial_logmap(i) = full_logmap(indices_.at(i));
} }
return partial_logmap - prior_; return partial_logmap - prior_;
@ -132,7 +130,7 @@ namespace gtsam {
// access // access
const Vector& prior() const { return prior_; } const Vector& prior() const { return prior_; }
const std::vector<size_t>& mask() const { return mask_; } const std::vector<size_t>& indices() const { return indices_; }
private: private:
/** Serialization function */ /** Serialization function */
@ -142,7 +140,7 @@ namespace gtsam {
ar & boost::serialization::make_nvp("NoiseModelFactor1", ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(prior_); ar & BOOST_SERIALIZATION_NVP(prior_);
ar & BOOST_SERIALIZATION_NVP(mask_); ar & BOOST_SERIALIZATION_NVP(indices_);
// ar & BOOST_SERIALIZATION_NVP(H_); // ar & BOOST_SERIALIZATION_NVP(H_);
} }
}; // \class PartialPriorFactor }; // \class PartialPriorFactor