diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 1b630374c..9c5df7ea0 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -74,25 +74,26 @@ namespace gtsam { std::cout << s << "BetweenFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; - measured_.print(" measured: "); + traits::print()(measured_, " measured: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); + return e != NULL && Base::equals(*e, tol) && traits::equals()(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors */ Vector evaluateError(const T& p1, const T& p2, - boost::optional H1 = boost::none, boost::optional H2 = + boost::optional H1 = boost::none,boost::optional H2 = boost::none) const { T hx = p1.between(p2, H1, H2); // h(x) + DefaultChart chart; // manifold equivalent of h(x)-z -> log(z,h(x)) - return measured_.localCoordinates(hx); + return chart.local(measured_, hx); } /** return the measured */ @@ -129,7 +130,9 @@ namespace gtsam { /** Syntactic sugar for constrained version */ BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) : - BetweenFactor(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {} + BetweenFactor(key1, key2, measured, + noiseModel::Constrained::All(DefaultChart::getDimension(measured), fabs(mu))) + {} private: diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 888dcb76b..8fbbd4d88 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -67,23 +67,24 @@ namespace gtsam { /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; - prior_.print(" prior mean: "); + traits::print()(prior_, " prior mean: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This* e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol); + return e != NULL && Base::equals(*e, tol) && traits::equals()(prior_, e->prior_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { - if (H) (*H) = eye(p.dim()); + DefaultChart chart; + if (H) (*H) = eye(chart.getDimension(p)); // manifold equivalent of h(x)-z -> log(z,h(x)) - return prior_.localCoordinates(p); + return chart.local(prior_,p); } const VALUE & prior() const { return prior_; } diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index d0dbe7908..b22763099 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -44,6 +44,30 @@ TEST(BetweenFactor, Rot3) { EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } +/* ************************************************************************* */ +/* +// Constructor scalar +TEST(BetweenFactor, ConstructorScalar) { + SharedNoiseModel model; + double measured_value = 0.0; + BetweenFactor factor(1, 2, measured_value, model); +} + +// Constructor vector3 +TEST(BetweenFactor, ConstructorVector3) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + Vector3 measured_value(1, 2, 3); + BetweenFactor factor(1, 2, measured_value, model); +} + +// Constructor dynamic sized vector +TEST(BetweenFactor, ConstructorDynamicSizeVector) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); + Vector measured_value(5); measured_value << 1, 2, 3, 4, 5; + BetweenFactor factor(1, 2, measured_value, model); +} +*/ + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp index b3e54bedb..b26d633f5 100644 --- a/gtsam/slam/tests/testPriorFactor.cpp +++ b/gtsam/slam/tests/testPriorFactor.cpp @@ -5,8 +5,8 @@ * @date Nov 4, 2014 */ +#include #include -#include #include using namespace std; @@ -14,10 +14,23 @@ using namespace gtsam; /* ************************************************************************* */ -// Constructor -TEST(PriorFactor, Constructor) { +// Constructor scalar +TEST(PriorFactor, ConstructorScalar) { SharedNoiseModel model; - PriorFactor factor(1, LieScalar(1.0), model); + PriorFactor factor(1, 1.0, model); +} + +// Constructor vector3 +TEST(PriorFactor, ConstructorVector3) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + PriorFactor factor(1, Vector3(1,2,3), model); +} + +// Constructor dynamic sized vector +TEST(PriorFactor, ConstructorDynamicSizeVector) { + Vector v(5); v << 1, 2, 3, 4, 5; + SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); + PriorFactor factor(1, v, model); } /* ************************************************************************* */