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

corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp

@@ -0,0 +1,122 @@
+/*
+ * edge_se2MaxMixture.cpp
+ *
+ *  Created on: 12.06.2012
+ *      Author: niko
+ */
+
+
+#include "edge_se2MaxMixture.h"
+
+#include <GL/gl.h>
+using namespace std;
+using namespace Eigen;
+
+// ================================================
+EdgeSE2MaxMixture::EdgeSE2MaxMixture() : g2o::EdgeSE2::EdgeSE2()
+{
+  nullHypothesisMoreLikely = false;
+}
+
+// ================================================
+bool EdgeSE2MaxMixture::read(std::istream& is)
+  {
+    Vector3d p;
+    is >>  weight >> p[0] >> p[1] >> p[2];
+    setMeasurement(g2o::SE2(p));
+    _inverseMeasurement = measurement().inverse();
+    //measurement().fromVector(p);
+    //inverseMeasurement() = measurement().inverse();
+    for (int i = 0; i < 3; ++i)
+      for (int j = i; j < 3; ++j) {
+        is >> information()(i, j);
+        if (i != j)
+          information()(j, i) = information()(i, j);
+      }
+
+    information_constraint = _information;
+    nu_constraint = 1.0/sqrt(information_constraint.inverse().determinant());
+    information_nullHypothesis = information_constraint*weight;
+    nu_nullHypothesis = 1.0/sqrt(information_nullHypothesis.inverse().determinant());
+
+    return true;
+  }
+// ================================================
+bool EdgeSE2MaxMixture::write(std::ostream& os) const
+{
+    Vector3d p = measurement().toVector();
+    os << p.x() << " " << p.y() << " " << p.z();
+    for (int i = 0; i < 3; ++i)
+      for (int j = i; j < 3; ++j)
+        os << " " << information()(i, j);
+    return os.good();
+}
+
+// ================================================
+void EdgeSE2MaxMixture::linearizeOplus()
+{
+  g2o::EdgeSE2::linearizeOplus();
+  if (nullHypothesisMoreLikely) {
+    _jacobianOplusXi *= weight;
+    _jacobianOplusXj *= weight;
+  }
+}
+
+// ================================================
+void EdgeSE2MaxMixture::computeError()
+{
+  // calculate the error for this constraint
+  g2o::EdgeSE2::computeError();
+
+  // determine the likelihood for constraint and null hypothesis
+  double mahal_constraint = _error.transpose() * information_constraint * _error;
+  double likelihood_constraint = nu_constraint * exp(-mahal_constraint);
+
+  double mahal_nullHypothesis  = _error.transpose() * (information_nullHypothesis) * _error;
+  double likelihood_nullHypothesis = nu_nullHypothesis * exp(-mahal_nullHypothesis);
+
+  // if the nullHypothesis is more likely ...
+  if (likelihood_nullHypothesis > likelihood_constraint) {
+    _information = information_nullHypothesis;
+    nullHypothesisMoreLikely = true;
+  }
+  else {
+    _information = information_constraint;
+    nullHypothesisMoreLikely = false;
+  }
+
+
+}
+
+
+// ================================================
+#ifdef G2O_HAVE_OPENGL
+  EdgeSE2MaxMixtureDrawAction::EdgeSE2MaxMixtureDrawAction(): DrawAction(typeid(EdgeSE2MaxMixture).name()){}
+
+  g2o::HyperGraphElementAction* EdgeSE2MaxMixtureDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element,
+               g2o::HyperGraphElementAction::Parameters* /*params_*/){
+    if (typeid(*element).name()!=_typeName)
+      return 0;
+    EdgeSE2MaxMixture* e =  static_cast<EdgeSE2MaxMixture*>(element);
+
+
+    g2o::VertexSE2* fromEdge = static_cast<g2o::VertexSE2*>(e->vertices()[0]);
+    g2o::VertexSE2* toEdge   = static_cast<g2o::VertexSE2*>(e->vertices()[1]);
+
+
+    if (e->nullHypothesisMoreLikely) glColor3f(0.0,0.0,0.0);
+    else glColor3f(1.0,0.5,0.2);
+
+    glPushAttrib(GL_ENABLE_BIT);
+    glDisable(GL_LIGHTING);
+    glBegin(GL_LINES);
+    glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.);
+    glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.);
+    glEnd();
+    glPopAttrib();
+    return this;
+  }
+#endif
+
+
+

