switchVariableLinear.h
Go to the documentation of this file.
00001 /*
00002  * switchVariableLinear.h
00003  *
00004  *  Created on: 02.08.2012
00005  *      Author: niko
00006  */
00007 
00008 #ifndef SWITCHVARIABLELINEAR_H_
00009 #define SWITCHVARIABLELINEAR_H_
00010 
00011 #pragma once
00012 
00013 #include <gtsam/base/DerivedValue.h>
00014 #include <gtsam/base/Lie.h>
00015 
00016 namespace vertigo {
00017 
00021   struct SwitchVariableLinear : public gtsam::DerivedValue<SwitchVariableLinear> {
00022 
00024     SwitchVariableLinear() : d_(0.0) {};
00025 
00027     SwitchVariableLinear(double d) : d_(d) {
00028      // if (d_ < 0.0) d_=0.0;
00029      // else if(d_>1.0) d_=1.0;
00030     };
00031 
00033     double value() const { return d_; }
00034 
00036     inline void print(const std::string& name="") const {
00037       std::cout << name << ": " << d_ << std::endl;
00038     }
00039 
00041     inline bool equals(const SwitchVariableLinear& expected, double tol=1e-5) const {
00042       return fabs(expected.d_ - d_) <= tol;
00043     }
00044 
00045     // Manifold requirements
00046 
00048     inline size_t dim() const { return 1; }
00049     inline static size_t Dim() { return 1; }
00050 
00052     inline SwitchVariableLinear retract(const gtsam::Vector& v) const {
00053       double x = value() + v(0);
00054 
00055       if (x>1.0) x=1.0;
00056       else if (x<0.0) x=0.0;
00057 
00058       return SwitchVariableLinear(x);
00059     }
00060 
00062     inline gtsam::Vector localCoordinates(const SwitchVariableLinear& t2) const { return gtsam::Vector1(t2.value() - value()); }
00063 
00064     // Group requirements
00065 
00067     inline static SwitchVariableLinear identity() {
00068       return SwitchVariableLinear();
00069     }
00070 
00072     inline SwitchVariableLinear compose(const SwitchVariableLinear& p) const {
00073       return SwitchVariableLinear(d_ + p.d_);
00074     }
00075 
00077     inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
00078         boost::optional<gtsam::Matrix&> H1=boost::none,
00079         boost::optional<gtsam::Matrix&> H2=boost::none) const {
00080       if(H1) *H1 = -gtsam::eye(1);
00081       if(H2) *H2 = gtsam::eye(1);
00082       return SwitchVariableLinear(l2.value() - value());
00083     }
00084 
00086     inline SwitchVariableLinear inverse() const {
00087       return SwitchVariableLinear(-1.0 * value());
00088     }
00089 
00090     // Lie functions
00091 
00093     static inline SwitchVariableLinear Expmap(const gtsam::Vector& v) { return SwitchVariableLinear(v(0)); }
00094 
00096     static inline gtsam::Vector Logmap(const SwitchVariableLinear& p) { return gtsam::Vector1(p.value()); }
00097 
00098   private:
00099       double d_;
00100   };
00101 }
00102 
00103 namespace gtsam {
00104 // Define Key to be Testable by specializing gtsam::traits
00105 template<typename T> struct traits;
00106 template<> struct traits<vertigo::SwitchVariableLinear> {
00107   static void Print(const vertigo::SwitchVariableLinear& key, const std::string& str = "") {
00108     key.print(str);
00109   }
00110   static bool Equals(const vertigo::SwitchVariableLinear& key1, const vertigo::SwitchVariableLinear& key2, double tol = 1e-8) {
00111     return key1.equals(key2, tol);
00112   }
00113   static int GetDimension(const vertigo::SwitchVariableLinear & key) {return key.Dim();}
00114 
00115   typedef OptionalJacobian<3, 3> ChartJacobian;
00116   typedef gtsam::Vector TangentVector;
00117   static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other,
00118   ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
00119     return origin.localCoordinates(other);
00120   }
00121   static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v,
00122         ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
00123       return g.retract(v);
00124     }
00125 };
00126 }
00127 
00128 
00129 
00130 #endif /* SWITCHVARIABLELINEAR_H_ */


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:31