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