simulated2DConstraints.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */
 
// \callgraph
 
#pragma once
 
#include <gtsam/base/numericalDerivative.h>
 
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BoundingConstraint.h>
#include <tests/simulated2D.h>
#include "gtsam/nonlinear/NonlinearFactor.h"
 
// \namespace
 
namespace simulated2D {
 
  namespace equality_constraints {
 
    typedef NonlinearEquality1<Point2> UnaryEqualityConstraint;
    typedef NonlinearEquality1<Point2> UnaryEqualityPointConstraint;
    typedef BetweenConstraint<Point2> OdoEqualityConstraint;
 
    typedef NonlinearEquality2<Point2> PoseEqualityConstraint;
    typedef NonlinearEquality2<Point2> PointEqualityConstraint;
 
  } // \namespace equality_constraints
 
  namespace inequality_constraints {
 
    template<class VALUE, unsigned int IDX>
    struct ScalarCoordConstraint1: public BoundingConstraint1<VALUE> {
      typedef BoundingConstraint1<VALUE> Base;  
      typedef ScalarCoordConstraint1<VALUE, IDX> This; 
      typedef std::shared_ptr<ScalarCoordConstraint1<VALUE, IDX> > shared_ptr; 
      typedef VALUE Point; 
 
      ~ScalarCoordConstraint1() override {}
 
      gtsam::NonlinearFactor::shared_ptr clone() const override {
        return std::static_pointer_cast<gtsam::NonlinearFactor>(
            gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
      ScalarCoordConstraint1(Key key, double c,
          bool isGreaterThan, double mu = 1000.0) :
            Base(key, c, isGreaterThan, mu) {
      }
 
      inline unsigned int index() const { return IDX; }
 
      double value(const Point& x, OptionalMatrixType H =
          OptionalNone) const override {
        if (H) {
          Matrix D = Matrix::Zero(1, traits<Point>::GetDimension(x));
          D(0, IDX) = 1.0;
          *H = D;
        }
        return traits<Point>::Logmap(x)(IDX);
      }
    };
 
    typedef ScalarCoordConstraint1<Point2, 0> PoseXInequality; 
    typedef ScalarCoordConstraint1<Point2, 1> PoseYInequality; 
 
    template<class T1, class T2>
    double range_trait(const T1& a, const T2& b) { return distance2(a, b); }
 
    template<class VALUE>
    struct MaxDistanceConstraint : public BoundingConstraint2<VALUE, VALUE> {
      typedef BoundingConstraint2<VALUE, VALUE> Base;  
      typedef MaxDistanceConstraint<VALUE> This;  
      typedef VALUE Point; 
 
      ~MaxDistanceConstraint() override {}
 
      gtsam::NonlinearFactor::shared_ptr clone() const override {
        return std::static_pointer_cast<gtsam::NonlinearFactor>(
            gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
      MaxDistanceConstraint(Key key1, Key key2, double range_bound, double mu = 1000.0) :
        Base(key1, key2, range_bound, false, mu) {}
 
      double value(const Point& x1, const Point& x2,
          OptionalMatrixType H1 = OptionalNone,
          OptionalMatrixType H2 = OptionalNone) const override {
        if (H1) *H1 = numericalDerivative21(range_trait<Point,Point>, x1, x2, 1e-5);
        if (H1) *H2 = numericalDerivative22(range_trait<Point,Point>, x1, x2, 1e-5);
        return range_trait(x1, x2);
      }
    };
 
    typedef MaxDistanceConstraint<Point2> PoseMaxDistConstraint; 
 
    template<class POSE, class POINT>
    struct MinDistanceConstraint : public BoundingConstraint2<POSE, POINT> {
      typedef BoundingConstraint2<POSE, POINT> Base; 
      typedef MinDistanceConstraint<POSE, POINT> This; 
      typedef POSE Pose; 
      typedef POINT Point; 
 
      ~MinDistanceConstraint() override {}
 
      gtsam::NonlinearFactor::shared_ptr clone() const override {
        return std::static_pointer_cast<gtsam::NonlinearFactor>(
            gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
      MinDistanceConstraint(Key key1, Key key2,
          double range_bound, double mu = 1000.0)
      : Base(key1, key2, range_bound, true, mu) {}
 
      double value(const Pose& x1, const Point& x2,
          OptionalMatrixType H1 = OptionalNone,
          OptionalMatrixType H2 = OptionalNone) const override {
        if (H1) *H1 = numericalDerivative21(range_trait<Pose,Point>, x1, x2, 1e-5);
        if (H1) *H2 = numericalDerivative22(range_trait<Pose,Point>, x1, x2, 1e-5);
        return range_trait(x1, x2);
      }
    };
 
    typedef MinDistanceConstraint<Point2, Point2> LandmarkAvoid; 
 
 
  } // \namespace inequality_constraints
 
} // \namespace simulated2D