testBoundingConstraint.cpp
Go to the documentation of this file.
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 
19 #include <gtsam/inference/Symbol.h>
22 
24 
26 using namespace std;
27 using namespace gtsam;
28 
29 static const double tol = 1e-5;
30 
31 SharedDiagonal soft_model2 = noiseModel::Unit::Create(2);
32 SharedDiagonal soft_model2_alt = noiseModel::Isotropic::Sigma(2, 0.1);
33 SharedDiagonal hard_model1 = noiseModel::Constrained::All(1);
34 
35 // some simple inequality constraints
37 double mu = 10.0;
38 // greater than
41 
42 // less than
45 
46 /* ************************************************************************* */
47 TEST( testBoundingConstraint, unary_basics_inactive1 ) {
48  Point2 pt1(2.0, 3.0);
49  Values config;
50  config.insert(key, pt1);
51  EXPECT(!constraint1.active(config));
52  EXPECT(!constraint2.active(config));
63 }
64 
65 /* ************************************************************************* */
66 TEST( testBoundingConstraint, unary_basics_inactive2 ) {
67  Point2 pt2(-2.0, -3.0);
68  Values config;
69  config.insert(key, pt2);
70  EXPECT(!constraint3.active(config));
71  EXPECT(!constraint4.active(config));
76  EXPECT(assert_equal(Vector::Constant(1, 3.0), constraint3.evaluateError(pt2), tol));
77  EXPECT(assert_equal(Vector::Constant(1, 5.0), constraint4.evaluateError(pt2), tol));
82 }
83 
84 /* ************************************************************************* */
85 TEST( testBoundingConstraint, unary_basics_active1 ) {
86  Point2 pt2(-2.0, -3.0);
87  Values config;
88  config.insert(key, pt2);
89  EXPECT(constraint1.active(config));
90  EXPECT(constraint2.active(config));
91  EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.evaluateError(pt2), tol));
92  EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.evaluateError(pt2), tol));
93  EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.unwhitenedError(config), tol));
94  EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.unwhitenedError(config), tol));
95  EXPECT_DOUBLES_EQUAL(45.0, constraint1.error(config), tol);
96  EXPECT_DOUBLES_EQUAL(125.0, constraint2.error(config), tol);
97 }
98 
99 /* ************************************************************************* */
100 TEST( testBoundingConstraint, unary_basics_active2 ) {
101  Point2 pt1(2.0, 3.0);
102  Values config;
103  config.insert(key, pt1);
104  EXPECT(constraint3.active(config));
105  EXPECT(constraint4.active(config));
106  EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol));
107  EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol));
108  EXPECT(assert_equal(-1.0 * I_1x1, constraint3.unwhitenedError(config), tol));
109  EXPECT(assert_equal(-1.0 * I_1x1, constraint4.unwhitenedError(config), tol));
110  EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol);
111  EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol);
112 }
113 
114 /* ************************************************************************* */
115 TEST( testBoundingConstraint, unary_linearization_inactive) {
116  Point2 pt1(2.0, 3.0);
117  Values config1;
118  config1.insert(key, pt1);
119  GaussianFactor::shared_ptr actual1 = constraint1.linearize(config1);
120  GaussianFactor::shared_ptr actual2 = constraint2.linearize(config1);
121  EXPECT(!actual1);
122  EXPECT(!actual2);
123 }
124 
125 /* ************************************************************************* */
126 TEST( testBoundingConstraint, unary_linearization_active) {
127  Point2 pt2(-2.0, -3.0);
128  Values config2;
129  config2.insert(key, pt2);
130  GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2);
131  GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2);
132  JacobianFactor expected1(key, (Matrix(1, 2) << 1.0, 0.0).finished(), Vector::Constant(1, 3.0), hard_model1);
133  JacobianFactor expected2(key, (Matrix(1, 2) << 0.0, 1.0).finished(), Vector::Constant(1, 5.0), hard_model1);
134  EXPECT(assert_equal((const GaussianFactor&)expected1, *actual1, tol));
135  EXPECT(assert_equal((const GaussianFactor&)expected2, *actual2, tol));
136 }
137 
138 /* ************************************************************************* */
139 TEST( testBoundingConstraint, unary_simple_optimization1) {
140  // create a single-node graph with a soft and hard constraint to
141  // ensure that the hard constraint overrides the soft constraint
142  Point2 goal_pt(1.0, 2.0);
143  Point2 start_pt(0.0, 1.0);
144 
146  Symbol x1('x',1);
147  graph.emplace_shared<iq2D::PoseXInequality>(x1, 1.0, true);
148  graph.emplace_shared<iq2D::PoseYInequality>(x1, 2.0, true);
149  graph.emplace_shared<simulated2D::Prior>(start_pt, soft_model2, x1);
150 
151  Values initValues;
152  initValues.insert(x1, start_pt);
153 
154  Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
156  expected.insert(x1, goal_pt);
157  CHECK(assert_equal(expected, actual, tol));
158 }
159 
160 /* ************************************************************************* */
161 TEST( testBoundingConstraint, unary_simple_optimization2) {
162  // create a single-node graph with a soft and hard constraint to
163  // ensure that the hard constraint overrides the soft constraint
164  Point2 goal_pt(1.0, 2.0);
165  Point2 start_pt(2.0, 3.0);
166 
168  graph.emplace_shared<iq2D::PoseXInequality>(key, 1.0, false);
169  graph.emplace_shared<iq2D::PoseYInequality>(key, 2.0, false);
171 
172  Values initValues;
173  initValues.insert(key, start_pt);
174 
175  Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
177  expected.insert(key, goal_pt);
178  CHECK(assert_equal(expected, actual, tol));
179 }
180 
181 /* ************************************************************************* */
182 TEST( testBoundingConstraint, MaxDistance_basics) {
183  gtsam::Key key1 = 1, key2 = 2;
184  Point2 pt1(0,0), pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
185  iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu);
186  EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol);
187  EXPECT(!rangeBound.isGreaterThan());
188  EXPECT(rangeBound.dim() == 1);
189 
190  EXPECT(assert_equal((Vector(1) << 2.0).finished(), rangeBound.evaluateError(pt1, pt1)));
191  EXPECT(assert_equal(I_1x1, rangeBound.evaluateError(pt1, pt2)));
192  EXPECT(assert_equal(Z_1x1, rangeBound.evaluateError(pt1, pt3)));
193  EXPECT(assert_equal(-1.0*I_1x1, rangeBound.evaluateError(pt1, pt4)));
194 
195  Values config1;
196  config1.insert(key1, pt1);
197  config1.insert(key2, pt1);
198  EXPECT(!rangeBound.active(config1));
199  EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1)));
200  EXPECT(!rangeBound.linearize(config1));
201  EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
202 
203  config1.update(key2, pt2);
204  EXPECT(!rangeBound.active(config1));
205  EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1)));
206  EXPECT(!rangeBound.linearize(config1));
207  EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
208 
209  config1.update(key2, pt3);
210  EXPECT(rangeBound.active(config1));
211  EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1)));
212  EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
213 
214  config1.update(key2, pt4);
215  EXPECT(rangeBound.active(config1));
216  EXPECT(assert_equal(-1.0*I_1x1, rangeBound.unwhitenedError(config1)));
217  EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol);
218 }
219 
220 /* ************************************************************************* */
221 TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
222 
223  Point2 pt1(0,0), pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
224  Symbol x1('x',1), x2('x',2);
225 
230 
231  Values initial_state;
232  initial_state.insert(x1, pt1);
233  initial_state.insert(x2, pt2_init);
234 
236  expected.insert(x1, pt1);
237  expected.insert(x2, pt2_goal);
238 
239  // FAILS: VectorValues assertion failure
240 // Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initial_state);
241 // EXPECT(assert_equal(expected, *actual, tol));
242 }
243 
244 /* ************************************************************************* */
245 TEST( testBoundingConstraint, avoid_demo) {
246 
247  Symbol x1('x',1), x2('x',2), x3('x',3), l1('l',1);
248  double radius = 1.0;
249  Point2 x1_pt(0,0), x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
250  Point2 odo(2.0, 0.0);
251 
255  graph.emplace_shared<iq2D::LandmarkAvoid>(x2, l1, radius);
259 
261  init.insert(x1, x1_pt);
262  init.insert(x3, x3_pt);
263  init.insert(l1, l1_pt);
264  expected = init;
265  init.insert(x2, x2_init);
266  expected.insert(x2, x2_goal);
267 
268  // FAILS: segfaults on optimization
269 // Optimizer::shared_values actual = Optimizer::optimizeLM(graph, init);
270 // EXPECT(assert_equal(expected, *actual, tol));
271 }
272 
273 /* ************************************************************************* */
274 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
275 /* ************************************************************************* */
276 
iq2D::PoseXInequality constraint1(key, 1.0, true, mu)
#define CHECK(condition)
Definition: Test.h:108
size_t dim() const override
virtual const Values & optimize()
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
static int runAllTests(TestResult &result)
Factor Graph consisting of non-linear factors.
Matrix expected
Definition: testMatrix.cpp:971
Point2 odo(const Point2 &x1, const Point2 &x2)
odometry between two poses
Definition: simulated2D.h:99
Vector2 Point2
Definition: Point2.h:32
iq2D::PoseYInequality constraint2(key, 2.0, true, mu)
double mu
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
void update(Key j, const Value &val)
Definition: Values.cpp:169
Definition: BFloat16.h:88
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
bool active(const Values &c) const override
NonlinearFactorGraph graph
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
SharedDiagonal hard_model1
gtsam::Key key
SharedDiagonal soft_model2_alt
const Symbol key1('v', 1)
#define EXPECT(condition)
Definition: Test.h:150
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
Vector evaluateError(const X &x, OptionalMatrixType H) const override
gtsam::Key l1
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
traits
Definition: chartTesting.h:28
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
double error(const Values &c) const override
Pose3 x1
Definition: testPose3.cpp:663
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:1882
iq2D::PoseYInequality constraint4(key, 2.0, false, mu)
iq2D::PoseXInequality constraint3(key, 1.0, false, mu)
SharedDiagonal soft_model2
void insert(Key j, const Value &val)
Definition: Values.cpp:155
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
bool active(const Values &c) const override
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
static Point3 pt3(1.0, 2.0, 3.0)
static const double tol
TEST(testBoundingConstraint, unary_basics_inactive1)
const Symbol key2('v', 2)
Vector evaluateError(const X1 &x1, const X2 &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
measurement functions and constraint definitions for simulated 2D robot


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:58