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 
37  template<class VALUE>
38  class PartialPriorFactor: public NoiseModelFactorN<VALUE> {
39 
40  public:
41  typedef VALUE T;
42 
43  protected:
44  // Concept checks on the variable type - currently requires Lie
46 
49 
51  std::vector<size_t> indices_;
52 
58  : Base(model, key) {}
59 
60  public:
61 
62  // Provide access to the Matrix& version of evaluateError:
63  using Base::evaluateError;
64 
67 
69  PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
70  Base(model, key),
71  prior_((Vector(1) << prior).finished()),
72  indices_(1, idx) {
73  assert(model->dim() == 1);
74  }
75 
77  PartialPriorFactor(Key key, const std::vector<size_t>& indices, const Vector& prior,
78  const SharedNoiseModel& model) :
79  Base(model, key),
80  prior_(prior),
81  indices_(indices) {
82  assert((size_t)prior_.size() == indices_.size());
83  assert(model->dim() == (size_t)prior.size());
84  }
85 
86  ~PartialPriorFactor() override {}
87 
90  return std::static_pointer_cast<gtsam::NonlinearFactor>(
92 
96  void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
97  Base::print(s, keyFormatter);
98  gtsam::print(prior_, "prior");
99  }
100 
102  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
103  const This *e = dynamic_cast<const This*> (&expected);
104  return e != nullptr && Base::equals(*e, tol) &&
105  gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) &&
106  this->indices_ == e->indices_;
107  }
108 
112  Vector evaluateError(const T& p, OptionalMatrixType H) const override {
114 
115  // If the Rot3 Cayley map is used, Rot3::LocalCoordinates will throw a runtime error
116  // when asked to compute the Jacobian matrix (see Rot3M.cpp).
117  #ifdef GTSAM_ROT3_EXPMAP
118  const Vector full_tangent = T::LocalCoordinates(p, H ? &H_local : nullptr);
119  #else
120  const Vector full_tangent = T::Logmap(p, H ? &H_local : nullptr);
121  #endif
122 
123  if (H) {
124  (*H) = Matrix::Zero(indices_.size(), T::dimension);
125  for (size_t i = 0; i < indices_.size(); ++i) {
126  (*H).row(i) = H_local.row(indices_.at(i));
127  }
128  }
129  // Select relevant parameters from the tangent vector.
130  Vector partial_tangent = Vector::Zero(indices_.size());
131  for (size_t i = 0; i < indices_.size(); ++i) {
132  partial_tangent(i) = full_tangent(indices_.at(i));
133  }
134 
135  return partial_tangent - prior_;
136  }
137 
138  // access
139  const Vector& prior() const { return prior_; }
140  const std::vector<size_t>& indices() const { return indices_; }
141 
142  private:
143 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
144 
145  friend class boost::serialization::access;
146  template<class ARCHIVE>
147  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
148  // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
149  ar & boost::serialization::make_nvp("NoiseModelFactor1",
150  boost::serialization::base_object<Base>(*this));
151  ar & BOOST_SERIALIZATION_NVP(prior_);
152  ar & BOOST_SERIALIZATION_NVP(indices_);
153  // ar & BOOST_SERIALIZATION_NVP(H_);
154  }
155 #endif
156  }; // \class PartialPriorFactor
157 
158 }
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
gtsam::PartialPriorFactor::indices_
std::vector< size_t > indices_
Indices of the measured tangent space parameters.
Definition: PartialPriorFactor.h:51
gtsam::PartialPriorFactor::~PartialPriorFactor
~PartialPriorFactor() override
Definition: PartialPriorFactor.h:86
gtsam::PartialPriorFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
Definition: PartialPriorFactor.h:102
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::PartialPriorFactor::PartialPriorFactor
PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel &model)
Definition: PartialPriorFactor.h:69
gtsam::PartialPriorFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: PartialPriorFactor.h:96
gtsam::NoiseModelFactorN< VALUE >::evaluateError
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
gtsam::Factor
Definition: Factor.h:70
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::PartialPriorFactor::This
PartialPriorFactor< VALUE > This
Definition: PartialPriorFactor.h:48
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
GTSAM_CONCEPT_LIE_TYPE
#define GTSAM_CONCEPT_LIE_TYPE(T)
Definition: Lie.h:373
Eigen::internal::VALUE
@ VALUE
Definition: SpecialFunctionsImpl.h:729
gtsam::PartialPriorFactor::prior_
Vector prior_
Measurement on tangent space parameters, in compressed form.
Definition: PartialPriorFactor.h:50
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::KeyFormatter
std::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::NoiseModelFactorN
Definition: NonlinearFactor.h:431
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:72
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:80
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::PartialPriorFactor::PartialPriorFactor
PartialPriorFactor()
Definition: PartialPriorFactor.h:66
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::PartialPriorFactor::PartialPriorFactor
PartialPriorFactor(Key key, const std::vector< size_t > &indices, const Vector &prior, const SharedNoiseModel &model)
Definition: PartialPriorFactor.h:77
gtsam::PoseRTV
Definition: PoseRTV.h:23
NonlinearFactor.h
Non-linear factor base classes.
gtsam::PartialPriorFactor::prior
const Vector & prior() const
Definition: PartialPriorFactor.h:139
Lie.h
Base class and basic functions for Lie types.
gtsam
traits
Definition: SFMdata.h:40
gtsam::PartialPriorFactor::T
VALUE T
Definition: PartialPriorFactor.h:41
gtsam::PartialPriorFactor::evaluateError
Vector evaluateError(const T &p, OptionalMatrixType H) const override
Definition: PartialPriorFactor.h:112
std
Definition: BFloat16.h:88
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::NoiseModelFactorN< VALUE >::key
Key key() const
Definition: NonlinearFactor.h:582
gtsam::PartialPriorFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: PartialPriorFactor.h:89
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::PartialPriorFactor
Definition: PartialPriorFactor.h:38
gtsam::PartialPriorFactor::indices
const std::vector< size_t > & indices() const
Definition: PartialPriorFactor.h:140


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:03:31