betweenFactorSwitchable.h
Go to the documentation of this file.
00001 /*
00002  * betweenFactorSwitchable.h
00003  *
00004  *  Created on: 02.08.2012
00005  *      Author: niko
00006  */
00007 
00008 #ifndef BETWEENFACTORSWITCHABLE_H_
00009 #define BETWEENFACTORSWITCHABLE_H_
00010 
00011 #include <gtsam/nonlinear/NonlinearFactor.h>
00012 
00013 #include <iostream>
00014 using std::cout;
00015 using std::endl;
00016 
00017 #include "switchVariableLinear.h"
00018 #include "switchVariableSigmoid.h"
00019 
00020 
00021 namespace vertigo {
00022 
00023   template<class VALUE>
00024   class BetweenFactorSwitchableLinear : public gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableLinear>
00025   {
00026     public:
00027       BetweenFactorSwitchableLinear() {};
00028       BetweenFactorSwitchableLinear(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE& measured, const gtsam::SharedNoiseModel& model)
00029       : gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableLinear>(model, key1, key2, key3),
00030         betweenFactor(key1, key2, measured, model) {};
00031 
00032       gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s,
00033           boost::optional<gtsam::Matrix&> H1 = boost::none,
00034           boost::optional<gtsam::Matrix&> H2 =  boost::none,
00035           boost::optional<gtsam::Matrix&> H3 =  boost::none) const
00036         {
00037 
00038           // calculate error
00039           gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);
00040           error *= s.value();
00041 
00042           // handle derivatives
00043           if (H1) *H1 = *H1 * s.value();
00044           if (H2) *H2 = *H2 * s.value();
00045           if (H3) *H3 = error;
00046 
00047           return error;
00048         };
00049 
00050     private:
00051       gtsam::BetweenFactor<VALUE> betweenFactor;
00052 
00053   };
00054 
00055 
00056 
00057   template<class VALUE>
00058   class BetweenFactorSwitchableSigmoid : public gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableSigmoid>
00059   {
00060     public:
00061       BetweenFactorSwitchableSigmoid() {};
00062       BetweenFactorSwitchableSigmoid(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE& measured, const gtsam::SharedNoiseModel& model)
00063       : gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableSigmoid>(model, key1, key2, key3),
00064         betweenFactor(key1, key2, measured, model) {};
00065 
00066       gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s,
00067           boost::optional<gtsam::Matrix&> H1 = boost::none,
00068           boost::optional<gtsam::Matrix&> H2 =  boost::none,
00069           boost::optional<gtsam::Matrix&> H3 =  boost::none) const
00070       {
00071 
00072         // calculate error
00073         gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);
00074 
00075 
00076         double w = sigmoid(s.value());
00077         error *= w;
00078 
00079         // handle derivatives
00080         if (H1) *H1 = *H1 * w;
00081         if (H2) *H2 = *H2 * w;
00082         if (H3) *H3 = error /* (w*(1.0-w))*/;  // sig(x)*(1-sig(x)) is the derivative of sig(x) wrt. x
00083 
00084         return error;
00085       };
00086 
00087     private:
00088       gtsam::BetweenFactor<VALUE> betweenFactor;
00089 
00090       double sigmoid(double x) const {
00091         return 1.0/(1.0+exp(-x));
00092       }
00093   };
00094 
00095 }
00096 
00097 #endif /* BETWEENFACTORSWITCHABLE_H_ */


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