testFunctorizedFactor.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  * -------------------------------1-------------------------------------------
11  */
12 
20 #include <gtsam/nonlinear/FunctorizedFactor.h>
21 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
22 #include <gtsam/nonlinear/factorTesting.h>
23 #include <gtsam/inference/Symbol.h>
24 #include <gtsam/base/Testable.h>
25 #include <gtsam/base/TestableAssertions.h>
26 
27 #include <CppUnitLite/TestHarness.h>
28 
29 using namespace std;
30 using namespace gtsam;
31 
32 // Key for FunctorizedFactor
33 Key key = Symbol('X', 0);
34 
35 // Keys for FunctorizedFactor2
36 Key keyA = Symbol('A', 0);
37 Key keyx = Symbol('x', 0);
38 
39 auto model = noiseModel::Isotropic::Sigma(9, 1);
40 auto model2 = noiseModel::Isotropic::Sigma(3, 1);
41 
43 class MultiplyFunctor {
44  double m_;
45 
46  public:
47  MultiplyFunctor(double m) : m_(m) {}
48 
49  Matrix operator()(const Matrix &X,
50  OptionalJacobian<-1, -1> H = {}) const {
51  if (H) *H = m_ * Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
52  return m_ * X;
53  }
54 };
55 
57 class ProjectionFunctor {
58  public:
59  Vector operator()(const Matrix &A, const Vector &x,
60  OptionalJacobian<-1, -1> H1 = {},
61  OptionalJacobian<-1, -1> H2 = {}) const {
62  if (H1) {
63  H1->resize(x.size(), A.size());
64  *H1 << I_3x3, I_3x3, I_3x3;
65  }
66  if (H2) *H2 = A;
67  return A * x;
68  }
69 };
70 
71 /* ************************************************************************* */
72 // Test identity operation for FunctorizedFactor.
73 TEST(FunctorizedFactor, Identity) {
74  Matrix X = Matrix::Identity(3, 3), measurement = Matrix::Identity(3, 3);
75 
76  double multiplier = 1.0;
77  auto functor = MultiplyFunctor(multiplier);
78  auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model, functor);
79 
80  Vector error = factor.evaluateError(X);
81 
82  EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
83 }
84 
85 /* ************************************************************************* */
86 // Test FunctorizedFactor with multiplier value of 2.
87 TEST(FunctorizedFactor, Multiply2) {
88  double multiplier = 2.0;
89  Matrix X = Matrix::Identity(3, 3);
90  Matrix measurement = multiplier * Matrix::Identity(3, 3);
91 
92  auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model,
93  MultiplyFunctor(multiplier));
94 
95  Vector error = factor.evaluateError(X);
96 
97  EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
98 }
99 
100 /* ************************************************************************* */
101 // Test equality function for FunctorizedFactor.
102 TEST(FunctorizedFactor, Equality) {
103  Matrix measurement = Matrix::Identity(2, 2);
104 
105  double multiplier = 2.0;
106 
107  auto factor1 = MakeFunctorizedFactor<Matrix>(key, measurement, model,
108  MultiplyFunctor(multiplier));
109  auto factor2 = MakeFunctorizedFactor<Matrix>(key, measurement, model,
110  MultiplyFunctor(multiplier));
111 
112  EXPECT(factor1.equals(factor2));
113 }
114 
115 /* ************************************************************************* */
116 // Test Jacobians of FunctorizedFactor.
117 TEST(FunctorizedFactor, Jacobians) {
118  Matrix X = Matrix::Identity(3, 3);
119  Matrix actualH;
120 
121  double multiplier = 2.0;
122 
123  auto factor =
124  MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier));
125 
126  Values values;
127  values.insert<Matrix>(key, X);
128 
129  // Check Jacobians
130  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
131 }
132 
133 /* ************************************************************************* */
134 // Test print result of FunctorizedFactor.
135 TEST(FunctorizedFactor, Print) {
136  Matrix X = Matrix::Identity(2, 2);
137 
138  double multiplier = 2.0;
139 
140  auto factor =
141  MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier));
142 
143  string expected =
144  " keys = { X0 }\n"
145  " noise model: unit (9) \n"
146  "FunctorizedFactor(X0)\n"
147  " measurement: [\n"
148  " 1, 0;\n"
149  " 0, 1\n"
150  "]\n"
151  " noise model sigmas: 1 1 1 1 1 1 1 1 1\n";
152 
153  EXPECT(assert_print_equal(expected, factor));
154 }
155 
156 /* ************************************************************************* */
157 // Test FunctorizedFactor using a std::function type.
158 TEST(FunctorizedFactor, Functional) {
159  double multiplier = 2.0;
160  Matrix X = Matrix::Identity(3, 3);
161  Matrix measurement = multiplier * Matrix::Identity(3, 3);
162 
163  std::function<Matrix(Matrix, OptionalMatrixType)> functional =
164  MultiplyFunctor(multiplier);
165  auto factor =
166  MakeFunctorizedFactor<Matrix>(key, measurement, model, functional);
167 
168  Vector error = factor.evaluateError(X);
169 
170  EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
171 }
172 
173 /* ************************************************************************* */
174 // Test FunctorizedFactor with a lambda function.
175 TEST(FunctorizedFactor, Lambda) {
176  double multiplier = 2.0;
177  Matrix X = Matrix::Identity(3, 3);
178  Matrix measurement = multiplier * Matrix::Identity(3, 3);
179 
180  auto lambda = [multiplier](const Matrix &X,
181  OptionalJacobian<-1, -1> H = {}) {
182  if (H)
183  *H = multiplier *
184  Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
185  return multiplier * X;
186  };
187  // FunctorizedFactor<Matrix> factor(key, measurement, model, lambda);
188  auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model, lambda);
189 
190  Vector error = factor.evaluateError(X);
191 
192  EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
193 }
194 
195 /* ************************************************************************* */
196 // Test identity operation for FunctorizedFactor2.
197 TEST(FunctorizedFactor, Identity2) {
198  // x = Ax since A is I_3x3
199  Matrix A = Matrix::Identity(3, 3);
200  Vector x = Vector::Ones(3);
201 
202  auto functor = ProjectionFunctor();
203  auto factor =
204  MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, x, model2, functor);
205 
206  Vector error = factor.evaluateError(A, x);
207 
208  EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
209 }
210 
211 /* ************************************************************************* */
212 // Test Jacobians of FunctorizedFactor2.
213 TEST(FunctorizedFactor, Jacobians2) {
214  Matrix A = Matrix::Identity(3, 3);
215  Vector x = Vector::Ones(3);
216  Matrix actualH1, actualH2;
217 
218  auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, x, model2,
219  ProjectionFunctor());
220 
221  Values values;
222  values.insert<Matrix>(keyA, A);
223  values.insert<Vector>(keyx, x);
224 
225  // Check Jacobians
226  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
227 }
228 
229 /* ************************************************************************* */
230 // Test FunctorizedFactor2 using a std::function type.
231 TEST(FunctorizedFactor, Functional2) {
232  Matrix A = Matrix::Identity(3, 3);
233  Vector3 x(1, 2, 3);
234  Vector measurement = A * x;
235 
236  std::function<Matrix(Matrix, Matrix, OptionalMatrixType, OptionalMatrixType)>
237  functional = ProjectionFunctor();
238  auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, measurement,
239  model2, functional);
240 
241  Vector error = factor.evaluateError(A, x);
242 
243  EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
244 }
245 
246 /* ************************************************************************* */
247 // Test FunctorizedFactor2 with a lambda function.
248 TEST(FunctorizedFactor, Lambda2) {
249  Matrix A = Matrix::Identity(3, 3);
250  Vector3 x = Vector3(1, 2, 3);
251  Matrix measurement = A * x;
252 
253  auto lambda = [](const Matrix &A, const Vector &x,
254  OptionalJacobian<-1, -1> H1 = {},
255  OptionalJacobian<-1, -1> H2 = {}) {
256  if (H1) {
257  H1->resize(x.size(), A.size());
258  *H1 << I_3x3, I_3x3, I_3x3;
259  }
260  if (H2) *H2 = A;
261  return A * x;
262  };
263  // FunctorizedFactor<Matrix> factor(key, measurement, model, lambda);
264  auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, measurement,
265  model2, lambda);
266 
267  Vector error = factor.evaluateError(A, x);
268 
269  EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
270 }
271 
272 /* ************************************************************************* */
273 int main() {
274  TestResult tr;
275  return TestRegistry::runAllTests(tr);
276 }
277 /* ************************************************************************* */
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
TestableAssertions.h
Provides additional testing facilities for common data structures.
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:187
main
int main()
Definition: testFunctorizedFactor.cpp:273
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:194
Testable.h
Concept check for values that can be used in unit tests.
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::assert_print_equal
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
Definition: TestableAssertions.h:352
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
ProjectionFunctor::operator()
Vector operator()(const Matrix &A, const Vector &x, OptionalJacobian<-1, -1 > H1={}, OptionalJacobian<-1, -1 > H2={}) const
Definition: testFunctorizedFactor.cpp:59
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
leaf::values
leaf::MyValues values
std
Definition: BFloat16.h:88
factorTesting.h
Evaluate derivatives of a nonlinear factor numerically.
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:12
model
auto model
Definition: testFunctorizedFactor.cpp:39
measurement
static Point2 measurement(323.0, 240.0)
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
MultiplyFunctor::operator()
Matrix operator()(const Matrix &X, OptionalJacobian<-1, -1 > H={}) const
Definition: testFunctorizedFactor.cpp:49
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
model2
auto model2
Definition: testFunctorizedFactor.cpp:40
MultiplyFunctor::m_
double m_
simple multiplier
Definition: testFunctorizedFactor.cpp:44
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
ProjectionFunctor
Functor that performs Ax where A is a matrix and x is a vector.
Definition: testFunctorizedFactor.cpp:57
lambda
cout<< "The eigenvalues of A are:"<< endl<< ces.eigenvalues()<< endl;cout<< "The matrix of eigenvectors, V, is:"<< endl<< ces.eigenvectors()<< endl<< endl;complex< float > lambda
Definition: ComplexEigenSolver_compute.cpp:9
gtsam
traits
Definition: chartTesting.h:28
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
MultiplyFunctor
Functor that takes a matrix and multiplies every element by m.
Definition: testFunctorizedFactor.cpp:43
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
error
static double error
Definition: testRot3.cpp:37
TEST
TEST(FunctorizedFactor, Identity)
Definition: testFunctorizedFactor.cpp:73
X
#define X
Definition: icosphere.cpp:20
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
gtsam::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:62

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:08