Go to the documentation of this file.
32 using namespace gtsam;
39 auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
56 for (
size_t i = 0;
i < poses.size(); ++
i)
59 for (
size_t j = 0;
j < points.size(); ++
j)
69 static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
70 Point3(0.05, -0.10, 0.20));
77 static auto kPosePrior = noiseModel::Diagonal::Sigmas(
78 (
Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
83 static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
88 static Point3 kDeltaPoint(-0.25, 0.20, 0.15);
89 for (
size_t j = 0;
j < points.size(); ++
j)
101 Values currentEstimate =
isam.calculateEstimate();
105 initialEstimate.
clear();
111 for (
size_t j = 0;
j < points.size(); ++
j)
static int runAllTests(TestResult &result)
std::vector< gtsam::Pose3 > createPoses(const gtsam::Pose3 &init=gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2, 0,-M_PI/2), gtsam::Point3(30, 0, 0)), const gtsam::Pose3 &delta=gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4, 0), gtsam::Point3(sin(M_PI/4) *30, 0, 30 *(1-sin(M_PI/4)))), int steps=8)
Reprojection of a LANDMARK to a 2D point.
#define EXPECT_LONGS_EQUAL(expected, actual)
#define EXPECT(condition)
std::vector< gtsam::Point3 > createPoints()
void update(const NonlinearFactorGraph &newFactors, const Values &initialValues)
const ValueType at(Key j) const
virtual void resize(size_t size)
static ConjugateGradientParameters parameters
static const Eigen::internal::all_t all
NonlinearISAM isam(relinearizeInterval)
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
std::shared_ptr< Cal3_S2 > shared_ptr
const gtsam::Symbol key('X', 0)
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
TEST(testVisualISAM2, all)
Factor Graph consisting of non-linear factors.
void insert(Key j, const Value &val)
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
The most common 5DOF 3D->2D calibration.
static const CalibratedCamera camera(kDefaultPose)
NonlinearFactorGraph graph
Simple example for the structure-from-motion problems.
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
static Point2 measurement(323.0, 240.0)
A non-templated config holding any types of Manifold-group elements.
Point2 project(const Point3 &point, OptionalJacobian< 2, 6 > Dcamera={}, OptionalJacobian< 2, 3 > Dpoint={}) const
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
gtsam
Author(s):
autogenerated on Sat Sep 28 2024 03:06:41