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 
18 #include <tests/simulated2DConstraints.h>
19 #include <gtsam/inference/Symbol.h>
20 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
21 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
22 
23 #include <CppUnitLite/TestHarness.h>
24 
25 namespace iq2D = simulated2D::inequality_constraints;
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
36 gtsam::Key key = 1;
37 double mu = 10.0;
38 // greater than
39 iq2D::PoseXInequality constraint1(key, 1.0, true, mu);
40 iq2D::PoseYInequality constraint2(key, 2.0, true, mu);
41 
42 // less than
43 iq2D::PoseXInequality constraint3(key, 1.0, false, mu);
44 iq2D::PoseYInequality constraint4(key, 2.0, false, mu);
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));
53  EXPECT_DOUBLES_EQUAL(1.0, constraint1.threshold(), tol);
54  EXPECT_DOUBLES_EQUAL(2.0, constraint2.threshold(), tol);
55  EXPECT(constraint1.isGreaterThan());
56  EXPECT(constraint2.isGreaterThan());
57  EXPECT(assert_equal(I_1x1, constraint1.evaluateError(pt1), tol));
58  EXPECT(assert_equal(I_1x1, constraint2.evaluateError(pt1), tol));
59  EXPECT(assert_equal(Z_1x1, constraint1.unwhitenedError(config), tol));
60  EXPECT(assert_equal(Z_1x1, constraint2.unwhitenedError(config), tol));
61  EXPECT_DOUBLES_EQUAL(0.0, constraint1.error(config), tol);
62  EXPECT_DOUBLES_EQUAL(0.0, constraint2.error(config), tol);
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));
72  EXPECT_DOUBLES_EQUAL(1.0, constraint3.threshold(), tol);
73  EXPECT_DOUBLES_EQUAL(2.0, constraint4.threshold(), tol);
74  EXPECT(!constraint3.isGreaterThan());
75  EXPECT(!constraint4.isGreaterThan());
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));
78  EXPECT(assert_equal(Z_1x1, constraint3.unwhitenedError(config), tol));
79  EXPECT(assert_equal(Z_1x1, constraint4.unwhitenedError(config), tol));
80  EXPECT_DOUBLES_EQUAL(0.0, constraint3.error(config), tol);
81  EXPECT_DOUBLES_EQUAL(0.0, constraint4.error(config), 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 
145  NonlinearFactorGraph graph;
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();
155  Values expected;
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 
167  NonlinearFactorGraph graph;
168  graph.emplace_shared<iq2D::PoseXInequality>(key, 1.0, false);
169  graph.emplace_shared<iq2D::PoseYInequality>(key, 2.0, false);
170  graph.emplace_shared<simulated2D::Prior>(start_pt, soft_model2, key);
171 
172  Values initValues;
173  initValues.insert(key, start_pt);
174 
175  Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
176  Values expected;
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 
226  NonlinearFactorGraph graph;
227  graph.emplace_shared<simulated2D::equality_constraints::UnaryEqualityConstraint>(pt1, x1);
228  graph.emplace_shared<simulated2D::Prior>(pt2_init, soft_model2_alt, x2);
229  graph.emplace_shared<iq2D::PoseMaxDistConstraint>(x1, x2, 2.0);
230 
231  Values initial_state;
232  initial_state.insert(x1, pt1);
233  initial_state.insert(x2, pt2_init);
234 
235  Values expected;
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 
252  NonlinearFactorGraph graph;
253  graph.emplace_shared<simulated2D::equality_constraints::UnaryEqualityConstraint>(x1_pt, x1);
254  graph.emplace_shared<simulated2D::Odometry>(odo, soft_model2_alt, x1, x2);
255  graph.emplace_shared<iq2D::LandmarkAvoid>(x2, l1, radius);
256  graph.emplace_shared<simulated2D::equality_constraints::UnaryEqualityPointConstraint>(l1_pt, l1);
257  graph.emplace_shared<simulated2D::Odometry>(odo, soft_model2_alt, x2, x3);
258  graph.emplace_shared<simulated2D::equality_constraints::UnaryEqualityConstraint>(x3_pt, x3);
259 
260  Values init, expected;
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 
key1
const Symbol key1('v', 1)
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::BoundingConstraint2::threshold
double threshold() const
Definition: BoundingConstraint.h:125
gtsam::NoiseModelFactor::dim
size_t dim() const override
Definition: NonlinearFactor.h:240
gtsam::BoundingConstraint2::evaluateError
Vector evaluateError(const X1 &x1, const X2 &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
Definition: BoundingConstraint.h:143
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
simulated2D::GenericOdometry
Definition: simulated2D.h:178
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Definition: NonlinearFactor.cpp:136
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
mu
double mu
Definition: testBoundingConstraint.cpp:37
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
constraint1
iq2D::PoseXInequality constraint1(key, 1.0, true, mu)
gtsam::BoundingConstraint1::isGreaterThan
bool isGreaterThan() const
Definition: BoundingConstraint.h:53
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::GaussianFactor
Definition: GaussianFactor.h:38
gtsam::Values::update
void update(Key j, const Value &val)
Definition: Values.cpp:169
main
int main()
Definition: testBoundingConstraint.cpp:274
gtsam::NoiseModelFactorN::unwhitenedError
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
Definition: NonlinearFactor.h:606
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
gtsam::BoundingConstraint1::threshold
double threshold() const
Definition: BoundingConstraint.h:52
soft_model2_alt
SharedDiagonal soft_model2_alt
Definition: testBoundingConstraint.cpp:32
gtsam::NoiseModelFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Definition: NonlinearFactor.cpp:150
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
gtsam::NonlinearEquality1
Definition: NonlinearEquality.h:209
gtsam::BoundingConstraint2::isGreaterThan
bool isGreaterThan() const
Definition: BoundingConstraint.h:126
simulated2D::inequality_constraints::MaxDistanceConstraint
Definition: simulated2DConstraints.h:124
gtsam::BoundingConstraint2::active
bool active(const Values &c) const override
Definition: BoundingConstraint.h:137
key2
const Symbol key2('v', 2)
pt3
static Point3 pt3(1.0, 2.0, 3.0)
x1
Pose3 x1
Definition: testPose3.cpp:663
soft_model2
SharedDiagonal soft_model2
Definition: testBoundingConstraint.cpp:31
simulated2D::inequality_constraints::ScalarCoordConstraint1
Definition: simulated2DConstraints.h:55
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
simulated2D::inequality_constraints
Definition: simulated2DConstraints.h:47
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
Symbol.h
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::BoundingConstraint1::evaluateError
Vector evaluateError(const X &x, OptionalMatrixType H) const override
Definition: BoundingConstraint.h:70
tol
static const double tol
Definition: testBoundingConstraint.cpp:29
init
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:1912
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
TestResult
Definition: TestResult.h:26
constraint4
iq2D::PoseYInequality constraint4(key, 2.0, false, mu)
hard_model1
SharedDiagonal hard_model1
Definition: testBoundingConstraint.cpp:33
key
gtsam::Key key
Definition: testBoundingConstraint.cpp:36
constraint3
iq2D::PoseXInequality constraint3(key, 1.0, false, mu)
gtsam::BoundingConstraint1::active
bool active(const Values &c) const override
Definition: BoundingConstraint.h:64
simulated2D::GenericPrior
Definition: simulated2D.h:129
gtsam
traits
Definition: chartTesting.h:28
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:24
std
Definition: BFloat16.h:88
simulated2D::odo
Point2 odo(const Point2 &x1, const Point2 &x2)
odometry between two poses
Definition: simulated2D.h:99
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
init
Definition: TutorialInplaceLU.cpp:2
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
constraint2
iq2D::PoseYInequality constraint2(key, 2.0, true, mu)
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
simulated2DConstraints.h
measurement functions and constraint definitions for simulated 2D robot
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
TEST
TEST(testBoundingConstraint, unary_basics_inactive1)
Definition: testBoundingConstraint.cpp:47
simulated2D::inequality_constraints::MinDistanceConstraint
Definition: simulated2DConstraints.h:172
gtsam::Symbol
Definition: inference/Symbol.h:37

gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:09:05