testNonlinearEqualityConstraint.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 <CppUnitLite/Test.h>
20 #include <CppUnitLite/TestHarness.h>
21 #include <gtsam/base/Testable.h>
22 #include <gtsam/base/TestableAssertions.h>
23 #include <gtsam/base/numericalDerivative.h>
24 #include <gtsam/constrained/NonlinearEqualityConstraint.h>
25 #include <gtsam/nonlinear/factorTesting.h>
26 #include <gtsam/slam/BetweenFactor.h>
27 
28 #include "constrainedExample.h"
29 
30 using namespace gtsam;
31 using constrained_example::pow;
32 using constrained_example::x1, constrained_example::x2;
33 using constrained_example::x1_key, constrained_example::x2_key;
34 
35 // Test methods of DoubleExpressionEquality.
36 TEST(ExpressionEqualityConstraint, double) {
37  // create constraint from double expression
38  // g(x1, x2) = x1 + x1^3 + x2 + x2^2, from Vanderbergh slides
39  Vector sigmas = Vector1(0.1);
40  auto g = x1 + pow(x1, 3) + x2 + pow(x2, 2);
41  auto constraint = ExpressionEqualityConstraint<double>(g, 0.0, sigmas);
42 
43  EXPECT(constraint.noiseModel()->isConstrained());
44  EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas()));
45 
46  // Create 2 sets of values for testing.
47  Values values1, values2;
48  values1.insert(x1_key, 0.0);
49  values1.insert(x2_key, 0.0);
50  values2.insert(x1_key, 1.0);
51  values2.insert(x2_key, 1.0);
52 
53  // Check that values1 are feasible.
54  EXPECT(constraint.feasible(values1));
55 
56  // Check that violation evaluates as 0 at values1.
57  EXPECT(assert_equal(Vector::Zero(1), constraint.unwhitenedError(values1)));
58  EXPECT(assert_equal(Vector::Zero(1), constraint.whitenedError(values1)));
59  EXPECT(assert_equal(0.0, constraint.error(values1)));
60 
61  // Check that values2 are indeed deemed infeasible.
62  EXPECT(!constraint.feasible(values2));
63 
64  // Check constraint violation is indeed g(x) at values2.
65  EXPECT(assert_equal(Vector::Constant(1, 4.0), constraint.unwhitenedError(values2)));
66  EXPECT(assert_equal(Vector::Constant(1, 40), constraint.whitenedError(values2)));
67  EXPECT(assert_equal(800, constraint.error(values2)));
68 
69  // Check dimension is 1 for scalar g.
70  EXPECT(constraint.dim() == 1);
71 
72  // Check keys include x1, x2.
73  EXPECT(constraint.keys().size() == 2);
74  EXPECT(x1_key == *constraint.keys().begin());
75  EXPECT(x2_key == *constraint.keys().rbegin());
76 
77  // Generate factor representing the term in merit function.
78  double mu = 4;
79  auto merit_factor = constraint.penaltyFactor(mu);
80 
81  // Check that noise model sigma == sigmas/sqrt(mu).
82  auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu));
83  EXPECT(expected_noise->equals(*merit_factor->noiseModel()));
84 
85  // Check that error is equal to 0.5*mu * (g(x)+bias)^2/sigmas^2.
86  double expected_error1 = 0; // 0.5 * 4 * ||0||_(0.1^2)^2
87  EXPECT(assert_equal(expected_error1, merit_factor->error(values1)));
88  double expected_error2 = 3200; // 0.5 * 4 * ||4||_(0.1^2)^2
89  EXPECT(assert_equal(expected_error2, merit_factor->error(values2)));
90 
91  // Check jacobian computation is correct.
92  EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5);
93  EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5);
94 }
95 
96 // Test methods of VectorExpressionEquality.
97 TEST(ExpressionEqualityConstraint, Vector2) {
98  // g(v1, v2) = v1 + v2, our own example.
99  Vector2_ x1_vec_expr(x1_key);
100  Vector2_ x2_vec_expr(x2_key);
101  auto g = x1_vec_expr + x2_vec_expr;
102  auto sigmas = Vector2(0.1, 0.5);
103  auto constraint = ExpressionEqualityConstraint<Vector2>(g, Vector2::Zero(), sigmas);
104 
105  EXPECT(constraint.noiseModel()->isConstrained());
106  EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas()));
107 
108  // Create 2 sets of values for testing.
109  Values values1, values2;
110  values1.insert(x1_key, Vector2(1, 1));
111  values1.insert(x2_key, Vector2(-1, -1));
112  values2.insert(x1_key, Vector2(1, 1));
113  values2.insert(x2_key, Vector2(1, 1));
114 
115  // Check that values1 are feasible.
116  EXPECT(constraint.feasible(values1));
117 
118  // Check that violation evaluates as 0 at values1.
119  auto expected_violation1 = (Vector(2) << 0, 0).finished();
120  EXPECT(assert_equal(expected_violation1, constraint.unwhitenedError(values1)));
121  auto expected_scaled_violation1 = (Vector(2) << 0, 0).finished();
122  EXPECT(assert_equal(expected_scaled_violation1, constraint.whitenedError(values1)));
123 
124  // Check that values2 are indeed deemed infeasible.
125  EXPECT(!constraint.feasible(values2));
126 
127  // Check constraint violation is indeed g(x) at values2.
128  auto expected_violation2 = (Vector(2) << 2, 2).finished();
129  EXPECT(assert_equal(expected_violation2, constraint.unwhitenedError(values2)));
130 
131  // Check scaled violation is indeed g(x)/sigmas at values2.
132  auto expected_scaled_violation2 = (Vector(2) << 20, 4).finished();
133  EXPECT(assert_equal(expected_scaled_violation2, constraint.whitenedError(values2)));
134 
135  // Check dim is the dimension of the vector.
136  EXPECT(constraint.dim() == 2);
137 
138  // Generate factor representing the term in merit function.
139  double mu = 4;
140  auto merit_factor = constraint.penaltyFactor(mu);
141 
142  // Check that noise model sigma == sigmas/sqrt(mu).
143  auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu));
144  EXPECT(expected_noise->equals(*merit_factor->noiseModel()));
145 
146  // Check that error is equal to 0.5*mu*||g(x)+bias)||^2_Diag(sigmas^2).
147  double expected_error1 = 0; // 0.5 * 4 * ||[1, 0.5]||_([0.1,0.5]^2)^2
148  EXPECT(assert_equal(expected_error1, merit_factor->error(values1)));
149  double expected_error2 = 832; // 0.5 * 4 * ||[2, 2]||_([0.1,0.5]^2)^2
150  EXPECT(assert_equal(expected_error2, merit_factor->error(values2)));
151 
152  // Check jacobian computation is correct.
153  EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5);
154  EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5);
155 }
156 
157 // Test methods of FactorZeroErrorConstraint.
158 TEST(ZeroCostConstraint, BetweenFactor) {
159  Key x1_key = 1;
160  Key x2_key = 2;
161  Vector sigmas = Vector2(0.5, 0.1);
162  auto noise = noiseModel::Diagonal::Sigmas(sigmas);
163 
164  auto factor = std::make_shared<BetweenFactor<Vector2>>(x1_key, x2_key, Vector2(1, 1), noise);
165  auto constraint = ZeroCostConstraint(factor);
166 
167  EXPECT(constraint.noiseModel()->isConstrained());
168  EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas()));
169 
170  // Create 2 sets of values for testing.
171  Values values1, values2;
172  values1.insert(x1_key, Vector2(1, 1));
173  values1.insert(x2_key, Vector2(2, 2));
174  values2.insert(x1_key, Vector2(0, 0));
175  values2.insert(x2_key, Vector2(2, 3));
176 
177  // Check that values1 are feasible.
178  EXPECT(constraint.feasible(values1));
179 
180  // Check that violation evaluates as 0 at values1.
181  auto expected_violation1 = (Vector(2) << 0, 0).finished();
182  EXPECT(assert_equal(expected_violation1, constraint.unwhitenedError(values1)));
183  auto expected_scaled_violation1 = (Vector(2) << 0, 0).finished();
184  EXPECT(assert_equal(expected_scaled_violation1, constraint.whitenedError(values1)));
185 
186  // Check that values2 are indeed deemed infeasible.
187  EXPECT(!constraint.feasible(values2));
188 
189  // Check constraint violation is indeed g(x) at values2.
190  auto expected_violation2 = (Vector(2) << 1, 2).finished();
191  EXPECT(assert_equal(expected_violation2, constraint.unwhitenedError(values2)));
192 
193  // Check scaled violation is indeed g(x)/sigmas at values2.
194  auto expected_scaled_violation2 = (Vector(2) << 2, 20).finished();
195  EXPECT(assert_equal(expected_scaled_violation2, constraint.whitenedError(values2)));
196 
197  // Check dim is the dimension of the vector.
198  EXPECT(constraint.dim() == 2);
199 
200  // Generate factor representing the term in merit function.
201  double mu = 4;
202  auto merit_factor = constraint.penaltyFactor(mu);
203 
204  // Check that noise model sigma == sigmas/sqrt(mu).
205  auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu));
206  EXPECT(expected_noise->equals(*merit_factor->noiseModel()));
207 
208  // Check that error is equal to 0.5*mu*||g(x)+bias)||^2_Diag(sigmas^2).
209  double expected_error1 = 0; // 0.5 * 4 * ||[0, 0]||_([0.5,0.1]^2)^2
210  EXPECT(assert_equal(expected_error1, merit_factor->error(values1)));
211  double expected_error2 = 808; // 0.5 * 4 * ||[1, 2]||_([0.5,0.1]^2)^2
212  EXPECT(assert_equal(expected_error2, merit_factor->error(values2)));
213 
214  // Check jacobian computation is correct.
215  EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5);
216  EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5);
217 }
218 
219 TEST(NonlinearEqualityConstraints, Container) {
220  NonlinearEqualityConstraints constraints;
221 
222  Vector sigmas1 = Vector1(10);
223  auto g1 = x1 + pow(x1, 3) + x2 + pow(x2, 2);
224  Vector2_ x1_vec_expr(x1_key);
225  Vector2_ x2_vec_expr(x2_key);
226  auto g2 = x1_vec_expr + x2_vec_expr;
227  Vector sigmas2 = Vector2(0.1, 0.5);
228 
229  constraints.emplace_shared<ExpressionEqualityConstraint<double>>(g1, 0.0, sigmas1);
230  constraints.emplace_shared<ExpressionEqualityConstraint<Vector2>>(g2, Vector2::Zero(), sigmas2);
231 
232  // Check size.
233  EXPECT_LONGS_EQUAL(2, constraints.size());
234 
235  // Check dimension.
236  EXPECT_LONGS_EQUAL(3, constraints.dim());
237 
238  // Check keys.
239  KeySet expected_keys;
240  expected_keys.insert(x1_key);
241  expected_keys.insert(x2_key);
242  EXPECT(assert_container_equality(expected_keys, constraints.keys()));
243 
244  // Check VariableIndex.
245  VariableIndex vi(constraints);
246  FactorIndices expected_vi_x1{0, 1};
247  FactorIndices expected_vi_x2{0, 1};
248  EXPECT(assert_container_equality(expected_vi_x1, vi[x1_key]));
249  EXPECT(assert_container_equality(expected_vi_x2, vi[x2_key]));
250 
251  // Check constraint violation.
252 }
253 
254 TEST(NonlinearEqualityConstraints, FromCostGraph) {}
255 
256 int main() {
257  TestResult tr;
258  return TestRegistry::runAllTests(tr);
259 }
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
constrained_example::x1_key
Symbol x1_key('x', 1)
Keys for creating expressions.
gtsam::Vector1
Eigen::Matrix< double, 1, 1 > Vector1
Definition: Vector.h:42
constrained_example::x2_key
Symbol x2_key('x', 2)
EXPECT_CORRECT_FACTOR_JACOBIANS
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
Definition: factorTesting.h:114
constrained_example::x1
Double_ x1(x1_key)
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:43
test_constructor::sigmas
Vector1 sigmas
Definition: testHybridNonlinearFactor.cpp:52
TestHarness.h
g1
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
mu
double mu
Definition: testBoundingConstraint.cpp:37
gtsam::NonlinearEqualityConstraints
Container of NonlinearEqualityConstraint.
Definition: NonlinearEqualityConstraint.h:147
gtsam::ZeroCostConstraint
Definition: NonlinearEqualityConstraint.h:106
NonlinearEqualityConstraint.h
Nonlinear equality constraints in constrained optimization.
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:283
gtsam::FastSet< Key >
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
TestableAssertions.h
Provides additional testing facilities for common data structures.
pruning_fixture::factor
DecisionTreeFactor factor(D &C &B &A, "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0")
gtsam::Expression
Definition: Expression.h:49
numericalDerivative.h
Some functions to compute numerical derivatives.
BetweenFactor.h
x1
Pose3 x1
Definition: testPose3.cpp:692
gtsam::NonlinearEqualityConstraints::dim
size_t dim() const
Definition: NonlinearEqualityConstraint.cpp:49
g2
Pose3 g2(g1.expmap(h *V1_g1))
constrained_example::x2
Double_ x2(x2_key)
constrainedExample.h
Simple constrained optimization scenarios.
gtsam::FactorGraph::keys
KeySet keys() const
Definition: FactorGraph-inst.h:85
constrained_example::pow
Double_ pow(const Double_ &x, const double &c)
Pow function.
Definition: constrainedExample.h:56
factorTesting.h
Evaluate derivatives of a nonlinear factor numerically.
Vector2
Definition: test_operator_overloading.cpp:18
ceres::pow
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
gtsam::VariableIndex
Definition: VariableIndex.h:41
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
TestResult
Definition: TestResult.h:26
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
Test.h
gtsam
traits
Definition: ABC.h:17
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
gtsam::ExpressionEqualityConstraint
Definition: NonlinearEqualityConstraint.h:55
gtsam::Values
Definition: Values.h:65
gtsam::assert_container_equality
bool assert_container_equality(const std::map< size_t, V2 > &expected, const std::map< size_t, V2 > &actual)
Definition: TestableAssertions.h:238
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:156
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
main
int main()
Definition: testNonlinearEqualityConstraint.cpp:256
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:37
gtsam::BetweenFactor
Definition: BetweenFactor.h:40

gtsam
Author(s):
autogenerated on Wed May 28 2025 03:06:58