Go to the documentation of this file.
25 using namespace gtsam;
48 SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(
51 constraintUncertainty);
56 initialEstimate.insert(1,
x1);
58 initialEstimate.insert(2,
x2);
60 initialEstimate.insert(3,
x3);
62 initialEstimate.insert(4,
x4);
64 initialEstimate.insert(5,
x5);
66 return {
graph, initialEstimate};
76 param.
verbosity = NonlinearOptimizerParams::SILENT;
static int runAllTests(TestResult &result)
Simple non-linear optimizer that solves using non-preconditioned CG.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Pose2 priorMean(0.0, 0.0, 0.0)
Verbosity verbosity
The printing verbosity during optimization (default SILENT)
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
double error(const Values &values) const
noiseModel::Diagonal::shared_ptr SharedDiagonal
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
TEST(NonlinearConjugateGradientOptimizer, Optimize)
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
const Values & optimize() override
Factor Graph consisting of non-linear factors.
size_t maxIterations
The maximum iterations to stop iterating (default 100)
NonlinearFactorGraph graph
std::tuple< NonlinearFactorGraph, Values > generateProblem()
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
A non-templated config holding any types of Manifold-group elements.
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
gtsam
Author(s):
autogenerated on Tue Jun 25 2024 03:05:57