switchVariableSigmoid.h
Go to the documentation of this file.
00001 /*
00002  * switchVariableSigmoid.h
00003  *
00004  *  Created on: 08.08.2012
00005  *      Author: niko
00006  */
00007 
00008 #ifndef SWITCHVARIABLESIGMOID_H_
00009 #define SWITCHVARIABLESIGMOID_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 SwitchVariableSigmoid : public gtsam::DerivedValue<SwitchVariableSigmoid> {
00022 
00024     SwitchVariableSigmoid() : d_(10.0) {};
00025 
00027     SwitchVariableSigmoid(double d) : d_(d) {
00028       if (d_ < -10.0) d_=-10.0;
00029       else if(d_>10.0) d_=10.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 SwitchVariableSigmoid& 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 SwitchVariableSigmoid retract(const gtsam::Vector& v) const {
00053       double x = value() + v(0);
00054 
00055       if (x>10.0) x=10.0;
00056       else if (x<-10.0) x=-10.0;
00057 
00058       return SwitchVariableSigmoid(x);
00059     }
00060 
00062     inline gtsam::Vector localCoordinates(const SwitchVariableSigmoid& t2) const { return gtsam::Vector1(t2.value() - value()); }
00063 
00064     // Group requirements
00065 
00067     inline static SwitchVariableSigmoid identity() {
00068       return SwitchVariableSigmoid();
00069     }
00070 
00072     inline SwitchVariableSigmoid compose(const SwitchVariableSigmoid& p) const {
00073       return SwitchVariableSigmoid(d_ + p.d_);
00074     }
00075 
00077     inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& 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 SwitchVariableSigmoid(l2.value() - value());
00083     }
00084 
00086     inline SwitchVariableSigmoid inverse() const {
00087       return SwitchVariableSigmoid(-1.0 * value());
00088     }
00089 
00090     // Lie functions
00091 
00093     static inline SwitchVariableSigmoid Expmap(const gtsam::Vector& v) { return SwitchVariableSigmoid(v(0)); }
00094 
00096     static inline gtsam::Vector Logmap(const SwitchVariableSigmoid& p) { return gtsam::Vector1(p.value()); }
00097 
00098   private:
00099       double d_;
00100   };
00101 }
00102 
00103 
00104 namespace gtsam {
00105 // Define Key to be Testable by specializing gtsam::traits
00106 template<typename T> struct traits;
00107 template<> struct traits<vertigo::SwitchVariableSigmoid> {
00108   static void Print(const vertigo::SwitchVariableSigmoid& key, const std::string& str = "") {
00109     key.print(str);
00110   }
00111   static bool Equals(const vertigo::SwitchVariableSigmoid& key1, const vertigo::SwitchVariableSigmoid& key2, double tol = 1e-8) {
00112     return key1.equals(key2, tol);
00113   }
00114   static int GetDimension(const vertigo::SwitchVariableSigmoid & key) {return key.Dim();}
00115 
00116   typedef OptionalJacobian<3, 3> ChartJacobian;
00117   typedef gtsam::Vector TangentVector;
00118   static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other,
00119   ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
00120     return origin.localCoordinates(other);
00121   }
00122   static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v,
00123         ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
00124       return g.retract(v);
00125     }
00126 };
00127 }
00128 
00129 #endif /* SWITCHVARIABLESIGMOID_H_ */


rtabmap
Author(s): Mathieu Labbe
autogenerated on Sat Jul 23 2016 11:44:27