Go to the documentation of this file.
55 using namespace gtsam;
58 int main(
int argc,
char* argv[]) {
63 auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
88 for (
size_t i = 0;
i < poses.size(); ++
i) {
90 for (
size_t j = 0;
j < points.size(); ++
j) {
99 static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
100 Point3(0.05, -0.10, 0.20));
109 static auto kPosePrior = noiseModel::Diagonal::Sigmas(
110 (
Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
115 static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
120 static Point3 kDeltaPoint(-0.25, 0.20, 0.15);
121 for (
size_t j = 0;
j < points.size(); ++
j)
132 Values currentEstimate =
isam.calculateEstimate();
133 cout <<
"****************************************************" << endl;
134 cout <<
"Frame " <<
i <<
": " << endl;
135 currentEstimate.
print(
"Current estimate: ");
139 initialEstimate.
clear();
Reprojection of a LANDMARK to a 2D point.
std::vector< Point3 > createPoints()
Create a set of ground-truth landmarks.
void update(const NonlinearFactorGraph &newFactors, const Values &initialValues)
virtual void resize(size_t size)
int main(int argc, char *argv[])
static ConjugateGradientParameters parameters
NonlinearISAM isam(relinearizeInterval)
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
std::shared_ptr< Cal3_S2 > shared_ptr
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
std::vector< Pose3 > createPoses(const Pose3 &init=Pose3(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {30, 0, 0}), const Pose3 &delta=Pose3(Rot3::Ypr(0, -M_PI_4, 0), {sin(M_PI_4) *30, 0, 30 *(1 - sin(M_PI_4))}), int steps=8)
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Factor Graph consisting of non-linear factors.
void insert(Key j, const Value &val)
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 Nov 16 2024 04:09:43