testRegularJacobianFactor.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 
24 
26 
27 using namespace std;
28 using namespace gtsam;
29 
30 static const size_t fixedDim = 3;
31 static const size_t nrKeys = 3;
32 
33 // Keys are assumed to be from 0 to n
34 namespace {
35  namespace simple {
36  // Terms we'll use
37  const vector<pair<Key, Matrix> > terms{
38  {0, 1 * I_3x3}, {1, 2 * I_3x3}, {2, 3 * I_3x3}};
39 
40  // RHS and sigmas
41  const Vector b = (Vector(3) << 1., 2., 3.).finished();
42  const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5,0.5,0.5).finished());
43  }
44 
45  namespace simple2 {
46  // Terms
47  const vector<pair<Key, Matrix> > terms2{
48  {0, 2 * I_3x3}, {1, 4 * I_3x3}, {2, 6 * I_3x3}};
49 
50  // RHS
51  const Vector b2 = (Vector(3) << 2., 4., 6.).finished();
52  }
53 }
54 
55 /* ************************************************************************* */
56 // Convert from double* to VectorValues
57 VectorValues double2vv(const double* x,
58  const size_t nrKeys, const size_t dim) {
59  // create map with dimensions
60  std::map<gtsam::Key, size_t> dims;
61  for (size_t i = 0; i < nrKeys; i++)
62  dims.insert(make_pair(i, dim));
63 
64  size_t n = nrKeys*dim;
65  Vector xVec(n);
66  for (size_t i = 0; i < n; i++){
67  xVec(i) = x[i];
68  }
69  return VectorValues(xVec, dims);
70 }
71 
72 /* ************************************************************************* */
73 void vv2double(const VectorValues& vv, double* y,
74  const size_t nrKeys, const size_t dim) {
75  // create map with dimensions
76  std::map<gtsam::Key, size_t> dims;
77  for (size_t i = 0; i < nrKeys; i++)
78  dims.insert(make_pair(i, dim));
79 
80  Vector yvector = vv.vector(dims);
81  size_t n = nrKeys*dim;
82  for (size_t j = 0; j < n; j++)
83  y[j] = yvector[j];
84 }
85 
86 /* ************************************************************************* */
87 TEST(RegularJacobianFactor, constructorNway)
88 {
89  using namespace simple;
90  JacobianFactor factor(terms[0].first, terms[0].second,
91  terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
92  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
93 
94  LONGS_EQUAL((long)terms[2].first, (long)regularFactor.keys().back());
95  EXPECT(assert_equal(terms[2].second, regularFactor.getA(regularFactor.end() - 1)));
96  EXPECT(assert_equal(b, factor.getb()));
97  EXPECT(assert_equal(b, regularFactor.getb()));
98  EXPECT(noise == factor.get_model());
99  EXPECT(noise == regularFactor.get_model());
100 }
101 
102 /* ************************************************************************* */
103 TEST(RegularJacobianFactor, hessianDiagonal)
104 {
105  using namespace simple;
106  JacobianFactor factor(terms[0].first, terms[0].second,
107  terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
108  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
109 
110  // we compute hessian diagonal from the standard Jacobian
111  VectorValues expectedHessianDiagonal = factor.hessianDiagonal();
112 
113  // we compare against the Raw memory access implementation of hessianDiagonal
114  double actualValue[9]={0};
115  regularFactor.hessianDiagonal(actualValue);
116  VectorValues actualHessianDiagonalRaw = double2vv(actualValue,nrKeys,fixedDim);
117  EXPECT(assert_equal(expectedHessianDiagonal, actualHessianDiagonalRaw));
118 }
119 
120 /* ************************************************************************* */
121 TEST(RegularJacobian, gradientAtZero)
122 {
123  using namespace simple;
124  JacobianFactor factor(terms[0].first, terms[0].second,
125  terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
126  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
127 
128  // we compute gradient at zero from the standard Jacobian
129  VectorValues expectedGradientAtZero = factor.gradientAtZero();
130 
131  //EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero()));
132 
133  // we compare against the Raw memory access implementation of gradientAtZero
134  double actualValue[9]={0};
135  regularFactor.gradientAtZero(actualValue);
136  VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim);
137  EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw));
138 }
139 
140 /* ************************************************************************* */
141 TEST(RegularJacobian, gradientAtZero_multiFactors)
142 {
143  using namespace simple;
144  JacobianFactor factor(terms[0].first, terms[0].second,
145  terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
146  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
147 
148  // we compute gradient at zero from the standard Jacobian
149  VectorValues expectedGradientAtZero = factor.gradientAtZero();
150 
151  // we compare against the Raw memory access implementation of gradientAtZero
152  double actualValue[9]={0};
153  regularFactor.gradientAtZero(actualValue);
154  VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim);
155  EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw));
156 
157  // One more factor
158  using namespace simple2;
159  JacobianFactor factor2(terms2[0].first, terms2[0].second,
160  terms2[1].first, terms2[1].second, terms2[2].first, terms2[2].second, b2, noise);
161  RegularJacobianFactor<fixedDim> regularFactor2(terms2, b2, noise);
162 
163  // we accumulate computed gradient at zero from the standard Jacobian
164  VectorValues expectedGradientAtZero2 = expectedGradientAtZero.add(factor2.gradientAtZero());
165 
166  // we compare against the Raw memory access implementation of gradientAtZero
167  regularFactor2.gradientAtZero(actualValue);
168  VectorValues actualGradientAtZeroRaw2 = double2vv(actualValue,nrKeys,fixedDim);
169  EXPECT(assert_equal(expectedGradientAtZero2, actualGradientAtZeroRaw2));
170 
171 }
172 
173 /* ************************************************************************* */
174 TEST(RegularJacobian, multiplyHessianAdd)
175 {
176  using namespace simple;
177  JacobianFactor factor(terms[0].first, terms[0].second,
178  terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
179  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
180 
181  // arbitrary vector X
182  VectorValues X;
183  X.insert(0, (Vector(3) << 10.,20.,30.).finished());
184  X.insert(1, (Vector(3) << 10.,20.,30.).finished());
185  X.insert(2, (Vector(3) << 10.,20.,30.).finished());
186 
187  // arbitrary vector Y
188  VectorValues Y;
189  Y.insert(0, (Vector(3) << 10.,10.,10.).finished());
190  Y.insert(1, (Vector(3) << 20.,20.,20.).finished());
191  Y.insert(2, (Vector(3) << 30.,30.,30.).finished());
192 
193  // multiplyHessianAdd Y += alpha*A'A*X
194  double alpha = 2.0;
195  VectorValues expectedMHA = Y;
196  factor.multiplyHessianAdd(alpha, X, expectedMHA);
197 
198  // create data for raw memory access
199  double XRaw[9];
200  vv2double(X, XRaw, nrKeys, fixedDim);
201 
202  // test 1st version: multiplyHessianAdd(double alpha, const double* x, double* y)
203  double actualMHARaw[9];
204  vv2double(Y, actualMHARaw, nrKeys, fixedDim);
205  regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw);
206  VectorValues actualMHARawVV = double2vv(actualMHARaw,nrKeys,fixedDim);
207  EXPECT(assert_equal(expectedMHA,actualMHARawVV));
208 
209  // test 2nd version: multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys)
210  double actualMHARaw2[9];
211  vv2double(Y, actualMHARaw2, nrKeys, fixedDim);
212  vector<size_t> dims;
213  size_t accumulatedDim = 0;
214  for (size_t i = 0; i < nrKeys+1; i++){
215  dims.push_back(accumulatedDim);
216  accumulatedDim += fixedDim;
217  }
218  regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw2, dims);
219  VectorValues actualMHARawVV2 = double2vv(actualMHARaw2,nrKeys,fixedDim);
220  EXPECT(assert_equal(expectedMHA,actualMHARawVV2));
221 }
222 
223 /* ************************************************************************* */
224 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
225 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
GaussianConditional.h
Conditional Gaussian Base class.
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:203
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
simple2
Definition: testRegularJacobianFactor.cpp:45
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
Definition: JacobianFactor.h:292
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
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::Y
GaussianFactorGraphValuePair Y
Definition: HybridGaussianProductFactor.cpp:29
nrKeys
static const size_t nrKeys
Definition: testRegularJacobianFactor.cpp:31
simple_graph::b2
Vector2 b2(4, -5)
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::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::JacobianFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
Raw memory access version of hessianDiagonal.
Definition: JacobianFactor.cpp:545
X
#define X
Definition: icosphere.cpp:20
vv
static const VectorValues vv
Definition: testHybridGaussianConditional.cpp:44
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
double2vv
VectorValues double2vv(const double *x, const size_t nrKeys, const size_t dim)
Definition: testRegularJacobianFactor.cpp:57
gtsam::JacobianFactor::getb
const constBVector getb() const
Definition: JacobianFactor.h:298
TestableAssertions.h
Provides additional testing facilities for common data structures.
gtsam::JacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
Definition: JacobianFactor.cpp:638
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::RegularJacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
Definition: RegularJacobianFactor.h:73
gtsam::VectorValues
Definition: VectorValues.h:74
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::JacobianFactor::gradientAtZero
VectorValues gradientAtZero() const override
A'*b for Jacobian.
Definition: JacobianFactor.cpp:690
fixedDim
static const size_t fixedDim
Definition: testRegularJacobianFactor.cpp:30
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
main
int main()
Definition: testRegularJacobianFactor.cpp:224
VectorValues.h
Factor Graph Values.
gtsam::Factor::end
const_iterator end() const
Definition: Factor.h:149
TestResult
Definition: TestResult.h:26
y
Scalar * y
Definition: level1_cplx_impl.h:124
TEST
TEST(RegularJacobianFactor, constructorNway)
Definition: testRegularJacobianFactor.cpp:87
gtsam
traits
Definition: SFMdata.h:40
gtsam::JacobianFactor::getA
constABlock getA(const_iterator variable) const
Definition: JacobianFactor.h:301
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:143
std
Definition: BFloat16.h:88
gtsam::RegularJacobianFactor
Definition: RegularJacobianFactor.h:32
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
simple
Definition: testJacobianFactor.cpp:34
vv2double
void vv2double(const VectorValues &vv, double *y, const size_t nrKeys, const size_t dim)
Definition: testRegularJacobianFactor.cpp:73
gtsam::VectorValues::add
VectorValues add(const VectorValues &c) const
Definition: VectorValues.cpp:287
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
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 Fri Nov 1 2024 03:41:16