Added parameter "RGBD/OptimizeRobust" (default true) to use Vertigo robust graph optimization (only for g2o and GTSAM optimization strategies). Added GTSAM support.

2026-09-02 09:30:25 +08:00 · 2015-09-02 17:51:43 -04:00
parent a2b9c9f9a0
commit 7a721105ae
40 changed files with 2125 additions and 158 deletions
--- a/corelib/src/vertigo/gtsam/betweenFactorMaxMix.h
+++ b/corelib/src/vertigo/gtsam/betweenFactorMaxMix.h
@@ -0,0 +1,67 @@
+/*
+ * betweenFactorMaxMix.h
+ *
+ *  Created on: 14.08.2012
+ *      Author: niko
+ */
+
+#ifndef BETWEENFACTORMAXMIX_H_
+#define BETWEENFACTORMAXMIX_H_
+
+#include <gtsam/linear/NoiseModel.h>
+#include <Eigen/Eigen>
+
+namespace vertigo {
+
+  template<class VALUE>
+  class BetweenFactorMaxMix : public gtsam::NoiseModelFactor2<VALUE, VALUE>
+  {
+    public:
+      BetweenFactorMaxMix() : weight(0.0) {};
+      BetweenFactorMaxMix(gtsam::Key key1, gtsam::Key key2, const VALUE& measured, const gtsam::SharedNoiseModel& model, const gtsam::SharedNoiseModel& model2, double w)
+      : gtsam::NoiseModelFactor2<VALUE, VALUE>(model, key1, key2), weight(w), nullHypothesisModel(model2),
+        betweenFactor(key1, key2, measured, model)  {   };
+
+      gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2,
+          boost::optional<gtsam::Matrix&> H1 = boost::none,
+          boost::optional<gtsam::Matrix&> H2 =  boost::none) const
+        {
+
+          // calculate error
+          gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);
+
+
+
+          // which hypothesis is more likely
+          double m1 = this->noiseModel_->distance(error);
+          gtsam::noiseModel::Gaussian::shared_ptr g1 = this->noiseModel_;
+          gtsam::Matrix info1(g1->R().transpose()*g1->R());
+          double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant());
+          double l1 = nu1 * exp(-0.5*m1);
+
+          double m2 = nullHypothesisModel->distance(error);
+          gtsam::noiseModel::Gaussian::shared_ptr g2 = nullHypothesisModel;
+          gtsam::Matrix info2(g2->R().transpose()*g2->R());
+          double nu2 = 1.0/sqrt(gtsam::inverse(info2).determinant());
+          double l2 = nu2 * exp(-0.5*m2);
+
+          // if the null hypothesis is more likely, than proceed by applying the weight ...
+          if (l2>l1) {
+            if (H1) *H1 = *H1 * weight;
+            if (H2) *H2 = *H2 * weight;
+            error *= sqrt(weight);
+          }
+
+          return error;
+        };
+
+    private:
+      gtsam::BetweenFactor<VALUE> betweenFactor;
+      gtsam::SharedNoiseModel nullHypothesisModel;
+      double weight;
+
+  };
+}
+
+
+#endif /* BETWEENFACTORMAXMIX_H_ */
--- a/corelib/src/vertigo/gtsam/betweenFactorSwitchable.h
+++ b/corelib/src/vertigo/gtsam/betweenFactorSwitchable.h
@@ -0,0 +1,97 @@
+/*
+ * 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_ */
--- a/corelib/src/vertigo/gtsam/switchVariableLinear.h
+++ b/corelib/src/vertigo/gtsam/switchVariableLinear.h
@@ -0,0 +1,130 @@
+/*
+ * switchVariableLinear.h
+ *
+ *  Created on: 02.08.2012
+ *      Author: niko
+ */
+
+#ifndef SWITCHVARIABLELINEAR_H_
+#define SWITCHVARIABLELINEAR_H_
+
+#pragma once
+
+#include <gtsam/base/DerivedValue.h>
+#include <gtsam/base/Lie.h>
+
+namespace vertigo {
+
+  /**
+   * SwitchVariableLinear is a wrapper around double to allow it to be a Lie type
+   */
+  struct SwitchVariableLinear : public gtsam::DerivedValue<SwitchVariableLinear> {
+
+    /** default constructor */
+    SwitchVariableLinear() : d_(0.0) {};
+
+    /** wrap a double */
+    SwitchVariableLinear(double d) : d_(d) {
+     // if (d_ < 0.0) d_=0.0;
+     // else if(d_>1.0) d_=1.0;
+    };
+
+    /** access the underlying value */
+    double value() const { return d_; }
+
+    /** print @param s optional string naming the object */
+    inline void print(const std::string& name="") const {
+      std::cout << name << ": " << d_ << std::endl;
+    }
+
+    /** equality up to tolerance */
+    inline bool equals(const SwitchVariableLinear& expected, double tol=1e-5) const {
+      return fabs(expected.d_ - d_) <= tol;
+    }
+
+    // Manifold requirements
+
+    /** Returns dimensionality of the tangent space */
+    inline size_t dim() const { return 1; }
+    inline static size_t Dim() { return 1; }
+
+    /** Update the SwitchVariableLinear with a tangent space update */
+    inline SwitchVariableLinear retract(const gtsam::Vector& v) const {
+      double x = value() + v(0);
+
+      if (x>1.0) x=1.0;
+      else if (x<0.0) x=0.0;
+
+      return SwitchVariableLinear(x);
+    }
+
+    /** @return the local coordinates of another object */
+    inline gtsam::Vector localCoordinates(const SwitchVariableLinear& t2) const { return gtsam::Vector1(t2.value() - value()); }
+
+    // Group requirements
+
+    /** identity */
+    inline static SwitchVariableLinear identity() {
+      return SwitchVariableLinear();
+    }
+
+    /** compose with another object */
+    inline SwitchVariableLinear compose(const SwitchVariableLinear& p) const {
+      return SwitchVariableLinear(d_ + p.d_);
+    }
+
+    /** between operation */
+    inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
+        boost::optional<gtsam::Matrix&> H1=boost::none,
+        boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      if(H1) *H1 = -gtsam::eye(1);
+      if(H2) *H2 = gtsam::eye(1);
+      return SwitchVariableLinear(l2.value() - value());
+    }
+
+    /** invert the object and yield a new one */
+    inline SwitchVariableLinear inverse() const {
+      return SwitchVariableLinear(-1.0 * value());
+    }
+
+    // Lie functions
+
+    /** Expmap around identity */
+    static inline SwitchVariableLinear Expmap(const gtsam::Vector& v) { return SwitchVariableLinear(v(0)); }
+
+    /** Logmap around identity - just returns with default cast back */
+    static inline gtsam::Vector Logmap(const SwitchVariableLinear& p) { return gtsam::Vector1(p.value()); }
+
+  private:
+      double d_;
+  };
+}
+
+namespace gtsam {
+// Define Key to be Testable by specializing gtsam::traits
+template<typename T> struct traits;
+template<> struct traits<vertigo::SwitchVariableLinear> {
+  static void Print(const vertigo::SwitchVariableLinear& key, const std::string& str = "") {
+    key.print(str);
+  }
+  static bool Equals(const vertigo::SwitchVariableLinear& key1, const vertigo::SwitchVariableLinear& key2, double tol = 1e-8) {
+    return key1.equals(key2, tol);
+  }
+  static int GetDimension(const vertigo::SwitchVariableLinear & key) {return key.Dim();}
+
+  typedef OptionalJacobian<3, 3> ChartJacobian;
+  typedef gtsam::Vector TangentVector;
+  static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other,
+  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+    return origin.localCoordinates(other);
+  }
+  static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v,
+        ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+      return g.retract(v);
+    }
+};
+}
+
+
+
+#endif /* SWITCHVARIABLELINEAR_H_ */
--- a/corelib/src/vertigo/gtsam/switchVariableSigmoid.h
+++ b/corelib/src/vertigo/gtsam/switchVariableSigmoid.h
@@ -0,0 +1,129 @@
+/*
+ * switchVariableSigmoid.h
+ *
+ *  Created on: 08.08.2012
+ *      Author: niko
+ */
+
+#ifndef SWITCHVARIABLESIGMOID_H_
+#define SWITCHVARIABLESIGMOID_H_
+
+#pragma once
+
+#include <gtsam/base/DerivedValue.h>
+#include <gtsam/base/Lie.h>
+
+namespace vertigo {
+
+  /**
+   * SwitchVariableSigmoid is a wrapper around double to allow it to be a Lie type
+   */
+  struct SwitchVariableSigmoid : public gtsam::DerivedValue<SwitchVariableSigmoid> {
+
+    /** default constructor */
+    SwitchVariableSigmoid() : d_(10.0) {};
+
+    /** wrap a double */
+    SwitchVariableSigmoid(double d) : d_(d) {
+      if (d_ < -10.0) d_=-10.0;
+      else if(d_>10.0) d_=10.0;
+    };
+
+    /** access the underlying value */
+    double value() const { return d_; }
+
+    /** print @param s optional string naming the object */
+    inline void print(const std::string& name="") const {
+      std::cout << name << ": " << d_ << std::endl;
+    }
+
+    /** equality up to tolerance */
+    inline bool equals(const SwitchVariableSigmoid& expected, double tol=1e-5) const {
+      return fabs(expected.d_ - d_) <= tol;
+    }
+
+    // Manifold requirements
+
+    /** Returns dimensionality of the tangent space */
+    inline size_t dim() const { return 1; }
+    inline static size_t Dim() { return 1; }
+
+    /** Update the SwitchVariableSigmoid with a tangent space update */
+    inline SwitchVariableSigmoid retract(const gtsam::Vector& v) const {
+      double x = value() + v(0);
+
+      if (x>10.0) x=10.0;
+      else if (x<-10.0) x=-10.0;
+
+      return SwitchVariableSigmoid(x);
+    }
+
+    /** @return the local coordinates of another object */
+    inline gtsam::Vector localCoordinates(const SwitchVariableSigmoid& t2) const { return gtsam::Vector1(t2.value() - value()); }
+
+    // Group requirements
+
+    /** identity */
+    inline static SwitchVariableSigmoid identity() {
+      return SwitchVariableSigmoid();
+    }
+
+    /** compose with another object */
+    inline SwitchVariableSigmoid compose(const SwitchVariableSigmoid& p) const {
+      return SwitchVariableSigmoid(d_ + p.d_);
+    }
+
+    /** between operation */
+    inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
+        boost::optional<gtsam::Matrix&> H1=boost::none,
+        boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      if(H1) *H1 = -gtsam::eye(1);
+      if(H2) *H2 = gtsam::eye(1);
+      return SwitchVariableSigmoid(l2.value() - value());
+    }
+
+    /** invert the object and yield a new one */
+    inline SwitchVariableSigmoid inverse() const {
+      return SwitchVariableSigmoid(-1.0 * value());
+    }
+
+    // Lie functions
+
+    /** Expmap around identity */
+    static inline SwitchVariableSigmoid Expmap(const gtsam::Vector& v) { return SwitchVariableSigmoid(v(0)); }
+
+    /** Logmap around identity - just returns with default cast back */
+    static inline gtsam::Vector Logmap(const SwitchVariableSigmoid& p) { return gtsam::Vector1(p.value()); }
+
+  private:
+      double d_;
+  };
+}
+
+
+namespace gtsam {
+// Define Key to be Testable by specializing gtsam::traits
+template<typename T> struct traits;
+template<> struct traits<vertigo::SwitchVariableSigmoid> {
+  static void Print(const vertigo::SwitchVariableSigmoid& key, const std::string& str = "") {
+    key.print(str);
+  }
+  static bool Equals(const vertigo::SwitchVariableSigmoid& key1, const vertigo::SwitchVariableSigmoid& key2, double tol = 1e-8) {
+    return key1.equals(key2, tol);
+  }
+  static int GetDimension(const vertigo::SwitchVariableSigmoid & key) {return key.Dim();}
+
+  typedef OptionalJacobian<3, 3> ChartJacobian;
+  typedef gtsam::Vector TangentVector;
+  static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other,
+  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+    return origin.localCoordinates(other);
+  }
+  static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v,
+        ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+      return g.retract(v);
+    }
+};
+}
+
+#endif /* SWITCHVARIABLESIGMOID_H_ */