testRegularImplicitSchurFactor.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 <gtsam/slam/JacobianFactorQ.h>
20 #include <gtsam/slam/JacobianFactorQR.h>
21 #include <gtsam/slam/RegularImplicitSchurFactor.h>
22 #include <gtsam/geometry/CalibratedCamera.h>
23 #include <gtsam/geometry/Point2.h>
24 
25 #include <gtsam/linear/VectorValues.h>
26 #include <gtsam/linear/NoiseModel.h>
27 #include <gtsam/linear/GaussianFactor.h>
28 #include <gtsam/base/timing.h>
29 
30 #include <boost/assign/list_of.hpp>
31 #include <boost/assign/std/vector.hpp>
32 #include <boost/range/iterator_range.hpp>
33 #include <boost/range/adaptor/map.hpp>
34 #include <CppUnitLite/TestHarness.h>
35 
36 using namespace std;
37 using namespace boost::assign;
38 using namespace gtsam;
39 
40 // F
41 const Matrix26 F0 = Matrix26::Ones();
42 const Matrix26 F1 = 2 * Matrix26::Ones();
43 const Matrix26 F3 = 3 * Matrix26::Ones();
44 const vector<Matrix26, Eigen::aligned_allocator<Matrix26> > FBlocks {F0, F1, F3};
45 const KeyVector keys {0, 1, 3};
46 // RHS and sigmas
47 const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished();
48 
49 //*************************************************************************************
50 TEST( regularImplicitSchurFactor, creation ) {
51  // Matrix E = Matrix::Ones(6,3);
52  Matrix E = Matrix::Zero(6, 3);
53  E.block<2,2>(0, 0) = I_2x2;
54  E.block<2,3>(2, 0) = 2 * Matrix::Ones(2, 3);
55  Matrix3 P = (E.transpose() * E).inverse();
56  RegularImplicitSchurFactor<CalibratedCamera> expected(keys, FBlocks, E, P, b);
57  Matrix expectedP = expected.getPointCovariance();
58  EXPECT(assert_equal(expectedP, P));
59 }
60 
61 /* ************************************************************************* */
62 TEST( regularImplicitSchurFactor, addHessianMultiply ) {
63 
64  Matrix E = Matrix::Zero(6, 3);
65  E.block<2,2>(0, 0) = I_2x2;
66  E.block<2,3>(2, 0) = 2 * Matrix::Ones(2, 3);
67  E.block<2,2>(4, 1) = I_2x2;
68  Matrix3 P = (E.transpose() * E).inverse();
69 
70  double alpha = 0.5;
71  VectorValues xvalues = map_list_of //
72  (0, Vector::Constant(6, 2))//
73  (1, Vector::Constant(6, 4))//
74  (2, Vector::Constant(6, 0))// distractor
75  (3, Vector::Constant(6, 8));
76 
77  VectorValues yExpected = map_list_of//
78  (0, Vector::Constant(6, 27))//
79  (1, Vector::Constant(6, -40))//
80  (2, Vector::Constant(6, 0))// distractor
81  (3, Vector::Constant(6, 279));
82 
83  // Create full F
84  size_t M=4, m = 3, d = 6;
85  Matrix F(2 * m, d * M);
86  F << F0, Matrix::Zero(2, d * 3), Matrix::Zero(2, d), F1, Matrix::Zero(2, d*2), Matrix::Zero(2, d * 3), F3;
87 
88  // Calculate expected result F'*alpha*(I - E*P*E')*F*x
89  KeyVector keys2{0,1,2,3};
90  Vector x = xvalues.vector(keys2);
91  Vector expected = Vector::Zero(24);
92  RegularImplicitSchurFactor<CalibratedCamera>::multiplyHessianAdd(F, E, P, alpha, x, expected);
93  EXPECT(assert_equal(expected, yExpected.vector(keys2), 1e-8));
94 
95  // Create ImplicitSchurFactor
96  RegularImplicitSchurFactor<CalibratedCamera> implicitFactor(keys, FBlocks, E, P, b);
97 
98  VectorValues zero = 0 * yExpected;// quick way to get zero w right structure
99  { // First Version
100  VectorValues yActual = zero;
101  implicitFactor.multiplyHessianAdd(alpha, xvalues, yActual);
102  EXPECT(assert_equal(yExpected, yActual, 1e-8));
103  implicitFactor.multiplyHessianAdd(alpha, xvalues, yActual);
104  EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
105  implicitFactor.multiplyHessianAdd(-1, xvalues, yActual);
106  EXPECT(assert_equal(zero, yActual, 1e-8));
107  }
108 
109  typedef Eigen::Matrix<double, 24, 1> DeltaX;
110  typedef Eigen::Map<DeltaX> XMap;
111  double* y = new double[24];
112  double* xdata = x.data();
113 
114  { // Raw memory Version
115  std::fill(y, y + 24, 0);// zero y !
116  implicitFactor.multiplyHessianAdd(alpha, xdata, y);
117  EXPECT(assert_equal(expected, XMap(y), 1e-8));
118  implicitFactor.multiplyHessianAdd(alpha, xdata, y);
119  EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
120  implicitFactor.multiplyHessianAdd(-1, xdata, y);
121  EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
122  }
123 
124  // Create JacobianFactor with same error
125  const SharedDiagonal model;
126  JacobianFactorQ<6, 2> jfQ(keys, FBlocks, E, P, b, model);
127 
128  // error
129  double expectedError = 11875.083333333334;
130  {
131  EXPECT_DOUBLES_EQUAL(expectedError,jfQ.error(xvalues),1e-7)
132  EXPECT_DOUBLES_EQUAL(expectedError,implicitFactor.errorJF(xvalues),1e-7)
133  EXPECT_DOUBLES_EQUAL(11903.500000000007,implicitFactor.error(xvalues),1e-7)
134  }
135 
136  {
137  VectorValues yActual = zero;
138  jfQ.multiplyHessianAdd(alpha, xvalues, yActual);
139  EXPECT(assert_equal(yExpected, yActual, 1e-8));
140  jfQ.multiplyHessianAdd(alpha, xvalues, yActual);
141  EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
142  jfQ.multiplyHessianAdd(-1, xvalues, yActual);
143  EXPECT(assert_equal(zero, yActual, 1e-8));
144  }
145 
146  { // check hessian Diagonal
147  VectorValues diagExpected = jfQ.hessianDiagonal();
148  VectorValues diagActual = implicitFactor.hessianDiagonal();
149  EXPECT(assert_equal(diagExpected, diagActual, 1e-8));
150  }
151 
152  { // check hessian Block Diagonal
153  map<Key,Matrix> BD = jfQ.hessianBlockDiagonal();
154  map<Key,Matrix> actualBD = implicitFactor.hessianBlockDiagonal();
155  LONGS_EQUAL(3,actualBD.size());
156  EXPECT(assert_equal(BD[0],actualBD[0]));
157  EXPECT(assert_equal(BD[1],actualBD[1]));
158  EXPECT(assert_equal(BD[3],actualBD[3]));
159  }
160 
161  { // Raw memory Version
162  std::fill(y, y + 24, 0);// zero y !
163  jfQ.multiplyHessianAdd(alpha, xdata, y);
164  EXPECT(assert_equal(expected, XMap(y), 1e-8));
165  jfQ.multiplyHessianAdd(alpha, xdata, y);
166  EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
167  jfQ.multiplyHessianAdd(-1, xdata, y);
168  EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
169  }
170 
171  VectorValues expectedVV;
172  expectedVV.insert(0,-3.5*Vector::Ones(6));
173  expectedVV.insert(1,10*Vector::Ones(6)/3);
174  expectedVV.insert(3,-19.5*Vector::Ones(6));
175  { // Check gradientAtZero
176  VectorValues actual = implicitFactor.gradientAtZero();
177  EXPECT(assert_equal(expectedVV, jfQ.gradientAtZero(), 1e-8));
178  EXPECT(assert_equal(expectedVV, implicitFactor.gradientAtZero(), 1e-8));
179  }
180 
181  // Create JacobianFactorQR
182  JacobianFactorQR<6, 2> jfQR(keys, FBlocks, E, P, b, model);
183  EXPECT_DOUBLES_EQUAL(expectedError, jfQR.error(xvalues),1e-7)
184  EXPECT(assert_equal(expectedVV, jfQR.gradientAtZero(), 1e-8));
185  {
186  const SharedDiagonal model;
187  VectorValues yActual = zero;
188  jfQR.multiplyHessianAdd(alpha, xvalues, yActual);
189  EXPECT(assert_equal(yExpected, yActual, 1e-8));
190  jfQR.multiplyHessianAdd(alpha, xvalues, yActual);
191  EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
192  jfQR.multiplyHessianAdd(-1, xvalues, yActual);
193  EXPECT(assert_equal(zero, yActual, 1e-8));
194  }
195 
196  { // Raw memory Version
197  std::fill(y, y + 24, 0);// zero y !
198  jfQR.multiplyHessianAdd(alpha, xdata, y);
199  EXPECT(assert_equal(expected, XMap(y), 1e-8));
200  jfQR.multiplyHessianAdd(alpha, xdata, y);
201  EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
202  jfQR.multiplyHessianAdd(-1, xdata, y);
203  EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
204  }
205  delete [] y;
206 }
207 
208 /* ************************************************************************* */
209 TEST(regularImplicitSchurFactor, hessianDiagonal)
210 {
211  /* TESTED AGAINST MATLAB
212  * F = [Vector::Ones(2,6) zeros(2,6) zeros(2,6)
213  zeros(2,6) 2*Vector::Ones(2,6) zeros(2,6)
214  zeros(2,6) zeros(2,6) 3*Vector::Ones(2,6)]
215  E = [[1:6] [1:6] [0.5 1:5]];
216  E = reshape(E',3,6)'
217  P = inv(E' * E)
218  H = F' * (eye(6) - E * P * E') * F
219  diag(H)
220  */
221  Matrix E(6,3);
222  E.block<2,3>(0, 0) << 1,2,3,4,5,6;
223  E.block<2,3>(2, 0) << 1,2,3,4,5,6;
224  E.block<2,3>(4, 0) << 0.5,1,2,3,4,5;
225  Matrix3 P = (E.transpose() * E).inverse();
226  RegularImplicitSchurFactor<CalibratedCamera> factor(keys, FBlocks, E, P, b);
227 
228  // hessianDiagonal
229  VectorValues expected;
230  expected.insert(0, 1.195652*Vector::Ones(6));
231  expected.insert(1, 4.782608*Vector::Ones(6));
232  expected.insert(3, 7.043478*Vector::Ones(6));
233  EXPECT(assert_equal(expected, factor.hessianDiagonal(),1e-5));
234 
235  // hessianBlockDiagonal
236  map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
237  LONGS_EQUAL(3,actualBD.size());
238  Matrix FtE0 = F0.transpose() * E.block<2,3>(0, 0);
239  Matrix FtE1 = F1.transpose() * E.block<2,3>(2, 0);
240  Matrix FtE3 = F3.transpose() * E.block<2,3>(4, 0);
241 
242  // variant one
243  EXPECT(assert_equal(F0.transpose()*F0-FtE0*P*FtE0.transpose(),actualBD[0]));
244  EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1]));
245  EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
246 
247  // variant two
248  Matrix I2 = I_2x2;
249  Matrix E0 = E.block<2,3>(0, 0);
250  Matrix F0t = F0.transpose();
251  EXPECT(assert_equal(F0t*F0-F0t*E0*P*E0.transpose()*F0,actualBD[0]));
252  EXPECT(assert_equal(F0t*(F0-E0*P*E0.transpose()*F0),actualBD[0]));
253 
254  Matrix M1 = F0t*(F0-E0*P*E0.transpose()*F0);
255  Matrix M2 = F0t*F0-F0t*E0*P*E0.transpose()*F0;
256 
257  EXPECT(assert_equal( M1 , actualBD[0] ));
258  EXPECT(assert_equal( M1 , M2 ));
259 
260  Matrix M1b = F0t*(E0*P*E0.transpose()*F0);
261  Matrix M2b = F0t*E0*P*E0.transpose()*F0;
262  EXPECT(assert_equal( M1b , M2b ));
263 
264  EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0]));
265  EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1]));
266  EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
267 
268  // augmentedInformation (test just checks diagonals)
269  Matrix actualInfo = factor.augmentedInformation();
270  EXPECT(assert_equal(actualBD[0],actualInfo.block<6,6>(0,0)));
271  EXPECT(assert_equal(actualBD[1],actualInfo.block<6,6>(6,6)));
272  EXPECT(assert_equal(actualBD[3],actualInfo.block<6,6>(12,12)));
273 
274  // information (test just checks diagonals)
275  Matrix actualInfo2 = factor.information();
276  EXPECT(assert_equal(actualBD[0],actualInfo2.block<6,6>(0,0)));
277  EXPECT(assert_equal(actualBD[1],actualInfo2.block<6,6>(6,6)));
278  EXPECT(assert_equal(actualBD[3],actualInfo2.block<6,6>(12,12)));
279 }
280 
281 /* ************************************************************************* */
282 int main(void) {
283  TestResult tr;
284  int result = TestRegistry::runAllTests(tr);
285  return result;
286 }
287 //*************************************************************************************
test_docs.d
d
Definition: test_docs.py:28
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:38
gtsam::RegularImplicitSchurFactor::errorJF
double errorJF(const VectorValues &x) const
Definition: RegularImplicitSchurFactor.h:317
gtsam::symbol_shorthand::E
Key E(std::uint64_t j)
Definition: inference/Symbol.h:147
y
Scalar * y
Definition: level1_cplx_impl.h:102
F3
const Matrix26 F3
Definition: testRegularImplicitSchurFactor.cpp:43
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
TEST
TEST(regularImplicitSchurFactor, creation)
Definition: testRegularImplicitSchurFactor.cpp:50
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
Eigen::Map
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
gtsam::RegularImplicitSchurFactor::multiplyHessianAdd
static void multiplyHessianAdd(const Matrix &F, const Matrix &E, const Matrix &PointCovariance, double alpha, const Vector &x, Vector &y)
Definition: RegularImplicitSchurFactor.h:253
zero
EIGEN_DONT_INLINE Scalar zero()
Definition: svd_common.h:271
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
gtsam::JacobianFactor::hessianBlockDiagonal
std::map< Key, Matrix > hessianBlockDiagonal() const override
Return the block diagonal of the Hessian for this factor.
Definition: JacobianFactor.cpp:569
gtsam::RegularImplicitSchurFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
add the contribution of this factor to the diagonal of the hessian d(output) = d(input) + deltaHessia...
Definition: RegularImplicitSchurFactor.h:182
FBlocks
const vector< Matrix26, Eigen::aligned_allocator< Matrix26 > > FBlocks
Definition: testRegularImplicitSchurFactor.cpp:44
M1
MatrixXf M1
Definition: Tutorial_SlicingCol.cpp:1
gtsam::RegularJacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
Definition: RegularJacobianFactor.h:73
std
Definition: Half.h:150
F1
const Matrix26 F1
Definition: testRegularImplicitSchurFactor.cpp:42
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:162
gtsam::RegularImplicitSchurFactor::error
double error(const VectorValues &x) const override
Definition: RegularImplicitSchurFactor.h:295
VectorValues.h
Factor Graph Values.
gtsam::RegularJacobianFactor::gradientAtZero
VectorValues gradientAtZero() const override
Expose base class gradientAtZero.
Definition: RegularJacobianFactor.h:128
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::parser::F
Signature::Row F
Definition: Signature.cpp:53
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
M2
M1<< 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;Map< MatrixXf > M2(M1.data(), 6, 2)
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:125
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
F0
const Matrix26 F0
Definition: testRegularImplicitSchurFactor.cpp:41
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
b
const Vector b
Definition: testRegularImplicitSchurFactor.cpp:47
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:135
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:736
gtsam
traits
Definition: chartTesting.h:28
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
RegularImplicitSchurFactor.h
A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor.
gtsam::RegularImplicitSchurFactor::gradientAtZero
VectorValues gradientAtZero() const override
Definition: RegularImplicitSchurFactor.h:438
gtsam::RegularImplicitSchurFactor::information
Matrix information() const override
Compute full information matrix
Definition: RegularImplicitSchurFactor.h:140
gtsam::JacobianFactor::error
double error(const VectorValues &c) const override
Definition: JacobianFactor.cpp:505
gtsam::RegularImplicitSchurFactor::hessianBlockDiagonal
std::map< Key, Matrix > hessianBlockDiagonal() const override
Return the block diagonal of the Hessian for this factor.
Definition: RegularImplicitSchurFactor.h:208
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:45
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
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
Point2.h
2D Point
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:90
inverse
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:332
gtsam::RegularImplicitSchurFactor::augmentedInformation
Matrix augmentedInformation() const override
Compute full augmented information matrix
Definition: RegularImplicitSchurFactor.h:132
timing.h
Timing utilities.
gtsam::RegularJacobianFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
Raw memory access version of hessianDiagonal.
Definition: RegularJacobianFactor.h:109
gtsam::VectorValues::vector
Vector vector() const
Definition: VectorValues.cpp:173

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:49:17