From 403dcd6a0017bec32a2d71fbeb1a781c77e8c9f1 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 26 Oct 2010 20:26:02 +0000 Subject: [PATCH] Moved unused simulated2DConstraints to experimental --- gtsam/slam/simulated2DConstraints.h | 134 ---------------------------- 1 file changed, 134 deletions(-) delete mode 100644 gtsam/slam/simulated2DConstraints.h diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h deleted file mode 100644 index 6dd05f14e..000000000 --- a/gtsam/slam/simulated2DConstraints.h +++ /dev/null @@ -1,134 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - - * -------------------------------------------------------------------------- */ - -/** - * @file simulated2DConstraints.h - * @brief measurement functions and constraint definitions for simulated 2D robot - * @author Alex Cunningham - */ - -// \callgraph - -#pragma once - -#include - -#include -#include -#include -#include - -// \namespace - -namespace gtsam { - - namespace simulated2D { - - namespace equality_constraints { - - /** Typedefs for regular use */ - typedef NonlinearEquality1 UnaryEqualityConstraint; - typedef NonlinearEquality1 UnaryEqualityPointConstraint; - typedef BetweenConstraint OdoEqualityConstraint; - - /** Equality between variables */ - typedef NonlinearEquality2 PoseEqualityConstraint; - typedef NonlinearEquality2 PointEqualityConstraint; - - } // \namespace equality_constraints - - namespace inequality_constraints { - - /** - * Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c) - * Demo implementation: should be made more general using BoundingConstraint - */ - template - struct ScalarCoordConstraint1: public BoundingConstraint1 { - typedef BoundingConstraint1 Base; - typedef boost::shared_ptr > shared_ptr; - - ScalarCoordConstraint1(const Key& key, double c, - bool isGreaterThan, double mu = 1000.0) : - Base(key, c, isGreaterThan, mu) { - } - - inline unsigned int index() const { return Idx; } - - /** extracts a single value from the point */ - virtual double value(const Point2& x, boost::optional H = - boost::none) const { - if (H) { - Matrix D = zeros(1, 2); - D(0, Idx) = 1.0; - *H = D; - } - return x.vector()(Idx); - } - }; - - /** typedefs for use with simulated2D systems */ - typedef ScalarCoordConstraint1 PoseXInequality; - typedef ScalarCoordConstraint1 PoseYInequality; - - double range(const Point2& a, const Point2& b) { return a.dist(b); } - - /** - * Binary inequality constraint forcing the range between points - * to be less than or equal to a bound - */ - template - struct MaxDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 Base; - - MaxDistanceConstraint(const Key& key1, const Key& key2, double range_bound, double mu = 1000.0) - : Base(key1, key2, range_bound, false, mu) {} - - /** extracts a single scalar value with derivatives */ - virtual double value(const Point2& x1, const Point2& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - if (H1) *H1 = numericalDerivative21(range, x1, x2, 1e-5); - if (H1) *H2 = numericalDerivative22(range, x1, x2, 1e-5); - return x1.dist(x2); - } - }; - - typedef MaxDistanceConstraint PoseMaxDistConstraint; - - /** - * Binary inequality constraint forcing a minimum range - * NOTE: this is not a convex function! Be careful with initialization. - */ - template - struct MinDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 Base; - - MinDistanceConstraint(const XKey& key1, const PKey& key2, double range_bound, double mu = 1000.0) - : Base(key1, key2, range_bound, true, mu) {} - - /** extracts a single scalar value with derivatives */ - virtual double value(const Point2& x1, const Point2& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - if (H1) *H1 = numericalDerivative21(range, x1, x2, 1e-5); - if (H1) *H2 = numericalDerivative22(range, x1, x2, 1e-5); - return x1.dist(x2); - } - }; - - typedef MinDistanceConstraint LandmarkAvoid; - - - } // \namespace inequality_constraints - - } // \namespace simulated2D -} // \namespace gtsam