29 #include <boost/assign/list_of.hpp> 173 inline boost::shared_ptr<const NonlinearFactorGraph>
176 using namespace impl;
225 using namespace impl;
237 boost::shared_ptr<Values>
c(
new Values);
238 c->insert(
X(1),
Point2(0.1, 0.1));
239 c->insert(
X(2),
Point2(1.4, 0.2));
240 c->insert(
L(1),
Point2(0.1, -1.1));
300 using namespace impl;
301 Matrix R11 = (
Matrix(1, 1) << 1.0).finished(), S12 = (
Matrix(1, 1) << 1.0).finished();
320 namespace smallOptimize {
329 0.0,
cos(v.y())).finished();
358 boost::shared_ptr<smallOptimize::UnaryFactor> factor(
360 fg->push_back(factor);
371 boost::shared_ptr<smallOptimize::UnaryFactor> factor(
373 fg->push_back(factor);
388 boost::shared_ptr<PriorFactor<Point2>> factor(
391 fg->push_back(factor);
392 fg->push_back(factor);
393 fg->push_back(factor);
397 boost::shared_ptr<PriorFactor<Point2>> factor_out(
399 fg->push_back(factor_out);
412 boost::shared_ptr<PriorFactor<Point2>> factor(
415 fg->push_back(factor);
416 fg->push_back(factor);
417 fg->push_back(factor);
421 boost::shared_ptr<PriorFactor<Point2>> factor_out(
423 fg->push_back(factor_out);
431 using namespace impl;
445 for (
int t = 2;
t <=
T;
t++) {
460 return std::make_pair(nlfg, poses);
474 using namespace impl;
503 using namespace impl;
515 using namespace impl;
549 using namespace impl;
550 VectorValues config = boost::assign::pair_list_of<Key, Vector>
558 using namespace impl;
613 using namespace impl;
614 VectorValues config = boost::assign::pair_list_of<Key, Vector>
631 inline std::pair<GaussianFactorGraph, VectorValues>
planarGraph(
size_t N) {
632 using namespace impl;
643 for (
size_t x = 1;
x <
N;
x++)
644 for (
size_t y = 1;
y <=
N;
y++) {
651 for (
size_t x = 1;
x <=
N;
x++)
652 for (
size_t y = 1;
y <
N;
y++) {
659 for (
size_t x = 1;
x <=
N;
x++)
660 for (
size_t y = 1;
y <=
N;
y++)
663 for (
size_t x = 1;
x <=
N;
x++)
664 for (
size_t y = 1;
y <=
N;
y++)
668 boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.
linearize(zeros);
669 return std::make_pair(*gfg, xtrue);
675 for (
size_t y = N;
y >= 1;
y--)
676 for (
size_t x = N;
x >= 1;
x--)
682 inline std::pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr >
splitOffPlanarTree(
size_t N,
684 auto T = boost::make_shared<GaussianFactorGraph>(),
C= boost::make_shared<GaussianFactorGraph>();
687 T->push_back(original[0]);
691 for (
size_t x = 1;
x <
N;
x++)
692 for (
size_t y = 1;
y <=
N;
y++, i++)
693 T->push_back(original[i]);
696 for (
size_t x = 1;
x <=
N;
x++)
697 for (
size_t y = 1;
y <
N;
y++, i++)
699 T->push_back(original[i]);
701 C->push_back(original[i]);
703 return std::make_pair(
T,
C);
GaussianFactorGraph createMultiConstraintGraph()
VectorValues createVectorValues()
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
VectorValues createMultiConstraintValues()
Values createNoisyValues()
static SharedDiagonal kSigma1_0
GaussianFactorGraph createSmoother(int T)
This is the base class for all measurement types.
Factor Graph consisting of non-linear factors.
GaussianFactorGraph createGaussianFactorGraph()
Point2 prior(const Point2 &x)
Prior on a single pose.
noiseModel::Diagonal::shared_ptr model
void insert(Key j, const Value &val)
Vector evaluateError(const Point2 &x, boost::optional< Matrix & > A=boost::none) const override
static enum @843 ordering
Point2 odo(const Point2 &x1, const Point2 &x2)
odometry between two poses
NonlinearFactorGraph sharedRobustFactorGraphWithOutliers()
static const double sigma
NonlinearFactorGraph createReallyNonlinearFactorGraph()
NonlinearFactorGraph nonlinearFactorGraphWithGivenSigma(const double sigma)
VectorValues createSingleConstraintValues()
double f2(const Vector2 &x)
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
Symbol key(size_t x, size_t y)
static SharedDiagonal kConstrainedModel
gtsam::NonlinearFactor::shared_ptr clone() const override
boost::shared_ptr< NonlinearFactor > shared_nlf
EIGEN_DEVICE_FUNC const CosReturnType cos() const
VectorValues createCorrectDelta()
static SharedDiagonal kSigma0_1
GaussianBayesNet createSmallGaussianBayesNet()
boost::shared_ptr< const Values > sharedNoisyValues()
boost::shared_ptr< const NonlinearFactorGraph > sharedReallyNonlinearFactorGraph()
std::pair< NonlinearFactorGraph, Values > createNonlinearSmoother(int T)
Matrix H(const Point2 &v)
boost::shared_ptr< This > shared_ptr
Eigen::Triplet< double > T
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
VectorValues createZeroDelta()
NonlinearFactorGraph createNonlinearFactorGraph(const SharedNoiseModel &noiseModel1=impl::kSigma0_1, const SharedNoiseModel &noiseModel2=impl::kSigma0_2)
Linear Factor Graph where all factors are Gaussians.
GaussianFactorGraph createSimpleConstraintGraph()
Matrix< Scalar, Dynamic, Dynamic > C
Ordering planarOrdering(size_t N)
noiseModel::Diagonal::shared_ptr SharedDiagonal
std::pair< GaussianFactorGraph, VectorValues > planarGraph(size_t N)
UnaryFactor(const Point2 &z, const SharedNoiseModel &model, Key key)
measurement functions and derivatives for simulated 2D robot
std::pair< GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr > splitOffPlanarTree(size_t N, const GaussianFactorGraph &original)
VectorValues createSimpleConstraintValues()
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
double f4(double x, double y, double z)
static SharedDiagonal kSigma0_2
boost::shared_ptr< NonlinearFactor > shared_nlf
double f3(double x1, double x2)
boost::shared_ptr< const NonlinearFactorGraph > sharedNonlinearFactorGraph(const SharedNoiseModel &noiseModel1=impl::kSigma0_1, const SharedNoiseModel &noiseModel2=impl::kSigma0_2)
Chordal Bayes Net, the result of eliminating a factor graph.
EIGEN_DEVICE_FUNC const SinReturnType sin() const
Pose2 odometry(2.0, 0.0, 0.0)
NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers()
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
boost::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
GaussianFactorGraph createSingleConstraintGraph()
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
iterator insert(const std::pair< Key, Vector > &key_value)
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
std::uint64_t Key
Integer nonlinear key type.
noiseModel::Base::shared_ptr SharedNoiseModel
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
static shared_ptr All(size_t dim)