diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index cac55563f..f1ff36c1e 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -29,19 +29,18 @@ namespace gtsam { /** * Factor that supports arbitrary expressions via AD */ -template +template class ExpressionFactor: public NoiseModelFactor { protected: typedef ExpressionFactor This; - - T measurement_; ///< the measurement to be compared with the expression - Expression expression_; ///< the expression that is AD enabled - FastVector dims_; ///< dimensions of the Jacobian matrices - static const int Dim = traits::dimension; + T measurement_; ///< the measurement to be compared with the expression + Expression expression_; ///< the expression that is AD enabled + FastVector dims_; ///< dimensions of the Jacobian matrices + public: typedef boost::shared_ptr > shared_ptr; @@ -62,6 +61,19 @@ public: boost::tie(keys_, dims_) = expression_.keysAndDims(); } + /// print relies on Testable traits being defined for T + void print(const std::string& s, const KeyFormatter& keyFormatter) const { + NoiseModelFactor::print(s, keyFormatter); + traits::Print(measurement_, s + ".measurement_"); + } + + /// equals relies on Testable traits being defined for T + bool equals(const NonlinearFactor& f, double tol) const { + const ExpressionFactor* p = dynamic_cast(&f); + return p && NoiseModelFactor::equals(f, tol) && + traits::Equals(measurement_, p->measurement_, tol); + } + /** * Error function *without* the NoiseModel, \f$ h(x)-z \f$. * We override this method to provide @@ -119,8 +131,17 @@ public: return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + protected: + /// Default constructor, for serialization + ExpressionFactor() {} + }; // ExpressionFactor +/// traits +template +struct traits > : public Testable > {}; + }// \ namespace gtsam