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
12/**
13 * @file PartialPriorFactor.h
14 * @brief A simple prior factor that allows for setting a prior only on a part of linear parameters
15 * @author Alex Cunningham
16 */
17
18#pragma once
19
20#include <gtsam/nonlinear/NonlinearFactor.h>
21#include <gtsam/base/Lie.h>
22
23#include <cassert>
24
25namespace gtsam {
26
27 /**
28 * A class for a soft partial prior on any Lie type, with a mask over Expmap
29 * parameters. Note that this will use Logmap() to find a tangent space parameterization
30 * for the variable attached, so this may fail for highly nonlinear manifolds.
31 *
32 * The prior vector used in this factor is stored in compressed form, such that
33 * it only contains values for measurements that are to be compared, and they are in
34 * the same order as VALUE::Logmap(). The provided indices will determine which components to
35 * extract in the error function.
36 *
37 * @tparam VALUE is the type of variable the prior effects
38 */
39 template<class VALUE>
40 class PartialPriorFactor: public NoiseModelFactorN<VALUE> {
41
42 public:
43 typedef VALUE T;
44
45 protected:
46 // Concept checks on the variable type - currently requires Lie
47 GTSAM_CONCEPT_LIE_TYPE(VALUE)
48
49 typedef NoiseModelFactorN<VALUE> Base;
50 typedef PartialPriorFactor<VALUE> This;
51
52 Vector prior_; ///< Measurement on tangent space parameters, in compressed form.
53 std::vector<size_t> indices_; ///< Indices of the measured tangent space parameters.
54
55 /**
56 * constructor with just minimum requirements for a factor - allows more
57 * computation in the constructor. This should only be used by subclasses
58 */
59 PartialPriorFactor(Key key, const SharedNoiseModel& model)
60 : Base(model, key) {}
61
62 public:
63
64 // Provide access to the Matrix& version of evaluateError:
65 using Base::evaluateError;
66
67 /** default constructor - only use for serialization */
68 PartialPriorFactor() {}
69
70 /** Single Element Constructor: Prior on a single parameter at index 'idx' in the tangent vector.*/
71 PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
72 Base(model, key),
73 prior_((Vector(1) << prior).finished()),
74 indices_(1, idx) {
75 assert(model->dim() == 1);
76 }
77
78 /** Indices Constructor: Specify the relevant measured indices in the tangent vector.*/
79 PartialPriorFactor(Key key, const std::vector<size_t>& indices, const Vector& prior,
80 const SharedNoiseModel& model) :
81 Base(model, key),
82 prior_(prior),
83 indices_(indices) {
84 assert((size_t)prior_.size() == indices_.size());
85 assert(model->dim() == (size_t)prior.size());
86 }
87
88 ~PartialPriorFactor() override {}
89
90 /// @return a deep copy of this factor
91 gtsam::NonlinearFactor::shared_ptr clone() const override {
92 return std::static_pointer_cast<gtsam::NonlinearFactor>(
93 r: gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
94
95 /** implement functions needed for Testable */
96
97 /** print */
98 void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
99 Base::print(s, keyFormatter);
100 gtsam::print(v: prior_, s: "prior");
101 }
102
103 /** equals */
104 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
105 const This *e = dynamic_cast<const This*> (&expected);
106 return e != nullptr && Base::equals(*e, tol) &&
107 gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) &&
108 this->indices_ == e->indices_;
109 }
110
111 /** implement functions needed to derive from Factor */
112
113 /** Returns a vector of errors for the measured tangent parameters. */
114 Vector evaluateError(const T& p, OptionalMatrixType H) const override {
115 Eigen::Matrix<double, T::dimension, T::dimension> H_local;
116
117 // If the Rot3 Cayley map is used, Rot3::LocalCoordinates will throw a runtime error
118 // when asked to compute the Jacobian matrix (see Rot3M.cpp).
119 #ifdef GTSAM_ROT3_EXPMAP
120 const Vector full_tangent = T::LocalCoordinates(p, H ? &H_local : nullptr);
121 #else
122 const Vector full_tangent = T::Logmap(p, H ? &H_local : nullptr);
123 #endif
124
125 if (H) {
126 (*H) = Matrix::Zero(indices_.size(), T::dimension);
127 for (size_t i = 0; i < indices_.size(); ++i) {
128 (*H).row(i) = H_local.row(indices_.at(n: i));
129 }
130 }
131 // Select relevant parameters from the tangent vector.
132 Vector partial_tangent = Vector::Zero(size: indices_.size());
133 for (size_t i = 0; i < indices_.size(); ++i) {
134 partial_tangent(i) = full_tangent(indices_.at(n: i));
135 }
136
137 return partial_tangent - prior_;
138 }
139
140 // access
141 const Vector& prior() const { return prior_; }
142 const std::vector<size_t>& indices() const { return indices_; }
143
144 private:
145#if GTSAM_ENABLE_BOOST_SERIALIZATION
146 /** Serialization function */
147 friend class boost::serialization::access;
148 template<class ARCHIVE>
149 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
150 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
151 ar & boost::serialization::make_nvp("NoiseModelFactor1",
152 boost::serialization::base_object<Base>(*this));
153 ar & BOOST_SERIALIZATION_NVP(prior_);
154 ar & BOOST_SERIALIZATION_NVP(indices_);
155 // ar & BOOST_SERIALIZATION_NVP(H_);
156 }
157#endif
158 }; // \class PartialPriorFactor
159
160} /// namespace gtsam
161