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 
23 #include <gtsam/inference/Symbol.h>
24 #include <gtsam/base/Testable.h>
26 
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 
44  double m_;
45 
46  public:
47  MultiplyFunctor(double m) : m_(m) {}
48 
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 
58  public:
59  Vector operator()(const Matrix &A, const Vector &x,
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.
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.
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.
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,
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.
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 /* ************************************************************************* */
Matrix3f m
Provides additional testing facilities for common data structures.
int main()
Concept check for values that can be used in unit tests.
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
Matrix expected
Definition: testMatrix.cpp:971
Vector operator()(const Matrix &A, const Vector &x, OptionalJacobian<-1, -1 > H1={}, OptionalJacobian<-1, -1 > H2={}) const
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
leaf::MyValues values
Definition: BFloat16.h:88
Evaluate derivatives of a nonlinear factor numerically.
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
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
auto model
static Point2 measurement(323.0, 240.0)
Eigen::VectorXd Vector
Definition: Vector.h:38
Matrix operator()(const Matrix &X, OptionalJacobian<-1, -1 > H={}) const
#define EXPECT(condition)
Definition: Test.h:150
auto model2
double m_
simple multiplier
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Functor that performs Ax where A is a matrix and x is a vector.
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
traits
Definition: chartTesting.h:28
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
Functor that takes a matrix and multiplies every element by m.
void insert(Key j, const Value &val)
Definition: Values.cpp:155
static double error
Definition: testRot3.cpp:37
TEST(FunctorizedFactor, Identity)
#define X
Definition: icosphere.cpp:20
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
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
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