corelib/src/vertigo/g2o/edge_se2MaxMixture.h

@@ -0,0 +1,46 @@
+/*
+ * edge_se2MaxMixture.h
+ *
+ *  Created on: 12.06.2012
+ *      Author: niko
+ */
+
+#ifndef EDGE_SE2MAXMIXTURE_H_
+#define EDGE_SE2MAXMIXTURE_H_
+
+#include "g2o/types/slam2d/vertex_se2.h"
+#include "g2o/types/slam2d/edge_se2.h"
+
+
+class EdgeSE2MaxMixture : public g2o::EdgeSE2
+{
+  public:
+    EdgeSE2MaxMixture();
+
+    virtual bool read(std::istream& is);
+    virtual bool write(std::ostream& os) const;
+    void computeError();
+    void linearizeOplus();
+
+    double weight;
+    
+    bool nullHypothesisMoreLikely;
+    
+    InformationType information_nullHypothesis;
+    double nu_nullHypothesis;
+    InformationType information_constraint;
+    double nu_constraint ;
+};
+
+
+#ifdef G2O_HAVE_OPENGL
+    class EdgeSE2MaxMixtureDrawAction: public g2o::DrawAction{
+    public:
+      EdgeSE2MaxMixtureDrawAction();
+      virtual g2o::HyperGraphElementAction* operator()(g2o::HyperGraph::HyperGraphElement* element,
+              g2o::HyperGraphElementAction::Parameters* params_);
+    };
+#endif
+
+
+#endif /* EDGE_SE2MAXMIXTURE_H_ */

corelib/src/vertigo/g2o/edge_se2Switchable.cpp

