betweenFactorSwitchable.h

Go to the documentation of this file.

/*
 * betweenFactorSwitchable.h
 *
 *  Created on: 02.08.2012
 *      Author: niko
 */

#ifndef BETWEENFACTORSWITCHABLE_H_
#define BETWEENFACTORSWITCHABLE_H_

#include <gtsam/nonlinear/NonlinearFactor.h>

#include <iostream>
using std::cout;
using std::endl;

#include "switchVariableLinear.h"
#include "switchVariableSigmoid.h"


namespace vertigo {

  template<class VALUE>
  class BetweenFactorSwitchableLinear : public gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableLinear>
  {
    public:
      BetweenFactorSwitchableLinear() {};
      BetweenFactorSwitchableLinear(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE& measured, const gtsam::SharedNoiseModel& model)
      : gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableLinear>(model, key1, key2, key3),
        betweenFactor(key1, key2, measured, model) {};

      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
        {

          // calculate error
          gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);
          error *= s.value();

          // handle derivatives
          if (H1) *H1 = *H1 * s.value();
          if (H2) *H2 = *H2 * s.value();
          if (H3) *H3 = error;

          return error;
        };

    private:
      gtsam::BetweenFactor<VALUE> betweenFactor;

  };



  template<class VALUE>
  class BetweenFactorSwitchableSigmoid : public gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableSigmoid>
  {
    public:
      BetweenFactorSwitchableSigmoid() {};
      BetweenFactorSwitchableSigmoid(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE& measured, const gtsam::SharedNoiseModel& model)
      : gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableSigmoid>(model, key1, key2, key3),
        betweenFactor(key1, key2, measured, 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
      {

        // calculate error
        gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);


        double w = sigmoid(s.value());
        error *= w;

        // handle derivatives
        if (H1) *H1 = *H1 * w;
        if (H2) *H2 = *H2 * w;
        if (H3) *H3 = error /* (w*(1.0-w))*/;  // sig(x)*(1-sig(x)) is the derivative of sig(x) wrt. x

        return error;
      };

    private:
      gtsam::BetweenFactor<VALUE> betweenFactor;

      double sigmoid(double x) const {
        return 1.0/(1.0+exp(-x));
      }
  };

}

#endif /* BETWEENFACTORSWITCHABLE_H_ */