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

gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:41:16