@@ -0,0 +1,131 @@
+/*
+ * edge_se2Switchable.cpp
+ *
+ *  Created on: 13.07.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+#include "vertigo/g2o/edge_se2Switchable.h"
+#include "vertigo/g2o/vertex_switchLinear.h"
+#include <GL/gl.h>
+
+using namespace std;
+using namespace Eigen;
+
+
+// ================================================
+EdgeSE2Switchable::EdgeSE2Switchable() : g2o::BaseMultiEdge<3, g2o::SE2>()
+{
+  resize(3);
+  _jacobianOplus[0].resize(3,3);    
+  _jacobianOplus[1].resize(3,3);
+  _jacobianOplus[2].resize(3,1); 
+
+}
+// ================================================
+bool EdgeSE2Switchable::read(std::istream& is)
+  {
+    Vector3d p;
+    is >> p[0] >> p[1] >> p[2];
+    setMeasurement(g2o::SE2(p));
+    _inverseMeasurement = measurement().inverse();
+
+    for (int i = 0; i < 3; ++i)
+      for (int j = i; j < 3; ++j) {
+        is >> information()(i, j);
+        if (i != j)
+          information()(j, i) = information()(i, j);
+      }
+    return true;
+  }
+// ================================================
+bool EdgeSE2Switchable::write(std::ostream& os) const
+{
+    Vector3d p = measurement().toVector();
+    os << p.x() << " " << p.y() << " " << p.z();
+    for (int i = 0; i < 3; ++i)
+      for (int j = i; j < 3; ++j)
+        os << " " << information()(i, j);
+    return os.good();
+}
+
+// ================================================
+void EdgeSE2Switchable::linearizeOplus()
+{
+   
+    const g2o::VertexSE2* vi = static_cast<const g2o::VertexSE2*>(_vertices[0]);
+    const g2o::VertexSE2* vj = static_cast<const g2o::VertexSE2*>(_vertices[1]);
+    const VertexSwitchLinear* vSwitch = static_cast<const VertexSwitchLinear*>(_vertices[2]);
+
+    double thetai = vi->estimate().rotation().angle();
+
+    Vector2d dt = vj->estimate().translation() - vi->estimate().translation();
+    double si=sin(thetai), ci=cos(thetai);
+
+    _jacobianOplus[0](0, 0) = -ci; _jacobianOplus[0](0, 1) = -si; _jacobianOplus[0](0, 2) = -si*dt.x()+ci*dt.y();
+    _jacobianOplus[0](1, 0) =  si; _jacobianOplus[0](1, 1) = -ci; _jacobianOplus[0](1, 2) = -ci*dt.x()-si*dt.y();
+    _jacobianOplus[0](2, 0) =  0;  _jacobianOplus[0](2, 1) = 0;   _jacobianOplus[0](2, 2) = -1;
+
+    _jacobianOplus[1](0, 0) = ci; _jacobianOplus[1](0, 1)= si; _jacobianOplus[1](0, 2)= 0;
+    _jacobianOplus[1](1, 0) =-si; _jacobianOplus[1](1, 1)= ci; _jacobianOplus[1](1, 2)= 0;
+    _jacobianOplus[1](2, 0) = 0;  _jacobianOplus[1](2, 1)= 0;  _jacobianOplus[1](2, 2)= 1;
+
+    const g2o::SE2& rmean = _inverseMeasurement;
+    Matrix3d z = Matrix3d::Zero();
+    z.block<2, 2>(0, 0) = rmean.rotation().toRotationMatrix();
+    z(2, 2) = 1.;
+    _jacobianOplus[0] = z * _jacobianOplus[0];
+    _jacobianOplus[1] = z * _jacobianOplus[1];
+
+
+    _jacobianOplus[0]*=vSwitch->estimate();
+    _jacobianOplus[1]*=vSwitch->estimate();
+
+
+    // derivative w.r.t switch vertex
+    _jacobianOplus[2].setZero();
+    g2o::SE2 delta = _inverseMeasurement * (vi->estimate().inverse()*vj->estimate());
+    _jacobianOplus[2] = delta.toVector() * vSwitch->gradient();  
+}
+
+// ================================================
+void EdgeSE2Switchable::computeError()
+{
+    const g2o::VertexSE2* v1 = static_cast<const g2o::VertexSE2*>(_vertices[0]);
+    const g2o::VertexSE2* v2 = static_cast<const g2o::VertexSE2*>(_vertices[1]);
+    const VertexSwitchLinear* v3 = static_cast<const VertexSwitchLinear*>(_vertices[2]);
+  
+    g2o::SE2 delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
+    _error = delta.toVector() * v3->estimate();
+}
+
+
+#ifdef G2O_HAVE_OPENGL
+  EdgeSE2SwitchableDrawAction::EdgeSE2SwitchableDrawAction(): DrawAction(typeid(EdgeSE2Switchable).name()){}
+
+  g2o::HyperGraphElementAction* EdgeSE2SwitchableDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element,
+               g2o::HyperGraphElementAction::Parameters* /*params_*/){
+    if (typeid(*element).name()!=_typeName)
+      return 0;
+    EdgeSE2Switchable* e =  static_cast<EdgeSE2Switchable*>(element);
+
+
+    g2o::VertexSE2* fromEdge = static_cast<g2o::VertexSE2*>(e->vertices()[0]);
+    g2o::VertexSE2* toEdge   = static_cast<g2o::VertexSE2*>(e->vertices()[1]);
+    VertexSwitchLinear* s   = static_cast<VertexSwitchLinear*>(e->vertices()[2]);
+
+    glColor3f(s->estimate()*1.0,s->estimate()*0.1,s->estimate()*0.1);
+    glPushAttrib(GL_ENABLE_BIT);
+    glDisable(GL_LIGHTING);
+    glBegin(GL_LINES);
+    glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.);
+    glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.);
+    glEnd();
+    glPopAttrib();
+    return this;
+  }
+#endif
+

corelib/src/vertigo/g2o/edge_se2Switchable.h

@@ -0,0 +1,50 @@
+/*
+ * edge_se2Switchable.h
+ *
+ *  Created on: 13.07.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+#ifndef EDGE_SE2SWITCHABLE_H_
+#define EDGE_SE2SWITCHABLE_H_
+
+#include "g2o/types/slam2d/vertex_se2.h"
+#include "g2o/core/base_multi_edge.h"
+#include "g2o/core/hyper_graph_action.h"
+
+class EdgeSE2Switchable : public g2o::BaseMultiEdge<3, g2o::SE2>
+{
+  public:
+    EdgeSE2Switchable();
+
+    virtual bool read(std::istream& is);
+    virtual bool write(std::ostream& os) const;
+    void computeError();
+    void linearizeOplus();
+
+
+    virtual void setMeasurement(const g2o::SE2& m){
+      _measurement = m;
+      _inverseMeasurement = m.inverse();
+    }
+
+    protected:
+      g2o::SE2 _inverseMeasurement;
+};
+
+
+#ifdef G2O_HAVE_OPENGL
+    class EdgeSE2SwitchableDrawAction: public g2o::DrawAction{
+    public:
+      EdgeSE2SwitchableDrawAction();
+      virtual g2o::HyperGraphElementAction* operator()(g2o::HyperGraph::HyperGraphElement* element,
+              g2o::HyperGraphElementAction::Parameters* params_);
+    };
+#endif
+
+
+
+#endif /* EDGE_SE2SWITCHABLE_H_ */

