betweenFactorSwitchable.h
Go to the documentation of this file.
1 /*
2  * betweenFactorSwitchable.h
3  *
4  * Created on: 02.08.2012
5  * Author: niko
6  */
7 
8 #ifndef BETWEENFACTORSWITCHABLE_H_
9 #define BETWEENFACTORSWITCHABLE_H_
10 
11 #include <gtsam/nonlinear/NonlinearFactor.h>
12 
13 #include <iostream>
14 using std::cout;
15 using std::endl;
16 
17 #include "switchVariableLinear.h"
18 #include "switchVariableSigmoid.h"
19 
20 
21 namespace vertigo {
22 
23  template<class VALUE>
24  class BetweenFactorSwitchableLinear : public gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableLinear>
25  {
26  public:
28  BetweenFactorSwitchableLinear(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE& measured, const gtsam::SharedNoiseModel& model)
29  : gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableLinear>(model, key1, key2, key3),
30  betweenFactor(key1, key2, measured, model) {};
31 
32  gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s,
33  boost::optional<gtsam::Matrix&> H1 = boost::none,
34  boost::optional<gtsam::Matrix&> H2 = boost::none,
35  boost::optional<gtsam::Matrix&> H3 = boost::none) const
36  {
37 
38  // calculate error
39  gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);
40  error *= s.value();
41 
42  // handle derivatives
43  if (H1) *H1 = *H1 * s.value();
44  if (H2) *H2 = *H2 * s.value();
45  if (H3) *H3 = error;
46 
47  return error;
48  };
49 
50  private:
51  gtsam::BetweenFactor<VALUE> betweenFactor;
52 
53  };
54 
55 
56 
57  template<class VALUE>
58  class BetweenFactorSwitchableSigmoid : public gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableSigmoid>
59  {
60  public:
62  BetweenFactorSwitchableSigmoid(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE& measured, const gtsam::SharedNoiseModel& model)
63  : gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableSigmoid>(model, key1, key2, key3),
64  betweenFactor(key1, key2, measured, model) {};
65 
66  gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s,
67  boost::optional<gtsam::Matrix&> H1 = boost::none,
68  boost::optional<gtsam::Matrix&> H2 = boost::none,
69  boost::optional<gtsam::Matrix&> H3 = boost::none) const
70  {
71 
72  // calculate error
73  gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);
74 
75 
76  double w = sigmoid(s.value());
77  error *= w;
78 
79  // handle derivatives
80  if (H1) *H1 = *H1 * w;
81  if (H2) *H2 = *H2 * w;
82  if (H3) *H3 = error /* (w*(1.0-w))*/; // sig(x)*(1-sig(x)) is the derivative of sig(x) wrt. x
83 
84  return error;
85  };
86 
87  private:
88  gtsam::BetweenFactor<VALUE> betweenFactor;
89 
90  double sigmoid(double x) const {
91  return 1.0/(1.0+exp(-x));
92  }
93  };
94 
95 }
96 
97 #endif /* BETWEENFACTORSWITCHABLE_H_ */
gtsam::Vector evaluateError(const VALUE &p1, const VALUE &p2, const SwitchVariableLinear &s, boost::optional< gtsam::Matrix & > H1=boost::none, boost::optional< gtsam::Matrix & > H2=boost::none, boost::optional< gtsam::Matrix & > H3=boost::none) const
BetweenFactorSwitchableSigmoid(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE &measured, const gtsam::SharedNoiseModel &model)
BetweenFactorSwitchableLinear(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE &measured, const gtsam::SharedNoiseModel &model)
gtsam::Vector evaluateError(const VALUE &p1, const VALUE &p2, const SwitchVariableSigmoid &s, boost::optional< gtsam::Matrix & > H1=boost::none, boost::optional< gtsam::Matrix & > H2=boost::none, boost::optional< gtsam::Matrix & > H3=boost::none) const
gtsam::BetweenFactor< VALUE > betweenFactor
GLM_FUNC_DECL genType exp(genType const &x)
gtsam::BetweenFactor< VALUE > betweenFactor


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58