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 <CppUnitLite/TestHarness.h>
21 #include <gtsam/base/Testable.h>
22 #include <gtsam/base/TestableAssertions.h>
23 #include <gtsam/inference/Symbol.h>
24 #include <gtsam/nonlinear/FunctorizedFactor.h>
25 #include <gtsam/nonlinear/factorTesting.h>
26 
27 using namespace std;
28 using namespace gtsam;
29 
30 // Key for FunctorizedFactor
31 Key key = Symbol('X', 0);
32 
33 // Keys for FunctorizedFactor2
34 Key keyA = Symbol('A', 0);
35 Key keyx = Symbol('x', 0);
36 
37 auto model = noiseModel::Isotropic::Sigma(9, 1);
38 auto model2 = noiseModel::Isotropic::Sigma(3, 1);
39 
41 class MultiplyFunctor {
42  double m_;
43 
44  public:
45  MultiplyFunctor(double m) : m_(m) {}
46 
47  Matrix operator()(const Matrix &X,
48  OptionalJacobian<-1, -1> H = boost::none) const {
49  if (H) *H = m_ * Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
50  return m_ * X;
51  }
52 };
53 
55 class ProjectionFunctor {
56  public:
57  Vector operator()(const Matrix &A, const Vector &x,
58  OptionalJacobian<-1, -1> H1 = boost::none,
59  OptionalJacobian<-1, -1> H2 = boost::none) const {
60  if (H1) {
61  H1->resize(x.size(), A.size());
62  *H1 << I_3x3, I_3x3, I_3x3;
63  }
64  if (H2) *H2 = A;
65  return A * x;
66  }
67 };
68 
69 /* ************************************************************************* */
70 // Test identity operation for FunctorizedFactor.
71 TEST(FunctorizedFactor, Identity) {
72  Matrix X = Matrix::Identity(3, 3), measurement = Matrix::Identity(3, 3);
73 
74  double multiplier = 1.0;
75  auto functor = MultiplyFunctor(multiplier);
76  auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model, functor);
77 
78  Vector error = factor.evaluateError(X);
79 
80  EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
81 }
82 
83 /* ************************************************************************* */
84 // Test FunctorizedFactor with multiplier value of 2.
85 TEST(FunctorizedFactor, Multiply2) {
86  double multiplier = 2.0;
87  Matrix X = Matrix::Identity(3, 3);
88  Matrix measurement = multiplier * Matrix::Identity(3, 3);
89 
90  auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model,
91  MultiplyFunctor(multiplier));
92 
93  Vector error = factor.evaluateError(X);
94 
95  EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
96 }
97 
98 /* ************************************************************************* */
99 // Test equality function for FunctorizedFactor.
100 TEST(FunctorizedFactor, Equality) {
101  Matrix measurement = Matrix::Identity(2, 2);
102 
103  double multiplier = 2.0;
104 
105  auto factor1 = MakeFunctorizedFactor<Matrix>(key, measurement, model,
106  MultiplyFunctor(multiplier));
107  auto factor2 = MakeFunctorizedFactor<Matrix>(key, measurement, model,
108  MultiplyFunctor(multiplier));
109 
110  EXPECT(factor1.equals(factor2));
111 }
112 
113 /* ************************************************************************* */
114 // Test Jacobians of FunctorizedFactor.
115 TEST(FunctorizedFactor, Jacobians) {
116  Matrix X = Matrix::Identity(3, 3);
117  Matrix actualH;
118 
119  double multiplier = 2.0;
120 
121  auto factor =
122  MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier));
123 
124  Values values;
125  values.insert<Matrix>(key, X);
126 
127  // Check Jacobians
128  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
129 }
130 
131 /* ************************************************************************* */
132 // Test print result of FunctorizedFactor.
133 TEST(FunctorizedFactor, Print) {
134  Matrix X = Matrix::Identity(2, 2);
135 
136  double multiplier = 2.0;
137 
138  auto factor =
139  MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier));
140 
141  string expected =
142  " keys = { X0 }\n"
143  " noise model: unit (9) \n"
144  "FunctorizedFactor(X0)\n"
145  " measurement: [\n"
146  " 1, 0;\n"
147  " 0, 1\n"
148  "]\n"
149  " noise model sigmas: 1 1 1 1 1 1 1 1 1\n";
150 
151  EXPECT(assert_print_equal(expected, factor));
152 }
153 
154 /* ************************************************************************* */
155 // Test FunctorizedFactor using a std::function type.
156 TEST(FunctorizedFactor, Functional) {
157  double multiplier = 2.0;
158  Matrix X = Matrix::Identity(3, 3);
159  Matrix measurement = multiplier * Matrix::Identity(3, 3);
160 
161  std::function<Matrix(Matrix, boost::optional<Matrix &>)> functional =
162  MultiplyFunctor(multiplier);
163  auto factor =
164  MakeFunctorizedFactor<Matrix>(key, measurement, model, functional);
165 
166  Vector error = factor.evaluateError(X);
167 
168  EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
169 }
170 
171 /* ************************************************************************* */
172 // Test FunctorizedFactor with a lambda function.
173 TEST(FunctorizedFactor, Lambda) {
174  double multiplier = 2.0;
175  Matrix X = Matrix::Identity(3, 3);
176  Matrix measurement = multiplier * Matrix::Identity(3, 3);
177 
178  auto lambda = [multiplier](const Matrix &X,
179  OptionalJacobian<-1, -1> H = boost::none) {
180  if (H)
181  *H = multiplier *
182  Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
183  return multiplier * X;
184  };
185  // FunctorizedFactor<Matrix> factor(key, measurement, model, lambda);
186  auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model, lambda);
187 
188  Vector error = factor.evaluateError(X);
189 
190  EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
191 }
192 
193 /* ************************************************************************* */
194 // Test identity operation for FunctorizedFactor2.
195 TEST(FunctorizedFactor, Identity2) {
196  // x = Ax since A is I_3x3
197  Matrix A = Matrix::Identity(3, 3);
198  Vector x = Vector::Ones(3);
199 
200  auto functor = ProjectionFunctor();
201  auto factor =
202  MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, x, model2, functor);
203 
204  Vector error = factor.evaluateError(A, x);
205 
206  EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
207 }
208 
209 /* ************************************************************************* */
210 // Test Jacobians of FunctorizedFactor2.
211 TEST(FunctorizedFactor, Jacobians2) {
212  Matrix A = Matrix::Identity(3, 3);
213  Vector x = Vector::Ones(3);
214  Matrix actualH1, actualH2;
215 
216  auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, x, model2,
217  ProjectionFunctor());
218 
219  Values values;
220  values.insert<Matrix>(keyA, A);
221  values.insert<Vector>(keyx, x);
222 
223  // Check Jacobians
224  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
225 }
226 
227 /* ************************************************************************* */
228 // Test FunctorizedFactor2 using a std::function type.
229 TEST(FunctorizedFactor, Functional2) {
230  Matrix A = Matrix::Identity(3, 3);
231  Vector3 x(1, 2, 3);
232  Vector measurement = A * x;
233 
234  std::function<Matrix(Matrix, Matrix, boost::optional<Matrix &>,
235  boost::optional<Matrix &>)>
236  functional = ProjectionFunctor();
237  auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, measurement,
238  model2, functional);
239 
240  Vector error = factor.evaluateError(A, x);
241 
242  EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
243 }
244 
245 /* ************************************************************************* */
246 // Test FunctorizedFactor2 with a lambda function.
247 TEST(FunctorizedFactor, Lambda2) {
248  Matrix A = Matrix::Identity(3, 3);
249  Vector3 x = Vector3(1, 2, 3);
250  Matrix measurement = A * x;
251 
252  auto lambda = [](const Matrix &A, const Vector &x,
253  OptionalJacobian<-1, -1> H1 = boost::none,
254  OptionalJacobian<-1, -1> H2 = boost::none) {
255  if (H1) {
256  H1->resize(x.size(), A.size());
257  *H1 << I_3x3, I_3x3, I_3x3;
258  }
259  if (H2) *H2 = A;
260  return A * x;
261  };
262  // FunctorizedFactor<Matrix> factor(key, measurement, model, lambda);
263  auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, measurement, model2, lambda);
264 
265  Vector error = factor.evaluateError(A, x);
266 
267  EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
268 }
269 
270 /* ************************************************************************* */
271 int main() {
272  TestResult tr;
273  return TestRegistry::runAllTests(tr);
274 }
275 /* ************************************************************************* */
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
TestableAssertions.h
Provides additional testing facilities for common data structures.
main
int main()
Definition: testFunctorizedFactor.cpp:271
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::JacobianFactor::equals
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
Definition: JacobianFactor.cpp:439
gtsam::assert_print_equal
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
Definition: TestableAssertions.h:379
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:140
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
leaf::values
leaf::MyValues values
measurement
double measurement(10.0)
std
Definition: Half.h:150
factorTesting.h
Evaluate derivatives of a nonlinear factor numerically.
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:37
ProjectionFunctor::operator()
Vector operator()(const Matrix &A, const Vector &x, OptionalJacobian<-1,-1 > H1=boost::none, OptionalJacobian<-1,-1 > H2=boost::none) const
Definition: testFunctorizedFactor.cpp:57
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
model2
auto model2
Definition: testFunctorizedFactor.cpp:38
MultiplyFunctor::m_
double m_
simple multiplier
Definition: testFunctorizedFactor.cpp:42
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:55
MultiplyFunctor::operator()
Matrix operator()(const Matrix &X, OptionalJacobian<-1,-1 > H=boost::none) const
Definition: testFunctorizedFactor.cpp:47
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
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
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:110
MultiplyFunctor
Functor that takes a matrix and multiplies every element by m.
Definition: testFunctorizedFactor.cpp:41
simple_graph::factor2
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2))
error
static double error
Definition: testRot3.cpp:39
TEST
TEST(FunctorizedFactor, Identity)
Definition: testFunctorizedFactor.cpp:71
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:61
gtsam::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:59
simple_graph::factor1
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1))

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:46:35