corelib/src/vertigo/g2o/edge_se3Switchable.cpp

@@ -0,0 +1,120 @@
+/*
+ * edge_se3Switchable.cpp
+ *
+ *  Created on: 17.10.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+#include "vertigo/g2o/edge_se3Switchable.h"
+#include "vertigo/g2o/vertex_switchLinear.h"
+#include <GL/gl.h>
+#include "g2o/types/slam3d/vertex_se3.h"
+#include "g2o/types/slam3d/isometry3d_gradients.h"
+
+
+using namespace std;
+using namespace Eigen;
+
+
+// ================================================
+EdgeSE3Switchable::EdgeSE3Switchable() : g2o::BaseMultiEdge<6, Eigen::Isometry3d>()
+{
+  resize(3);
+  _jacobianOplus[0].resize(6,6); 
+  _jacobianOplus[1].resize(6,6);
+  _jacobianOplus[2].resize(6,1);
+
+}
+// ================================================
+bool EdgeSE3Switchable::read(std::istream& is)
+  {    
+    g2o::Vector7d meas;
+    for (int i=0; i<7; i++) 
+      is >> meas[i];
+    // normalize the quaternion to recover numerical precision lost by storing as human readable text
+    Vector4d::MapType(meas.data()+3).normalize();
+    setMeasurement(g2o::internal::fromVectorQT(meas));
+
+    for (int i=0; i<6; i++)
+      for (int j=i; j<6; j++) {
+        is >> information()(i,j);
+        if (i!=j)
+          information()(j,i) = information()(i,j);
+      }
+    return true;
+
+  }
+// ================================================
+bool EdgeSE3Switchable::write(std::ostream& os) const
+{
+    g2o::Vector7d meas = g2o::internal::toVectorQT(measurement());
+    for (int i=0; i<7; i++) os  << meas[i] << " ";
+    for (int i = 0; i < 6; ++i)
+      for (int j = i; j < 6; ++j)
+        os << " " << information()(i, j);
+    return os.good();
+}
+
+// ================================================
+void EdgeSE3Switchable::linearizeOplus()
+{
+
+    g2o::VertexSE3* from = static_cast<g2o::VertexSE3*>(_vertices[0]);
+    g2o::VertexSE3* to = static_cast<g2o::VertexSE3*>(_vertices[1]);
+    const VertexSwitchLinear* vSwitch = static_cast<const VertexSwitchLinear*>(_vertices[2]);
+
+    Eigen::Isometry3d E;
+    const Eigen::Isometry3d& Xi=from->estimate();
+    const Eigen::Isometry3d& Xj=to->estimate();
+    const Eigen::Isometry3d& Z=_measurement;
+    g2o::internal::computeEdgeSE3Gradient(E, _jacobianOplus[0], _jacobianOplus[1], Z, Xi, Xj);
+
+    _jacobianOplus[0]*=vSwitch->estimate();
+    _jacobianOplus[1]*=vSwitch->estimate();
+
+    // derivative w.r.t switch vertex
+    _jacobianOplus[2].setZero();
+    _jacobianOplus[2] = g2o::internal::toVectorMQT(E) * vSwitch->gradient();
+}
+
+
+// ================================================
+void EdgeSE3Switchable::computeError()
+{
+    const g2o::VertexSE3* v1 = dynamic_cast<const g2o::VertexSE3*>(_vertices[0]);
+    const g2o::VertexSE3* v2 = dynamic_cast<const g2o::VertexSE3*>(_vertices[1]);
+    const VertexSwitchLinear* v3 = static_cast<const VertexSwitchLinear*>(_vertices[2]);
+
+    Eigen::Isometry3d delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
+    _error = g2o::internal::toVectorMQT(delta) * v3->estimate();
+}
+
+
+#ifdef G2O_HAVE_OPENGL
+  EdgeSE3SwitchableDrawAction::EdgeSE3SwitchableDrawAction(): DrawAction(typeid(EdgeSE3Switchable).name()){}
+
+  g2o::HyperGraphElementAction* EdgeSE3SwitchableDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element,
+               g2o::HyperGraphElementAction::Parameters* /*params_*/){
+    if (typeid(*element).name()!=_typeName)
+      return 0;
+    EdgeSE3Switchable* e =  static_cast<EdgeSE3Switchable*>(element);
+
+
+    g2o::VertexSE3* fromEdge = static_cast<g2o::VertexSE3*>(e->vertices()[0]);
+    g2o::VertexSE3* toEdge   = static_cast<g2o::VertexSE3*>(e->vertices()[1]);
+    VertexSwitchLinear* s   = static_cast<VertexSwitchLinear*>(e->vertices()[2]);
+
+    glColor3f(s->estimate()*1.0,s->estimate()*0.1,s->estimate()*0.1);
+    glPushAttrib(GL_ENABLE_BIT);
+    glDisable(GL_LIGHTING);
+    glBegin(GL_LINES);
+    glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),fromEdge->estimate().translation().z());
+    glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),toEdge->estimate().translation().z());
+    glEnd();
+    glPopAttrib();
+    return this;
+  }
+#endif

