switchVariableLinear.h
Go to the documentation of this file.
1 /*
2  * switchVariableLinear.h
3  *
4  * Created on: 02.08.2012
5  * Author: niko
6  */
7 
8 #ifndef SWITCHVARIABLELINEAR_H_
9 #define SWITCHVARIABLELINEAR_H_
10 
11 #pragma once
12 
13 // DerivedValue2.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
14 #include "DerivedValue.h"
15 #include <gtsam/base/Lie.h>
16 
17 namespace vertigo {
18 
22  struct SwitchVariableLinear : public rtabmap::DerivedValue<SwitchVariableLinear> {
23 
25  SwitchVariableLinear() : d_(0.0) {};
26 
28  SwitchVariableLinear(double d) : d_(d) {
29  // if (d_ < 0.0) d_=0.0;
30  // else if(d_>1.0) d_=1.0;
31  };
32 
34  double value() const { return d_; }
35 
37  inline void print(const std::string& name="") const {
38  std::cout << name << ": " << d_ << std::endl;
39  }
40 
42  inline bool equals(const SwitchVariableLinear& expected, double tol=1e-5) const {
43  return fabs(expected.d_ - d_) <= tol;
44  }
45 
46  // Manifold requirements
47 
49  inline size_t dim() const { return 1; }
50  inline static size_t Dim() { return 1; }
51 
53  inline SwitchVariableLinear retract(const gtsam::Vector& v) const {
54  double x = value() + v(0);
55 
56  if (x>1.0) x=1.0;
57  else if (x<0.0) x=0.0;
58 
59  return SwitchVariableLinear(x);
60  }
61 
63  inline gtsam::Vector localCoordinates(const SwitchVariableLinear& t2) const { return gtsam::Vector1(t2.value() - value()); }
64 
65  // Group requirements
66 
68  inline static SwitchVariableLinear identity() {
69  return SwitchVariableLinear();
70  }
71 
74  return SwitchVariableLinear(d_ + p.d_);
75  }
76 
79 #if GTSAM_VERSION_NUMERIC >= 40300
80  OptionalMatrixType H1=OptionalNone,
81  OptionalMatrixType H2=OptionalNone) const {
82 #else
83  boost::optional<gtsam::Matrix&> H1=boost::none,
84  boost::optional<gtsam::Matrix&> H2=boost::none) const {
85 #endif
86  if(H1) *H1 = -gtsam::Matrix::Identity(1, 1);
87  if(H2) *H2 = gtsam::Matrix::Identity(1, 1);
88  return SwitchVariableLinear(l2.value() - value());
89  }
90 
92  inline SwitchVariableLinear inverse() const {
93  return SwitchVariableLinear(-1.0 * value());
94  }
95 
96  // Lie functions
97 
99  static inline SwitchVariableLinear Expmap(const gtsam::Vector& v) { return SwitchVariableLinear(v(0)); }
100 
102  static inline gtsam::Vector Logmap(const SwitchVariableLinear& p) { return gtsam::Vector1(p.value()); }
103 
104  private:
105  double d_;
106  };
107 }
108 
109 namespace gtsam {
110 // Define Key to be Testable by specializing gtsam::traits
111 template<typename T> struct traits;
112 template<> struct traits<vertigo::SwitchVariableLinear> {
113  static void Print(const vertigo::SwitchVariableLinear& key, const std::string& str = "") {
114  key.print(str);
115  }
116  static bool Equals(const vertigo::SwitchVariableLinear& key1, const vertigo::SwitchVariableLinear& key2, double tol = 1e-8) {
117  return key1.equals(key2, tol);
118  }
119  static int GetDimension(const vertigo::SwitchVariableLinear & key) {return key.Dim();}
120 
124 #if GTSAM_VERSION_NUMERIC >= 40300
125  ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
126 #else
127  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
128 #endif
129  return origin.localCoordinates(other);
130  }
132 #if GTSAM_VERSION_NUMERIC >= 40300
133  ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
134 #else
135  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
136 #endif
137  return g.retract(v);
138  }
139 };
140 }
141 
142 
143 
144 #endif /* SWITCHVARIABLELINEAR_H_ */
key1
const Symbol key1( 'v', 1)
vertigo::SwitchVariableLinear::value
double value() const
Definition: switchVariableLinear.h:34
vertigo::SwitchVariableLinear::localCoordinates
gtsam::Vector localCoordinates(const SwitchVariableLinear &t2) const
Definition: switchVariableLinear.h:63
vertigo::SwitchVariableLinear::retract
SwitchVariableLinear retract(const gtsam::Vector &v) const
Definition: switchVariableLinear.h:53
gtsam::Vector1
Eigen::Matrix< double, 1, 1 > Vector1
name
l2
gtsam::Key l2
vertigo::SwitchVariableLinear::identity
static SwitchVariableLinear identity()
Definition: switchVariableLinear.h:68
expected
const set< Key > expected
gtsam::traits< vertigo::SwitchVariableLinear >::Retract
static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear &g, const TangentVector &v, ChartJacobian H1=boost::none, ChartJacobian H2=boost::none)
Definition: switchVariableLinear.h:131
gtsam::traits< vertigo::SwitchVariableLinear >::TangentVector
gtsam::Vector TangentVector
Definition: switchVariableLinear.h:122
gtsam::Vector
Eigen::VectorXd Vector
Lie.h
vertigo::SwitchVariableLinear::equals
bool equals(const SwitchVariableLinear &expected, double tol=1e-5) const
Definition: switchVariableLinear.h:42
fabs
Real fabs(const Real &a)
vertigo::SwitchVariableLinear::SwitchVariableLinear
SwitchVariableLinear(double d)
Definition: switchVariableLinear.h:28
key2
const Symbol key2( 'v', 2)
g
float g
vertigo::SwitchVariableLinear::print
void print(const std::string &name="") const
Definition: switchVariableLinear.h:37
vertigo::SwitchVariableLinear::Dim
static size_t Dim()
Definition: switchVariableLinear.h:50
DerivedValue.h
vertigo::SwitchVariableLinear::dim
size_t dim() const
Definition: switchVariableLinear.h:49
gtsam::traits< vertigo::SwitchVariableLinear >::Local
static TangentVector Local(const vertigo::SwitchVariableLinear &origin, const vertigo::SwitchVariableLinear &other, ChartJacobian Horigin=boost::none, ChartJacobian Hother=boost::none)
Definition: switchVariableLinear.h:123
vertigo::SwitchVariableLinear::SwitchVariableLinear
SwitchVariableLinear()
Definition: switchVariableLinear.h:25
d
d
x
x
p
Point3_ p(2)
vertigo::SwitchVariableLinear::d_
double d_
Definition: switchVariableLinear.h:105
gtsam::Symbol::print
void print(const std::string &s="") const
str
key
const gtsam::Symbol key( 'X', 0)
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
vertigo::SwitchVariableLinear::compose
SwitchVariableLinear compose(const SwitchVariableLinear &p) const
Definition: switchVariableLinear.h:73
vertigo::SwitchVariableLinear::between
SwitchVariableLinear between(const SwitchVariableLinear &l2, boost::optional< gtsam::Matrix & > H1=boost::none, boost::optional< gtsam::Matrix & > H2=boost::none) const
Definition: switchVariableLinear.h:78
gtsam
vertigo
Definition: betweenFactorMaxMix.h:24
rtabmap::DerivedValue
Definition: DerivedValue.h:44
gtsam::traits
Definition: switchVariableLinear.h:111
vertigo::SwitchVariableLinear
Definition: switchVariableLinear.h:22
gtsam::OptionalJacobian
vertigo::SwitchVariableLinear::Expmap
static SwitchVariableLinear Expmap(const gtsam::Vector &v)
Definition: switchVariableLinear.h:99
origin
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 origin
v
Array< int, Dynamic, 1 > v
vertigo::SwitchVariableLinear::Logmap
static gtsam::Vector Logmap(const SwitchVariableLinear &p)
Definition: switchVariableLinear.h:102
tol
const G double tol
gtsam::traits< vertigo::SwitchVariableLinear >::Equals
static bool Equals(const vertigo::SwitchVariableLinear &key1, const vertigo::SwitchVariableLinear &key2, double tol=1e-8)
Definition: switchVariableLinear.h:116
Eigen.Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor >
gtsam::traits< vertigo::SwitchVariableLinear >::GetDimension
static int GetDimension(const vertigo::SwitchVariableLinear &key)
Definition: switchVariableLinear.h:119
gtsam::traits< vertigo::SwitchVariableLinear >::Print
static void Print(const vertigo::SwitchVariableLinear &key, const std::string &str="")
Definition: switchVariableLinear.h:113
vertigo::SwitchVariableLinear::inverse
SwitchVariableLinear inverse() const
Definition: switchVariableLinear.h:92
gtsam::traits< vertigo::SwitchVariableLinear >::ChartJacobian
OptionalJacobian< 3, 3 > ChartJacobian
Definition: switchVariableLinear.h:121


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:22