Go to the documentation of this file.
172 inline std::shared_ptr<const NonlinearFactorGraph>
175 using namespace impl;
224 using namespace impl;
235 std::shared_ptr<Values>
c(
new Values);
298 using namespace impl;
299 Matrix R11 = (
Matrix(1, 1) << 1.0).finished(), S12 = (
Matrix(1, 1) << 1.0).finished();
318 namespace smallOptimize {
327 0.0,
cos(
v.y())).finished();
333 using gtsam::NoiseModelFactor1<Point2>::evaluateError;
347 return std::static_pointer_cast<gtsam::NonlinearFactor>(
359 std::shared_ptr<smallOptimize::UnaryFactor> factor(
361 fg->push_back(factor);
372 std::shared_ptr<smallOptimize::UnaryFactor> factor(
374 fg->push_back(factor);
389 std::shared_ptr<PriorFactor<Point2>> factor(
392 fg->push_back(factor);
393 fg->push_back(factor);
394 fg->push_back(factor);
398 std::shared_ptr<PriorFactor<Point2>> factor_out(
400 fg->push_back(factor_out);
413 std::shared_ptr<PriorFactor<Point2>> factor(
416 fg->push_back(factor);
417 fg->push_back(factor);
418 fg->push_back(factor);
422 std::shared_ptr<PriorFactor<Point2>> factor_out(
424 fg->push_back(factor_out);
432 using namespace impl;
446 for (
int t = 2;
t <=
T;
t++) {
461 return std::make_pair(nlfg, poses);
467 return *nlfg.linearize(poses);
472 using namespace impl;
501 using namespace impl;
513 using namespace impl;
547 using namespace impl;
554 using namespace impl;
609 using namespace impl;
626 inline std::pair<GaussianFactorGraph, VectorValues>
planarGraph(
size_t N) {
627 using namespace impl;
638 for (
size_t x = 1;
x <
N;
x++)
639 for (
size_t y = 1;
y <=
N;
y++) {
646 for (
size_t x = 1;
x <=
N;
x++)
647 for (
size_t y = 1;
y <
N;
y++) {
654 for (
size_t x = 1;
x <=
N;
x++)
655 for (
size_t y = 1;
y <=
N;
y++)
658 for (
size_t x = 1;
x <=
N;
x++)
659 for (
size_t y = 1;
y <=
N;
y++)
663 std::shared_ptr<GaussianFactorGraph> gfg = nlfg.
linearize(zeros);
664 return std::make_pair(*gfg, xtrue);
670 for (
size_t y =
N;
y >= 1;
y--)
671 for (
size_t x =
N;
x >= 1;
x--)
682 T.push_back(original[0]);
686 for (
size_t x = 1;
x <
N;
x++)
687 for (
size_t y = 1;
y <=
N;
y++,
i++)
T.push_back(original[
i]);
690 for (
size_t x = 1;
x <=
N;
x++)
691 for (
size_t y = 1;
y <
N;
y++,
i++)
693 T.push_back(original[
i]);
695 C.push_back(original[
i]);
697 return std::make_pair(
T,
C);
Values createNoisyValues()
std::shared_ptr< This > shared_ptr
std::shared_ptr< const NonlinearFactorGraph > sharedNonlinearFactorGraph(const SharedNoiseModel &noiseModel1=impl::kSigma0_1, const SharedNoiseModel &noiseModel2=impl::kSigma0_2)
Linear Factor Graph where all factors are Gaussians.
std::shared_ptr< NonlinearFactor > shared_nlf
Matrix H(const Point2 &v)
GaussianFactorGraph createMultiConstraintGraph()
Array< double, 1, 3 > e(1./3., 0.5, 2.)
NonlinearFactorGraph nonlinearFactorGraphWithGivenSigma(const double sigma)
Jet< T, N > sin(const Jet< T, N > &f)
iterator insert(const std::pair< Key, Vector > &key_value)
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
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
std::shared_ptr< const NonlinearFactorGraph > sharedReallyNonlinearFactorGraph()
VectorValues createMultiConstraintValues()
std::shared_ptr< NonlinearFactor > shared_nlf
gtsam::NonlinearFactor::shared_ptr clone() const override
double f2(const Vector2 &x)
Eigen::Triplet< double > T
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
VectorValues createVectorValues()
Chordal Bayes Net, the result of eliminating a factor graph.
Vector evaluateError(const Point2 &x, OptionalMatrixType A) const override
std::pair< GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N, const GaussianFactorGraph &original)
Jet< T, N > cos(const Jet< T, N > &f)
static const double sigma
This is the base class for all measurement types.
Pose2 odometry(2.0, 0.0, 0.0)
Symbol key(size_t x, size_t y)
NonlinearFactorGraph createNonlinearFactorGraph(const SharedNoiseModel &noiseModel1=impl::kSigma0_1, const SharedNoiseModel &noiseModel2=impl::kSigma0_2)
static SharedDiagonal kSigma0_2
GaussianFactorGraph createGaussianFactorGraph()
GaussianBayesNet createSmallGaussianBayesNet()
GaussianFactorGraph createSimpleConstraintGraph()
noiseModel::Diagonal::shared_ptr SharedDiagonal
Point2 h(const Point2 &v)
static shared_ptr All(size_t dim)
std::shared_ptr< const Values > sharedNoisyValues()
Ordering planarOrdering(size_t N)
noiseModel::Base::shared_ptr SharedNoiseModel
static const double A12[]
std::pair< NonlinearFactorGraph, Values > createNonlinearSmoother(int T)
noiseModel::Diagonal::shared_ptr model
static enum @1096 ordering
measurement functions and derivatives for simulated 2D robot
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
GaussianFactorGraph createSingleConstraintGraph()
std::pair< GaussianFactorGraph, VectorValues > planarGraph(size_t N)
static SharedDiagonal kSigma0_1
Matrix< Scalar, Dynamic, Dynamic > C
static SharedDiagonal kConstrainedModel
static SharedDiagonal kSigma1_0
Factor Graph consisting of non-linear factors.
double f3(double x1, double x2)
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers()
VectorValues createSimpleConstraintValues()
VectorValues createSingleConstraintValues()
Point2 odo(const Point2 &x1, const Point2 &x2)
odometry between two poses
NonlinearFactorGraph sharedRobustFactorGraphWithOutliers()
void insert(Key j, const Value &val)
std::shared_ptr< This > shared_ptr
shared_ptr to this class
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Array< int, Dynamic, 1 > v
UnaryFactor(const Point2 &z, const SharedNoiseModel &model, Key key)
double f4(double x, double y, double z)
static const double A11[]
std::shared_ptr< This > shared_ptr
shared_ptr to this class
NonlinearFactorGraph createReallyNonlinearFactorGraph()
Matrix * OptionalMatrixType
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
VectorValues createZeroDelta()
std::uint64_t Key
Integer nonlinear key type.
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
VectorValues createCorrectDelta()
static Point2 measurement(323.0, 240.0)
GaussianFactorGraph createSmoother(int T)
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:21