corelib/src/vertigo/g2o/edge_se3Switchable.h

@@ -0,0 +1,48 @@
+/*
+ * edge_se3Switchable.h
+ *
+ *  Created on: 17.10.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+#ifndef EDGE_SE3SWITCHABLE_H_
+#define EDGE_SE3SWITCHABLE_H_
+
+#include "g2o/types/slam3d/vertex_se3.h"
+#include "g2o/core/base_multi_edge.h"
+#include "g2o/core/hyper_graph_action.h"
+
+class EdgeSE3Switchable : public g2o::BaseMultiEdge<6, Eigen::Isometry3d>
+{
+  public:
+    EdgeSE3Switchable();
+
+    virtual bool read(std::istream& is);
+    virtual bool write(std::ostream& os) const;
+    void computeError();
+    void linearizeOplus();
+
+    virtual void setMeasurement(const Eigen::Isometry3d& m){
+      _measurement = m;
+      _inverseMeasurement = m.inverse();
+    }
+
+  protected:
+    Eigen::Isometry3d _inverseMeasurement;
+};
+
+
+#ifdef G2O_HAVE_OPENGL
+    class EdgeSE3SwitchableDrawAction: public g2o::DrawAction{
+    public:
+      EdgeSE3SwitchableDrawAction();
+      virtual g2o::HyperGraphElementAction* operator()(g2o::HyperGraph::HyperGraphElement* element,
+              g2o::HyperGraphElementAction::Parameters* params_);
+    };
+#endif
+
+
+#endif /* EDGE_SE3SWITCHABLE_H_ */

corelib/src/vertigo/g2o/edge_switchPrior.cpp

@@ -0,0 +1,44 @@
+#include "vertigo/g2o/edge_switchPrior.h"
+using namespace std;
+
+    EdgeSwitchPrior::EdgeSwitchPrior()
+    {
+    	setMeasurement(1.0);
+    }
+
+    bool EdgeSwitchPrior::read(std::istream &is)
+    {
+      double new_measurement;
+      is >> new_measurement;
+
+      setMeasurement(new_measurement);
+
+      is >> information()(0,0);
+      return true;
+    }
+
+    bool EdgeSwitchPrior::write(std::ostream &os) const
+    {
+      os << measurement() << " " << information()(0,0);
+      return true;
+    }
+
+    void EdgeSwitchPrior::setMeasurement(const double & m)
+    {
+    	g2o::BaseEdge<1, double>::setMeasurement(m);
+    	information()(0,0) = 1.0;
+    }
+
+    void EdgeSwitchPrior::linearizeOplus()
+    {
+      _jacobianOplusXi[0]=-1.0;
+    }
+
+    void EdgeSwitchPrior::computeError()
+    {
+      const VertexSwitchLinear* s = static_cast<const VertexSwitchLinear*>(_vertices[0]);
+
+      _error[0] = measurement() - s->x();
+
+    }
+

corelib/src/vertigo/g2o/edge_switchPrior.h

@@ -0,0 +1,22 @@
+#pragma once
+
+
+#include "vertex_switchLinear.h"
+#include "g2o/core/base_unary_edge.h"
+
+
+
+class EdgeSwitchPrior : public g2o::BaseUnaryEdge<1, double, VertexSwitchLinear>
+{
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    EdgeSwitchPrior();
+
+    virtual bool read(std::istream& is);
+    virtual bool write(std::ostream& os) const;
+
+    virtual void setMeasurement(const double & m);
+
+    virtual void linearizeOplus();
+    void computeError();
+};

