1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file simulated2DConstraints.h
14 * @brief measurement functions and constraint definitions for simulated 2D robot
15 * @author Alex Cunningham
16 */
17
18// \callgraph
19
20#pragma once
21
22#include <gtsam/base/numericalDerivative.h>
23
24#include <gtsam/nonlinear/NonlinearEquality.h>
25#include <gtsam/slam/BetweenFactor.h>
26#include <gtsam/slam/BoundingConstraint.h>
27#include <tests/simulated2D.h>
28#include "gtsam/nonlinear/NonlinearFactor.h"
29
30// \namespace
31
32namespace simulated2D {
33
34 namespace equality_constraints {
35
36 /** Typedefs for regular use */
37 typedef NonlinearEquality1<Point2> UnaryEqualityConstraint;
38 typedef NonlinearEquality1<Point2> UnaryEqualityPointConstraint;
39 typedef BetweenConstraint<Point2> OdoEqualityConstraint;
40
41 /** Equality between variables */
42 typedef NonlinearEquality2<Point2> PoseEqualityConstraint;
43 typedef NonlinearEquality2<Point2> PointEqualityConstraint;
44
45 } // \namespace equality_constraints
46
47 namespace inequality_constraints {
48
49 /**
50 * Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c)
51 * @tparam VALUE is the value type for the variable constrained, e.g. Pose2, Point3, etc.
52 * @tparam IDX is an index in tangent space to constrain, must be less than KEY::VALUE::Dim()
53 */
54 template<class VALUE, unsigned int IDX>
55 struct ScalarCoordConstraint1: public BoundingConstraint1<VALUE> {
56 typedef BoundingConstraint1<VALUE> Base; ///< Base class convenience typedef
57 typedef ScalarCoordConstraint1<VALUE, IDX> This; ///< This class convenience typedef
58 typedef std::shared_ptr<ScalarCoordConstraint1<VALUE, IDX> > shared_ptr; ///< std::shared_ptr convenience typedef
59 typedef VALUE Point; ///< Constrained variable type
60
61 ~ScalarCoordConstraint1() override {}
62
63 /// @return a deep copy of this factor
64 gtsam::NonlinearFactor::shared_ptr clone() const override {
65 return std::static_pointer_cast<gtsam::NonlinearFactor>(
66 r: gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
67
68 /**
69 * Constructor for constraint
70 * @param key is the label for the constrained variable
71 * @param c is the measured value for the fixed coordinate
72 * @param isGreaterThan is a flag to set inequality as greater than or less than
73 * @param mu is the penalty function gain
74 */
75 ScalarCoordConstraint1(Key key, double c,
76 bool isGreaterThan, double mu = 1000.0) :
77 Base(key, c, isGreaterThan, mu) {
78 }
79
80 /**
81 * Access function for the constrained index
82 * @return the index for the constrained coordinate
83 */
84 inline unsigned int index() const { return IDX; }
85
86 /**
87 * extracts a single value from the point to compute error
88 * @param x is the estimate of the constrained variable being evaluated
89 * @param H is an optional Jacobian, linearized at x
90 */
91 double value(const Point& x, OptionalMatrixType H =
92 OptionalNone) const override {
93 if (H) {
94 Matrix D = Matrix::Zero(1, traits<Point>::GetDimension(x));
95 D(0, IDX) = 1.0;
96 *H = D;
97 }
98 return traits<Point>::Logmap(x)(IDX);
99 }
100 };
101
102 /** typedefs for use with simulated2D systems */
103 typedef ScalarCoordConstraint1<Point2, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
104 typedef ScalarCoordConstraint1<Point2, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
105
106 /**
107 * Trait for distance constraints to provide distance
108 * @tparam T1 is a Lie value for which distance functions exist
109 * @tparam T2 is a Lie value for which distance functions exist
110 * @param a is the first Lie element
111 * @param b is the second Lie element
112 * @return a scalar distance between values
113 */
114 template<class T1, class T2>
115 double range_trait(const T1& a, const T2& b) { return distance2(a, b); }
116
117 /**
118 * Binary inequality constraint forcing the range between points
119 * to be less than or equal to a bound
120 * @tparam VALUES is the variable set for the graph
121 * @tparam KEY is the type of the keys for the variables constrained
122 */
123 template<class VALUE>
124 struct MaxDistanceConstraint : public BoundingConstraint2<VALUE, VALUE> {
125 typedef BoundingConstraint2<VALUE, VALUE> Base; ///< Base class for factor
126 typedef MaxDistanceConstraint<VALUE> This; ///< This class for factor
127 typedef VALUE Point; ///< Type of variable constrained
128
129 ~MaxDistanceConstraint() override {}
130
131 /// @return a deep copy of this factor
132 gtsam::NonlinearFactor::shared_ptr clone() const override {
133 return std::static_pointer_cast<gtsam::NonlinearFactor>(
134 r: gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
135
136 /**
137 * Primary constructor for factor
138 * @param key1 is the first variable key
139 * @param key2 is the second variable key
140 * @param range_bound is the maximum range allowed between the variables
141 * @param mu is the gain for the penalty function
142 */
143 MaxDistanceConstraint(Key key1, Key key2, double range_bound, double mu = 1000.0) :
144 Base(key1, key2, range_bound, false, mu) {}
145
146 /**
147 * computes the range with derivatives
148 * @param x1 is the first variable value
149 * @param x2 is the second variable value
150 * @param H1 is an optional Jacobian in x1
151 * @param H2 is an optional Jacobian in x2
152 * @return the distance between the variables
153 */
154 double value(const Point& x1, const Point& x2,
155 OptionalMatrixType H1 = OptionalNone,
156 OptionalMatrixType H2 = OptionalNone) const override {
157 if (H1) *H1 = numericalDerivative21(range_trait<Point,Point>, x1, x2, 1e-5);
158 if (H1) *H2 = numericalDerivative22(range_trait<Point,Point>, x1, x2, 1e-5);
159 return range_trait(x1, x2);
160 }
161 };
162
163 typedef MaxDistanceConstraint<Point2> PoseMaxDistConstraint; ///< Simulated2D domain example factor
164
165 /**
166 * Binary inequality constraint forcing a minimum range
167 * NOTE: this is not a convex function! Be careful with initialization.
168 * @tparam POSE is the type of the pose value constrained
169 * @tparam POINT is the type of the point value constrained
170 */
171 template<class POSE, class POINT>
172 struct MinDistanceConstraint : public BoundingConstraint2<POSE, POINT> {
173 typedef BoundingConstraint2<POSE, POINT> Base; ///< Base class for factor
174 typedef MinDistanceConstraint<POSE, POINT> This; ///< This class for factor
175 typedef POSE Pose; ///< Type of pose variable constrained
176 typedef POINT Point; ///< Type of point variable constrained
177
178 ~MinDistanceConstraint() override {}
179
180 /// @return a deep copy of this factor
181 gtsam::NonlinearFactor::shared_ptr clone() const override {
182 return std::static_pointer_cast<gtsam::NonlinearFactor>(
183 r: gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
184
185 /**
186 * Primary constructor for factor
187 * @param key1 is the first variable key
188 * @param key2 is the second variable key
189 * @param range_bound is the minimum range allowed between the variables
190 * @param mu is the gain for the penalty function
191 */
192 MinDistanceConstraint(Key key1, Key key2,
193 double range_bound, double mu = 1000.0)
194 : Base(key1, key2, range_bound, true, mu) {}
195
196 /**
197 * computes the range with derivatives
198 * @param x1 is the first variable value
199 * @param x2 is the second variable value
200 * @param H1 is an optional Jacobian in x1
201 * @param H2 is an optional Jacobian in x2
202 * @return the distance between the variables
203 */
204 double value(const Pose& x1, const Point& x2,
205 OptionalMatrixType H1 = OptionalNone,
206 OptionalMatrixType H2 = OptionalNone) const override {
207 if (H1) *H1 = numericalDerivative21(range_trait<Pose,Point>, x1, x2, 1e-5);
208 if (H1) *H2 = numericalDerivative22(range_trait<Pose,Point>, x1, x2, 1e-5);
209 return range_trait(x1, x2);
210 }
211 };
212
213 typedef MinDistanceConstraint<Point2, Point2> LandmarkAvoid; ///< Simulated2D domain example factor
214
215
216 } // \namespace inequality_constraints
217
218} // \namespace simulated2D
219
220