testDoglegOptimizer.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 <CppUnitLite/TestHarness.h>
20 
21 #include <tests/smallExample.h>
22 #include <gtsam/geometry/Pose2.h>
23 #include <gtsam/nonlinear/DoglegOptimizer.h>
24 #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
25 #include <gtsam/nonlinear/NonlinearEquality.h>
26 #include <gtsam/slam/BetweenFactor.h>
27 #include <gtsam/inference/Symbol.h>
28 #include <gtsam/linear/JacobianFactor.h>
29 #include <gtsam/linear/GaussianBayesTree.h>
30 #include <gtsam/base/numericalDerivative.h>
31 
32 #ifdef __GNUC__
33 #pragma GCC diagnostic push
34 #pragma GCC diagnostic ignored "-Wunused-variable"
35 #endif
36 #include <boost/bind.hpp>
37 #ifdef __GNUC__
38 #pragma GCC diagnostic pop
39 #endif
40 #include <boost/assign/list_of.hpp> // for 'list_of()'
41 #include <functional>
42 #include <boost/iterator/counting_iterator.hpp>
43 
44 using namespace std;
45 using namespace gtsam;
46 
47 // Convenience for named keys
48 using symbol_shorthand::X;
49 using symbol_shorthand::L;
50 
51 /* ************************************************************************* */
52 TEST(DoglegOptimizer, ComputeBlend) {
53  // Create an arbitrary Bayes Net
54  GaussianBayesNet gbn;
55  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
56  0, Vector2(1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(),
57  3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(),
58  4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished()));
59  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
60  1, Vector2(15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(),
61  2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(),
62  4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished()));
63  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
64  2, Vector2(29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(),
65  3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished()));
66  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
67  3, Vector2(39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(),
68  4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished()));
69  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
70  4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished()));
71 
72  // Compute steepest descent point
73  VectorValues xu = gbn.optimizeGradientSearch();
74 
75  // Compute Newton's method point
76  VectorValues xn = gbn.optimize();
77 
78  // The Newton's method point should be more "adventurous", i.e. larger, than the steepest descent point
79  EXPECT(xu.vector().norm() < xn.vector().norm());
80 
81  // Compute blend
82  double Delta = 1.5;
83  VectorValues xb = DoglegOptimizerImpl::ComputeBlend(Delta, xu, xn);
84  DOUBLES_EQUAL(Delta, xb.vector().norm(), 1e-10);
85 }
86 
87 /* ************************************************************************* */
88 TEST(DoglegOptimizer, ComputeDoglegPoint) {
89  // Create an arbitrary Bayes Net
90  GaussianBayesNet gbn;
91  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
92  0, Vector2(1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(),
93  3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(),
94  4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished()));
95  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
96  1, Vector2(15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(),
97  2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(),
98  4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished()));
99  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
100  2, Vector2(29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(),
101  3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished()));
102  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
103  3, Vector2(39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(),
104  4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished()));
105  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
106  4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished()));
107 
108  // Compute dogleg point for different deltas
109 
110  double Delta1 = 0.5; // Less than steepest descent
111  VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, gbn.optimizeGradientSearch(), gbn.optimize());
112  DOUBLES_EQUAL(Delta1, actual1.vector().norm(), 1e-5);
113 
114  double Delta2 = 1.5; // Between steepest descent and Newton's method
115  VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, gbn.optimizeGradientSearch(), gbn.optimize());
116  VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, gbn.optimizeGradientSearch(), gbn.optimize());
117  DOUBLES_EQUAL(Delta2, actual2.vector().norm(), 1e-5);
118  EXPECT(assert_equal(expected2, actual2));
119 
120  double Delta3 = 5.0; // Larger than Newton's method point
121  VectorValues expected3 = gbn.optimize();
122  VectorValues actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, gbn.optimizeGradientSearch(), gbn.optimize());
123  EXPECT(assert_equal(expected3, actual3));
124 }
125 
126 /* ************************************************************************* */
127 TEST(DoglegOptimizer, Iterate) {
128  // really non-linear factor graph
129  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
130 
131  // config far from minimum
132  Point2 x0(3,0);
133  Values config;
134  config.insert(X(1), x0);
135 
136  double Delta = 1.0;
137  for(size_t it=0; it<10; ++it) {
138  GaussianBayesNet gbn = *fg.linearize(config)->eliminateSequential();
139  // Iterate assumes that linear error = nonlinear error at the linearization point, and this should be true
140  double nonlinearError = fg.error(config);
141  double linearError = GaussianFactorGraph(gbn).error(config.zeroVectors());
142  DOUBLES_EQUAL(nonlinearError, linearError, 1e-5);
143 // cout << "it " << it << ", Delta = " << Delta << ", error = " << fg->error(*config) << endl;
144  VectorValues dx_u = gbn.optimizeGradientSearch();
145  VectorValues dx_n = gbn.optimize();
146  DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, dx_u, dx_n, gbn, fg, config, fg.error(config));
147  Delta = result.delta;
148  EXPECT(result.f_error < fg.error(config)); // Check that error decreases
149  Values newConfig(config.retract(result.dx_d));
150  config = newConfig;
151  DOUBLES_EQUAL(fg.error(config), result.f_error, 1e-5); // Check that error is correctly filled in
152  }
153 }
154 
155 /* ************************************************************************* */
156 TEST(DoglegOptimizer, Constraint) {
157  // Create a pose-graph graph with a constraint on the first pose
158  NonlinearFactorGraph graph;
159  const Pose2 origin(0, 0, 0), pose2(2, 0, 0);
160  graph.emplace_shared<NonlinearEquality<Pose2> >(1, origin);
161  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
162  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, pose2, model);
163 
164  // Create feasible initial estimate
165  Values initial;
166  initial.insert(1, origin); // feasible !
167  initial.insert(2, Pose2(2.3, 0.1, -0.2));
168 
169  // Optimize the initial values using DoglegOptimizer
170  DoglegParams params;
171  params.setVerbosityDL("VERBOSITY");
172  DoglegOptimizer optimizer(graph, initial, params);
173  Values result = optimizer.optimize();
174 
175  // Check result
176  EXPECT(assert_equal(pose2, result.at<Pose2>(2)));
177 
178  // Create infeasible initial estimate
179  Values infeasible;
180  infeasible.insert(1, Pose2(0.1, 0, 0)); // infeasible !
181  infeasible.insert(2, Pose2(2.3, 0.1, -0.2));
182 
183  // Try optimizing with infeasible initial estimate
184  DoglegOptimizer optimizer2(graph, infeasible, params);
185 
186 #ifdef GTSAM_USE_TBB
187  CHECK_EXCEPTION(optimizer2.optimize(), std::exception);
188 #else
189  CHECK_EXCEPTION(optimizer2.optimize(), std::invalid_argument);
190 #endif
191 }
192 
193 /* ************************************************************************* */
194 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
195 /* ************************************************************************* */
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::Values::zeroVectors
VectorValues zeroVectors() const
Definition: Values.cpp:216
origin
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 origin
Definition: gnuplot_common_settings.hh:12
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:140
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:142
gtsam::Point2
Vector2 Point2
Definition: Point2.h:27
TEST
TEST(DoglegOptimizer, ComputeBlend)
Definition: testDoglegOptimizer.cpp:52
gtsam::example::createReallyNonlinearFactorGraph
NonlinearFactorGraph createReallyNonlinearFactorGraph()
Definition: smallExample.h:377
initial
Values initial
Definition: OdometryOptimize.cpp:2
L
MatrixXd L
Definition: LLT_example.cpp:6
gtsam::GaussianBayesNet::optimize
VectorValues optimize() const
Solve the GaussianBayesNet, i.e. return , by back-substitution.
Definition: GaussianBayesNet.cpp:41
std
Definition: Half.h:150
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::GaussianBayesNet::optimizeGradientSearch
VectorValues optimizeGradientSearch() const
Definition: GaussianBayesNet.cpp:62
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Definition: Values.cpp:109
CHECK_EXCEPTION
#define CHECK_EXCEPTION(condition, exception_name)
Definition: Test.h:119
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::GaussianFactorGraph::error
double error(const VectorValues &x) const
Definition: GaussianFactorGraph.h:145
DoglegOptimizerImpl.h
Nonlinear factor graph optimizer using Powell&#39;s Dogleg algorithm (detail implementation) ...
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&...args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:172
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:342
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
params
static SmartStereoProjectionParams params
Definition: testSmartStereoProjectionPoseFactor.cpp:43
main
int main()
Definition: testDoglegOptimizer.cpp:194
x0
static Symbol x0('x', 0)
gtsam
traits
Definition: chartTesting.h:28
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
gtsam::DoglegParams::setVerbosityDL
void setVerbosityDL(const std::string &verbosityDL)
Definition: DoglegOptimizer.h:58
smallExample.h
Create small example with two poses and one landmark.
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
pose2
static const Pose3 pose2
Definition: testTriangulation.cpp:48
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:271
Pose2.h
2D Pose
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianConditional.h:42
gtsam::NonlinearFactorGraph::linearize
boost::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:340
X
#define X
Definition: icosphere.cpp:20
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
gtsam::VectorValues::vector
Vector vector() const
Definition: VectorValues.cpp:173

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