gtsam  4.1.0
gtsam
PartialPriorFactor.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 #pragma once
19 
21 #include <gtsam/base/Lie.h>
22 
23 namespace gtsam {
24 
39  template<class VALUE>
40  class PartialPriorFactor: public NoiseModelFactor1<VALUE> {
41 
42  public:
43  typedef VALUE T;
44 
45  protected:
46 
47  // Concept checks on the variable type - currently requires Lie
48  GTSAM_CONCEPT_LIE_TYPE(VALUE)
49 
52 
53  Vector prior_;
54  std::vector<size_t> mask_;
55  Matrix H_;
56 
59 
65  : Base(model, key) {}
66 
67  public:
68 
69  virtual ~PartialPriorFactor() {}
70 
72  PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
73  Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(Matrix::Zero(1, T::dimension)) {
74  assert(model->dim() == 1);
75  this->fillH();
76  }
77 
79  PartialPriorFactor(Key key, const std::vector<size_t>& mask, const Vector& prior,
80  const SharedNoiseModel& model) :
81  Base(model, key), prior_(prior), mask_(mask), H_(Matrix::Zero(mask.size(), T::dimension)) {
82  assert((size_t)prior_.size() == mask.size());
83  assert(model->dim() == (size_t) prior.size());
84  this->fillH();
85  }
86 
88  gtsam::NonlinearFactor::shared_ptr clone() const override {
89  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
90  gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
91 
95  void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
96  Base::print(s, keyFormatter);
97  gtsam::print(prior_, "prior");
98  }
99 
101  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
102  const This *e = dynamic_cast<const This*> (&expected);
103  return e != nullptr && Base::equals(*e, tol) &&
104  gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) &&
105  this->mask_ == e->mask_;
106  }
107 
111  Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const override {
112  if (H) (*H) = H_;
113  // FIXME: this was originally the generic retraction - may not produce same results
114  Vector full_logmap = T::Logmap(p);
115 // Vector full_logmap = T::identity().localCoordinates(p); // Alternate implementation
116  Vector masked_logmap = Vector::Zero(this->dim());
117  for (size_t i=0; i<mask_.size(); ++i)
118  masked_logmap(i) = full_logmap(mask_[i]);
119  return masked_logmap - prior_;
120  }
121 
122  // access
123  const Vector& prior() const { return prior_; }
124  const std::vector<size_t>& mask() const { return mask_; }
125  const Matrix& H() const { return H_; }
126 
127  protected:
128 
130  void fillH() {
131  for (size_t i=0; i<mask_.size(); ++i)
132  H_(i, mask_[i]) = 1.0;
133  }
134 
135  private:
137  friend class boost::serialization::access;
138  template<class ARCHIVE>
139  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
140  ar & boost::serialization::make_nvp("NoiseModelFactor1",
141  boost::serialization::base_object<Base>(*this));
142  ar & BOOST_SERIALIZATION_NVP(prior_);
143  ar & BOOST_SERIALIZATION_NVP(mask_);
144  ar & BOOST_SERIALIZATION_NVP(H_);
145  }
146  }; // \class PartialPriorFactor
147 
148 }
gtsam::PartialPriorFactor::PartialPriorFactor
PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel &model)
Single Element Constructor: acts on a single parameter specified by idx.
Definition: PartialPriorFactor.h:72
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition: Matrix.h:84
gtsam::PartialPriorFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: PartialPriorFactor.h:88
gtsam::PartialPriorFactor::mask_
std::vector< size_t > mask_
indices of values to constrain in compressed prior vector
Definition: PartialPriorFactor.h:54
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
gtsam::PartialPriorFactor::PartialPriorFactor
PartialPriorFactor()
default constructor - only use for serialization
Definition: PartialPriorFactor.h:58
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::NoiseModelFactor1
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:271
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::PartialPriorFactor::evaluateError
Vector evaluateError(const T &p, boost::optional< Matrix & > H=boost::none) const override
implement functions needed to derive from Factor
Definition: PartialPriorFactor.h:111
gtsam::PartialPriorFactor::PartialPriorFactor
PartialPriorFactor(Key key, const std::vector< size_t > &mask, const Vector &prior, const SharedNoiseModel &model)
Indices Constructor: specify the mask with a set of indices.
Definition: PartialPriorFactor.h:79
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::NoiseModelFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: NonlinearFactor.h:198
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::PartialPriorFactor
A class for a soft partial prior on any Lie type, with a mask over Expmap parameters.
Definition: PartialPriorFactor.h:40
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::PartialPriorFactor::fillH
void fillH()
Constructs the jacobian matrix in place.
Definition: PartialPriorFactor.h:130
gtsam::PartialPriorFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
implement functions needed for Testable
Definition: PartialPriorFactor.h:95
gtsam::Factor::size
size_t size() const
Definition: Factor.h:129
gtsam::PartialPriorFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals
Definition: PartialPriorFactor.h:101
gtsam::KeyFormatter
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::PartialPriorFactor::prior_
Vector prior_
measurement on tangent space parameters, in compressed form
Definition: PartialPriorFactor.h:53
gtsam::PartialPriorFactor::H_
Matrix H_
Constant Jacobian - computed at creation.
Definition: PartialPriorFactor.h:55
gtsam::PoseRTV
Robot state for use with IMU measurements.
Definition: PoseRTV.h:23
gtsam::PartialPriorFactor::PartialPriorFactor
PartialPriorFactor(Key key, const SharedNoiseModel &model)
constructor with just minimum requirements for a factor - allows more computation in the constructor.
Definition: PartialPriorFactor.h:64
Lie.h
Base class and basic functions for Lie types.