29 #include <boost/shared_ptr.hpp> 30 #include <boost/assign/std/list.hpp> 37 using namespace gtsam;
50 0., 0., 1.).finished();
55 0., 0., 1.).finished();
65 Matrix Rhat = AtA.llt().matrixL().transpose();
79 xhat = Radapter.transpose().triangularView<
Eigen::Upper>().solve(b);
92 simpleGFG +=
JacobianFactor(2, (
Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (
Matrix(2,2)<< 10, 0, 0, 10).finished(), (
Vector(2) << 2, -1).finished(), unit2);
93 simpleGFG +=
JacobianFactor(2, (
Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (
Matrix(2,2)<< 5, 0, 0, 5).finished(), (
Vector(2) << 0, 1).finished(), unit2);
94 simpleGFG +=
JacobianFactor(0, (
Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (
Matrix(2,2)<< 5, 0, 0, 5).finished(), (
Vector(2) << -1, 1.5).finished(), unit2);
102 std::map<Key,Vector>
lambda;
103 dummyPreconditioner.
build(simpleGFG, keyInfo, lambda);
108 initial = (
Vector(6) << 0., 0., 0., 0., 0., 0.).finished();
117 Vector expectedAp = (
Vector(6) << 100400, -249074.074, -2080, 148148.148, -146480, 37962.963).finished();
121 Vector expectedb = (
Vector(6) << 100.0, -194.444, -20.0, 138.889, -120.0, -55.556).finished();
132 auto pcg = boost::make_shared<PCGSolverParameters>();
133 pcg->preconditioner_ = boost::make_shared<DummyPreconditionerParameters>();
152 auto pcg = boost::make_shared<PCGSolverParameters>();
153 pcg->preconditioner_ =
154 boost::make_shared<BlockJacobiPreconditionerParameters>();
173 auto pcg = boost::make_shared<PCGSolverParameters>();
174 pcg->preconditioner_ = boost::make_shared<SubgraphPreconditionerParameters>();
void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda) override
build/factorize the preconditioner
virtual const Values & optimize()
static int runAllTests(TestResult &result)
void insert(Key j, const Value &val)
A matrix or vector expression mapping an existing array of data.
#define DOUBLES_EQUAL(expected, actual, threshold)
NonlinearFactorGraph createReallyNonlinearFactorGraph()
Rot2 R(Rot2::fromAngle(0.1))
void leftPrecondition(const Vector &x, Vector &y) const
void residual(const Vector &x, Vector &r) const
Matrix2 transpose() const
#define EXPECT(condition)
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Linear Factor Graph where all factors are Gaussians.
void multiply(const Vector &x, Vector &y) const
IterativeOptimizationParameters::shared_ptr iterativeParams
The container for iterativeOptimization parameters. used in CG Solvers.
static SmartStereoProjectionParams params
cout<< "The eigenvalues of A are:"<< endl<< ces.eigenvalues()<< endl;cout<< "The matrix of eigenvectors, V, is:"<< endl<< ces.eigenvectors()<< endl<< endl;complex< float > lambda
static SharedNoiseModel unit2(noiseModel::Unit::Create(2))
noiseModel::Diagonal::shared_ptr SharedDiagonal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
void getb(Vector &b) const
Create small example with two poses and one landmark.
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
double error(const Values &values) const
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
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
void rightPrecondition(const Vector &x, Vector &y) const
LinearSolverType linearSolverType
The type of linear solver to use in the nonlinear optimizer.