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 
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;
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 
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 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
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:74
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
model2
auto model2
Definition: testFunctorizedFactor.cpp:40
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:203
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
MultiplyFunctor::m_
double m_
simple multiplier
Definition: testFunctorizedFactor.cpp:44
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
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_print_equal
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
Definition: TestableAssertions.h:352
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::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
X
#define X
Definition: icosphere.cpp:20
MultiplyFunctor::MultiplyFunctor
MultiplyFunctor(double m)
Definition: testFunctorizedFactor.cpp:47
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
TEST
TEST(FunctorizedFactor, Identity)
Definition: testFunctorizedFactor.cpp:73
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
TestableAssertions.h
Provides additional testing facilities for common data structures.
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
key
Key key
Definition: testFunctorizedFactor.cpp:33
main
int main()
Definition: testFunctorizedFactor.cpp:273
keyx
Key keyx
Definition: testFunctorizedFactor.cpp:37
MultiplyFunctor
Functor that takes a matrix and multiplies every element by m.
Definition: testFunctorizedFactor.cpp:43
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
factorTesting.h
Evaluate derivatives of a nonlinear factor numerically.
Symbol.h
model
auto model
Definition: testFunctorizedFactor.cpp:39
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
MultiplyFunctor::operator()
Matrix operator()(const Matrix &X, OptionalJacobian<-1, -1 > H={}) const
Definition: testFunctorizedFactor.cpp:49
lambda
static double lambda[]
Definition: jv.c:524
TestResult
Definition: TestResult.h:26
gtsam::FunctorizedFactor
Definition: FunctorizedFactor.h:59
gtsam
traits
Definition: SFMdata.h:40
error
static double error
Definition: testRot3.cpp:37
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
ProjectionFunctor
Functor that performs Ax where A is a matrix and x is a vector.
Definition: testFunctorizedFactor.cpp:57
keyA
Key keyA
Definition: testFunctorizedFactor.cpp:36
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
gtsam::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:65
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
FunctorizedFactor.h
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:196
measurement
static Point2 measurement(323.0, 240.0)
gtsam::Symbol
Definition: inference/Symbol.h:37


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:57