diff --git a/gtsam.h b/gtsam.h index 50743c239..13364b210 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1124,6 +1124,7 @@ class KeyVector { void push_back(size_t key) const; }; +#include class Marginals { Marginals(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& solution); @@ -1302,18 +1303,28 @@ class ISAM2 { //************************************************************************* // Nonlinear factor types //************************************************************************* +#include +#include +#include +#include +#include +#include + +#include template virtual class PriorFactor : gtsam::NonlinearFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); }; +#include template virtual class BetweenFactor : gtsam::NonlinearFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); }; +#include template virtual class NonlinearEquality : gtsam::NonlinearFactor { // Constructor - forces exact evaluation @@ -1323,6 +1334,7 @@ virtual class NonlinearEquality : gtsam::NonlinearFactor { }; +#include template virtual class RangeFactor : gtsam::NonlinearFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); @@ -1337,6 +1349,8 @@ typedef gtsam::RangeFactor RangeFactorSimple typedef gtsam::RangeFactor RangeFactorCalibratedCamera; typedef gtsam::RangeFactor RangeFactorSimpleCamera; + +#include template virtual class BearingFactor : gtsam::NonlinearFactor { BearingFactor(size_t key1, size_t key2, const ROT& measured, const gtsam::noiseModel::Base* noiseModel);