testNonlinearConjugateGradientOptimizer.cpp
Go to the documentation of this file.
1 
10 #include <gtsam/geometry/Pose2.h>
11 #include <gtsam/inference/Symbol.h>
15 #include <gtsam/nonlinear/Values.h>
18 
19 using namespace std;
20 using namespace gtsam;
21 
24 
25 // Generate a small PoseSLAM problem
26 std::tuple<NonlinearFactorGraph, Values> generateProblem() {
27  // 1. Create graph container and add factors to it
29 
30  // 2a. Add Gaussian prior
31  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
33  noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
35 
36  // 2b. Add odometry factors
38  noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
39  graph.emplace_shared<BetweenFactor<Pose2>>(1, 2, Pose2(2.0, 0.0, 0.0),
47 
48  // 2c. Add pose constraint
49  SharedDiagonal constraintUncertainty =
50  noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
52  constraintUncertainty);
53 
54  // 3. Create the data structure to hold the initialEstimate estimate to the
55  // solution
56  Values initialEstimate;
57  Pose2 x1(0.5, 0.0, 0.2);
58  initialEstimate.insert(1, x1);
59  Pose2 x2(2.3, 0.1, -0.2);
60  initialEstimate.insert(2, x2);
61  Pose2 x3(4.1, 0.1, M_PI_2);
62  initialEstimate.insert(3, x3);
63  Pose2 x4(4.0, 2.0, M_PI);
64  initialEstimate.insert(4, x4);
65  Pose2 x5(2.1, 2.1, -M_PI_2);
66  initialEstimate.insert(5, x5);
67 
68  return {graph, initialEstimate};
69 }
70 
71 /* ************************************************************************* */
73  const auto [graph, initialEstimate] = generateProblem();
74  // cout << "initial error = " << graph.error(initialEstimate) << endl;
75 
77  param.maxIterations =
78  500; /* requires a larger number of iterations to converge */
79  param.verbosity = NonlinearOptimizerParams::SILENT;
80 
81  NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param);
82  Values result = optimizer.optimize();
83  // cout << "cg final error = " << graph.error(result) << endl;
84 
86 }
87 
88 namespace rosenbrock {
89 
90 class Rosenbrock1Factor : public NoiseModelFactorN<double> {
91  private:
94 
95  double a_;
96 
97  public:
99  Rosenbrock1Factor(Key key, double a, const SharedNoiseModel& model = nullptr)
100  : Base(model, key), a_(a) {}
101 
103  Vector evaluateError(const double& x, OptionalMatrixType H) const override {
104  double d = x - a_;
105  // Because linearized gradient is -A'b/sigma, it will multiply by d
106  if (H) (*H) = Vector1(1).transpose();
107  return Vector1(d);
108  }
109 };
110 
117 class Rosenbrock2Factor : public NoiseModelFactorN<double, double> {
118  private:
121 
122  public:
125  : Base(model, key1, key2) {}
126 
128  Vector evaluateError(const double& x, const double& y, OptionalMatrixType H1,
129  OptionalMatrixType H2) const override {
130  double x2 = x * x, d = x2 - y;
131  // Because linearized gradient is -A'b/sigma, it will multiply by d
132  if (H1) (*H1) = Vector1(2 * x).transpose();
133  if (H2) (*H2) = Vector1(-1).transpose();
134  return Vector1(d);
135  }
136 };
137 
149  double b = 100.0) {
152  X(0), a, noiseModel::Isotropic::Precision(1, 2));
154  X(0), Y(0), noiseModel::Isotropic::Precision(1, 2 * b));
155 
156  return graph;
157 }
158 
161 double f(const NonlinearFactorGraph& graph, double x, double y) {
162  Values initial;
163  initial.insert<double>(X(0), x);
164  initial.insert<double>(Y(0), y);
165 
166  return graph.error(initial);
167 }
168 
170 double rosenbrock_func(double x, double y, double a = 1.0, double b = 100.0) {
171  double m = (a - x) * (a - x);
172  double n = b * (y - x * x) * (y - x * x);
173  return m + n;
174 }
175 } // namespace rosenbrock
176 
177 /* ************************************************************************* */
178 // Test whether the 2 factors are properly implemented.
180  using namespace rosenbrock;
181  double a = 1.0, b = 100.0;
182  auto graph = GetRosenbrockGraph(a, b);
184  *std::static_pointer_cast<Rosenbrock1Factor>(graph.at(0));
186  *std::static_pointer_cast<Rosenbrock2Factor>(graph.at(1));
187  Values values;
188  values.insert<double>(X(0), 3.0);
189  values.insert<double>(Y(0), 5.0);
192 
193  std::mt19937 rng(42);
194  std::uniform_real_distribution<double> dist(0.0, 100.0);
195  for (size_t i = 0; i < 50; ++i) {
196  double x = dist(rng);
197  double y = dist(rng);
198 
199  auto graph = GetRosenbrockGraph(a, b);
201  }
202 }
203 
204 /* ************************************************************************* */
205 // Optimize the Rosenbrock function to verify optimizer works
207  using namespace rosenbrock;
208 
209  double a = 12;
210  auto graph = GetRosenbrockGraph(a);
211 
212  // Assert that error at global minimum is 0.
213  double error = f(graph, a, a * a);
214  EXPECT_DOUBLES_EQUAL(0.0, error, 1e-12);
215 
217  param.maxIterations = 350;
218  // param.verbosity = NonlinearOptimizerParams::LINEAR;
219  param.verbosity = NonlinearOptimizerParams::SILENT;
220 
221  double x = 3.0, y = 5.0;
222  Values initialEstimate;
223  initialEstimate.insert<double>(X(0), x);
224  initialEstimate.insert<double>(Y(0), y);
225 
226  GaussianFactorGraph::shared_ptr linear = graph.linearize(initialEstimate);
227  // std::cout << "error: " << f(graph, x, y) << std::endl;
228  // linear->print();
229  // linear->gradientAtZero().print("Gradient: ");
230 
231  NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param);
232  Values result = optimizer.optimize();
233  // result.print();
234  // cout << "cg final error = " << graph.error(result) << endl;
235 
237  expected.insert<double>(X(0), a);
238  expected.insert<double>(Y(0), a * a);
240 }
241 
242 /* ************************************************************************* */
245  const auto [graph, initialEstimate] = generateProblem();
246 
248  param.maxIterations =
249  500; /* requires a larger number of iterations to converge */
250  param.verbosity = NonlinearOptimizerParams::SILENT;
251 
252  // Fletcher-Reeves
253  {
255  graph, initialEstimate, param, DirectionMethod::FletcherReeves);
256  Values result = optimizer.optimize();
257 
259  }
260  // Polak-Ribiere
261  {
263  graph, initialEstimate, param, DirectionMethod::PolakRibiere);
264  Values result = optimizer.optimize();
265 
267  }
268  // Hestenes-Stiefel
269  {
271  graph, initialEstimate, param, DirectionMethod::HestenesStiefel);
272  Values result = optimizer.optimize();
273 
275  }
276  // Dai-Yuan
277  {
278  NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param,
280  Values result = optimizer.optimize();
281 
283  }
284 }
285 /* ************************************************************************* */
286 int main() {
287  TestResult tr;
288  return TestRegistry::runAllTests(tr);
289 }
290 /* ************************************************************************* */
key1
const Symbol key1('v', 1)
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::PolakRibiere
double PolakRibiere(const Gradient &currentGradient, const Gradient &prevGradient)
Polak-Ribiere formula for computing β, the direction of steepest descent.
Definition: NonlinearConjugateGradientOptimizer.h:40
rosenbrock::Rosenbrock1Factor::This
Rosenbrock1Factor This
Definition: testNonlinearConjugateGradientOptimizer.cpp:92
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:74
NonlinearConjugateGradientOptimizer.h
Simple non-linear optimizer that solves using non-preconditioned CG.
Pose2.h
2D Pose
rosenbrock::Rosenbrock1Factor::evaluateError
Vector evaluateError(const double &x, OptionalMatrixType H) const override
evaluate error
Definition: testNonlinearConjugateGradientOptimizer.cpp:103
rosenbrock::Rosenbrock2Factor
Factor for the second term of the Rosenbrock function. f2 = (y - x*x)
Definition: testNonlinearConjugateGradientOptimizer.cpp:117
gtsam::GaussianFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:82
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
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:114
test_constructor::f1
auto f1
Definition: testHybridNonlinearFactor.cpp:56
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
x4
static string x4("x4")
b
Scalar * b
Definition: benchVecAdd.cpp:17
rosenbrock::Rosenbrock2Factor::Base
NoiseModelFactorN< double, double > Base
Definition: testNonlinearConjugateGradientOptimizer.cpp:120
gtsam::Y
GaussianFactorGraphValuePair Y
Definition: HybridGaussianProductFactor.cpp:29
initial
Values initial
Definition: OdometryOptimize.cpp:2
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
priorMean
Pose2 priorMean(0.0, 0.0, 0.0)
rosenbrock::Rosenbrock2Factor::This
Rosenbrock2Factor This
Definition: testNonlinearConjugateGradientOptimizer.cpp:119
f2
double f2(const Vector2 &x)
Definition: testNumericalDerivative.cpp:56
gtsam::NonlinearOptimizerParams::verbosity
Verbosity verbosity
The printing verbosity during optimization (default SILENT)
Definition: NonlinearOptimizerParams.h:46
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
X
#define X
Definition: icosphere.cpp:20
gtsam::Factor
Definition: Factor.h:70
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
rosenbrock::Rosenbrock1Factor
Definition: testNonlinearConjugateGradientOptimizer.cpp:90
rosenbrock::GetRosenbrockGraph
static NonlinearFactorGraph GetRosenbrockGraph(double a=1.0, double b=100.0)
Get a nonlinear factor graph representing the Rosenbrock Banana function.
Definition: testNonlinearConjugateGradientOptimizer.cpp:148
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
rosenbrock::f
double f(const NonlinearFactorGraph &graph, double x, double y)
Definition: testNonlinearConjugateGradientOptimizer.cpp:161
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:306
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
Vector1
Eigen::Matrix< double, 1, 1 > Vector1
Definition: testEvent.cpp:33
PriorFactor.h
TEST
TEST(NonlinearConjugateGradientOptimizer, Optimize)
Definition: testNonlinearConjugateGradientOptimizer.cpp:72
key2
const Symbol key2('v', 2)
gtsam::NonlinearOptimizerParams
Definition: NonlinearOptimizerParams.h:35
BetweenFactor.h
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::DaiYuan
double DaiYuan(const Gradient &currentGradient, const Gradient &prevGradient, const Gradient &direction)
The Dai-Yuan formula for computing β, the direction of steepest descent.
Definition: NonlinearConjugateGradientOptimizer.h:63
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
main
int main()
Definition: testNonlinearConjugateGradientOptimizer.cpp:286
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
rosenbrock::Rosenbrock2Factor::Rosenbrock2Factor
Rosenbrock2Factor(Key key1, Key key2, const SharedNoiseModel &model=nullptr)
Definition: testNonlinearConjugateGradientOptimizer.cpp:124
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
rosenbrock::Rosenbrock1Factor::Base
NoiseModelFactorN< double > Base
Definition: testNonlinearConjugateGradientOptimizer.cpp:93
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
factorTesting.h
Evaluate derivatives of a nonlinear factor numerically.
rosenbrock::rosenbrock_func
double rosenbrock_func(double x, double y, double a=1.0, double b=100.0)
True Rosenbrock Banana function.
Definition: testNonlinearConjugateGradientOptimizer.cpp:170
x5
static string x5("x5")
Symbol.h
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
rosenbrock::Rosenbrock1Factor::Rosenbrock1Factor
Rosenbrock1Factor(Key key, double a, const SharedNoiseModel &model=nullptr)
Definition: testNonlinearConjugateGradientOptimizer.cpp:99
priorNoise
auto priorNoise
Definition: doc/Code/OdometryExample.cpp:6
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
TestResult
Definition: TestResult.h:26
y
Scalar * y
Definition: level1_cplx_impl.h:124
key
const gtsam::Symbol key('X', 0)
M_PI_2
#define M_PI_2
Definition: mconf.h:118
gtsam::NonlinearConjugateGradientOptimizer::optimize
const Values & optimize() override
Definition: NonlinearConjugateGradientOptimizer.cpp:82
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam::NonlinearConjugateGradientOptimizer
Definition: NonlinearConjugateGradientOptimizer.h:80
gtsam
traits
Definition: SFMdata.h:40
error
static double error
Definition: testRot3.cpp:37
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
odometryNoise
auto odometryNoise
Definition: doc/Code/OdometryExample.cpp:11
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::FletcherReeves
double FletcherReeves(const Gradient &currentGradient, const Gradient &prevGradient)
Fletcher-Reeves formula for computing β, the direction of steepest descent.
Definition: NonlinearConjugateGradientOptimizer.h:30
gtsam::NonlinearOptimizerParams::maxIterations
size_t maxIterations
The maximum iterations to stop iterating (default 100)
Definition: NonlinearOptimizerParams.h:42
initial
Definition: testScenarioRunner.cpp:148
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
rosenbrock::Rosenbrock1Factor::a_
double a_
Definition: testNonlinearConjugateGradientOptimizer.cpp:95
M_PI
#define M_PI
Definition: mconf.h:117
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
rosenbrock
Definition: testNonlinearConjugateGradientOptimizer.cpp:88
gtsam::HestenesStiefel
double HestenesStiefel(const Gradient &currentGradient, const Gradient &prevGradient, const Gradient &direction)
Definition: NonlinearConjugateGradientOptimizer.h:52
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
generateProblem
std::tuple< NonlinearFactorGraph, Values > generateProblem()
Definition: testNonlinearConjugateGradientOptimizer.cpp:26
rosenbrock::Rosenbrock2Factor::evaluateError
Vector evaluateError(const double &x, const double &y, OptionalMatrixType H1, OptionalMatrixType H2) const override
evaluate error
Definition: testNonlinearConjugateGradientOptimizer.cpp:128
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::Pose2
Definition: Pose2.h:39
gtsam::BetweenFactor
Definition: BetweenFactor.h:40


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:40:44