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