corelib/src/vertigo/g2o/types_g2o_robust.cpp

@@ -0,0 +1,22 @@
+#include "g2o/core/factory.h"
+#include "g2o/stuff/macros.h"
+
+#include "vertigo/g2o/edge_switchPrior.h"
+#include "vertigo/g2o/edge_se2Switchable.h"
+//#include "vertigo/g2o/edge_se2MaxMixture.h"
+#include "vertigo/g2o/edge_se3Switchable.h"
+#include "vertigo/g2o/vertex_switchLinear.h"
+
+
+G2O_REGISTER_TYPE(EDGE_SWITCH_PRIOR, EdgeSwitchPrior);
+G2O_REGISTER_TYPE(EDGE_SE2_SWITCHABLE, EdgeSE2Switchable);
+//G2O_REGISTER_TYPE(EDGE_SE2_MAXMIX, EdgeSE2MaxMixture);
+G2O_REGISTER_TYPE(EDGE_SE3_SWITCHABLE, EdgeSE3Switchable);
+G2O_REGISTER_TYPE(VERTEX_SWITCH, VertexSwitchLinear);
+
+#ifdef G2O_HAVE_OPENGL
+  G2O_REGISTER_ACTION(EdgeSE2SwitchableDrawAction);
+  //G2O_REGISTER_ACTION(EdgeSE2MaxMixtureDrawAction);
+  G2O_REGISTER_ACTION(EdgeSE3SwitchableDrawAction);
+#endif
+

corelib/src/vertigo/g2o/vertex_switchLinear.cpp

@@ -0,0 +1,59 @@
+/*
+ * vertex_switchLinear.cpp
+ *
+ *  Created on: 17.10.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+#include "vertigo/g2o/vertex_switchLinear.h"
+#include <iostream>
+
+using namespace std;
+
+    VertexSwitchLinear::VertexSwitchLinear() :
+		_x(0)
+    {
+    	setToOrigin();
+    	setEstimate(1.0);
+    }
+
+    bool VertexSwitchLinear:: read(std::istream& is)
+    {
+      is >> _x;
+      _estimate=_x;
+
+      return true;
+    }
+
+    bool  VertexSwitchLinear::write(std::ostream& os) const
+    {
+      os << _x;
+      return os.good();
+    }
+
+    void VertexSwitchLinear::setToOriginImpl()
+    {
+      _x=0;
+      _estimate=_x;
+    }
+
+
+    void VertexSwitchLinear::setEstimate(const double &et)
+    {
+      _x=et;
+      _estimate=_x;
+    }
+
+
+    void VertexSwitchLinear::oplusImpl(const double* update)
+    {
+      _x += update[0];
+
+      if (_x<0) _x=0;
+      if (_x>1) _x=1;
+
+      _estimate=_x;
+    }

corelib/src/vertigo/g2o/vertex_switchLinear.h

@@ -0,0 +1,43 @@
+/*
+ * vertex_switchLinear.h
+ *
+ *  Created on: 17.10.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+
+#pragma once
+
+#include "g2o/core/base_vertex.h"
+#include <math.h>
+
+
+
+    class VertexSwitchLinear : public g2o::BaseVertex<1, double>
+    {
+
+    public:
+      VertexSwitchLinear();
+
+      virtual void setToOriginImpl();
+
+      virtual void oplusImpl(const double* update);
+
+      virtual bool read(std::istream& is);
+      virtual bool write(std::ostream& os) const;
+      virtual void setEstimate(const double &et);
+
+
+      double x() const { return _x; };
+
+
+      //! The gradient at the current estimate is always 1;
+      double gradient() const { return 1; } ;
+
+    private:
+      double _x;
+
+    };

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_ */

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_ */

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_ */

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_ */

corelib/src/vertigo/readme.txt

@@ -0,0 +1,8 @@
+
+Info: http://openslam.org/vertigo.html
+Source: https://github.com/christiankerl/vertigo/tree/master/trunk
+Commit: fbd438488a56cdba805fa2f75aa3e394e3eda7ff
+License: GPL v3
+
+Tested with g2o (ROS Indigo/2014.02.18)
+Tested with GTSAM commit c73b835