diff --git a/.cproject b/.cproject index 7d302b39a..0665eaf06 100644 --- a/.cproject +++ b/.cproject @@ -600,7 +600,6 @@ make - tests/testBayesTree.run true false @@ -608,7 +607,6 @@ make - testBinaryBayesNet.run true false @@ -656,7 +654,6 @@ make - testSymbolicBayesNet.run true false @@ -664,7 +661,6 @@ make - tests/testSymbolicFactor.run true false @@ -672,7 +668,6 @@ make - testSymbolicFactorGraph.run true false @@ -688,7 +683,6 @@ make - tests/testBayesTree true false @@ -1040,7 +1034,6 @@ make - testErrors.run true false @@ -1270,46 +1263,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1392,6 +1345,7 @@ make + testSimulated2DOriented.run true false @@ -1431,6 +1385,7 @@ make + testSimulated2D.run true false @@ -1438,6 +1393,7 @@ make + testSimulated3D.run true false @@ -1451,6 +1407,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1708,7 +1704,6 @@ cpack - -G DEB true false @@ -1716,7 +1711,6 @@ cpack - -G RPM true false @@ -1724,7 +1718,6 @@ cpack - -G TGZ true false @@ -1732,7 +1725,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2459,7 +2451,6 @@ make - testGraph.run true false @@ -2467,7 +2458,6 @@ make - testJunctionTree.run true false @@ -2475,7 +2465,6 @@ make - testSymbolicBayesNetB.run true false @@ -2561,6 +2550,14 @@ true true + + make + -j5 + testExpressionMeta.run + true + true + true + make -j2 @@ -2963,6 +2960,7 @@ make + tests/testGaussianISAM2 true false diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index d2e1febd0..08b131152 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -72,24 +72,24 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const { && noiseModel_->equals(*e->noiseModel_, tol))); } -static void check(const SharedNoiseModel& noiseModel, const Vector& b) { +static void check(const SharedNoiseModel& noiseModel, size_t m) { if (!noiseModel) throw std::invalid_argument("NoiseModelFactor: no NoiseModel."); - if ((size_t) b.size() != noiseModel->dim()) + if (m != noiseModel->dim()) throw std::invalid_argument( "NoiseModelFactor was created with a NoiseModel of incorrect dimension."); } Vector NoiseModelFactor::whitenedError(const Values& c) const { const Vector b = unwhitenedError(c); - check(noiseModel_, b); + check(noiseModel_, b.size()); return noiseModel_->whiten(b); } double NoiseModelFactor::error(const Values& c) const { if (this->active(c)) { const Vector b = unwhitenedError(c); - check(noiseModel_, b); + check(noiseModel_, b.size()); return 0.5 * noiseModel_->distance(b); } else { return 0.0; @@ -106,7 +106,7 @@ boost::shared_ptr NoiseModelFactor::linearize( // Call evaluate error to get Jacobians and RHS vector b std::vector A(this->size()); Vector b = -unwhitenedError(x, A); - check(noiseModel_, b); + check(noiseModel_, b.size()); // Whiten the corresponding system now this->noiseModel_->WhitenSystem(A, b); @@ -124,7 +124,7 @@ boost::shared_ptr NoiseModelFactor::linearize( boost::dynamic_pointer_cast(this->noiseModel_); if (constrained) { // Create a factor of reduced row dimension d_ - size_t d_ = terms[0].second.rows() - constrained->dim(); + size_t d_ = b.size() - constrained->dim(); Vector zero_ = zero(d_); Vector b_ = concatVectors(2, &b, &zero_); noiseModel::Constrained::shared_ptr model = constrained->unit(d_); diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index c40dfb405..3d619b5b4 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -24,8 +24,8 @@ #include #include #include -#include // for placement new -struct TestBinaryExpression; +#include +#include // template meta-programming headers #include @@ -35,15 +35,21 @@ struct TestBinaryExpression; #include #include #include - +#include +#include namespace MPL = boost::mpl::placeholders; +#include // for placement new + +class ExpressionFactorBinaryTest; +// Forward declare for testing + namespace gtsam { template class Expression; -typedef std::map JacobianMap; +typedef std::map > JacobianMap; //----------------------------------------------------------------------------- /** @@ -55,6 +61,7 @@ typedef std::map JacobianMap; */ template struct CallRecord { + static size_t const N = 0; virtual void print(const std::string& indent) const { } virtual void startReverseAD(JacobianMap& jacobians) const { @@ -79,7 +86,7 @@ template class ExecutionTrace { enum { Constant, Leaf, Function - } type; + } kind; union { Key key; CallRecord* ptr; @@ -87,25 +94,25 @@ class ExecutionTrace { public: /// Pointer always starts out as a Constant ExecutionTrace() : - type(Constant) { + kind(Constant) { } /// Change pointer to a Leaf Record void setLeaf(Key key) { - type = Leaf; + kind = Leaf; content.key = key; } /// Take ownership of pointer to a Function Record void setFunction(CallRecord* record) { - type = Function; + kind = Function; content.ptr = record; } /// Print void print(const std::string& indent = "") const { - if (type == Constant) + if (kind == Constant) std::cout << indent << "Constant" << std::endl; - else if (type == Leaf) + else if (kind == Leaf) std::cout << indent << "Leaf, key = " << content.key << std::endl; - else if (type == Function) { + else if (kind == Function) { std::cout << indent << "Function" << std::endl; content.ptr->print(indent + " "); } @@ -113,54 +120,64 @@ public: /// Return record pointer, quite unsafe, used only for testing template boost::optional record() { - if (type != Function) + if (kind != Function) return boost::none; else { Record* p = dynamic_cast(content.ptr); return p ? boost::optional(p) : boost::none; } } - // *** This is the main entry point for reverseAD, called from Expression::augmented *** - // Called only once, either inserts identity into Jacobians (Leaf) or starts AD (Function) + /// reverseAD in case of Leaf + template + static void handleLeafCase(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + JacobianMap::iterator it = jacobians.find(key); + if (it == jacobians.end()) { + std::cout << "No block for key " << key << std::endl; + throw std::runtime_error("Reverse AD internal error"); + } + // we have pre-loaded them with zeros + Eigen::Block& block = it->second; + block += dTdA; + } + /** + * *** This is the main entry point for reverseAD, called from Expression *** + * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) + */ void startReverseAD(JacobianMap& jacobians) const { - if (type == Leaf) { + if (kind == Leaf) { // This branch will only be called on trivial Leaf expressions, i.e. Priors size_t n = T::Dim(); - jacobians[content.key] = Eigen::MatrixXd::Identity(n, n); - } else if (type == Function) + handleLeafCase(Eigen::MatrixXd::Identity(n, n), jacobians, content.key); + } else if (kind == Function) // This is the more typical entry point, starting the AD pipeline - // It is inside the startReverseAD that the correctly dimensioned pipeline is chosen. + // Inside the startReverseAD that the correctly dimensioned pipeline is chosen. content.ptr->startReverseAD(jacobians); } // Either add to Jacobians (Leaf) or propagate (Function) void reverseAD(const Matrix& dTdA, JacobianMap& jacobians) const { - if (type == Leaf) { - JacobianMap::iterator it = jacobians.find(content.key); - if (it != jacobians.end()) - it->second += dTdA; - else - jacobians[content.key] = dTdA; - } else if (type == Function) + if (kind == Leaf) + handleLeafCase(dTdA, jacobians, content.key); + else if (kind == Function) content.ptr->reverseAD(dTdA, jacobians); } // Either add to Jacobians (Leaf) or propagate (Function) typedef Eigen::Matrix Jacobian2T; void reverseAD2(const Jacobian2T& dTdA, JacobianMap& jacobians) const { - if (type == Leaf) { - JacobianMap::iterator it = jacobians.find(content.key); - if (it != jacobians.end()) - it->second += dTdA; - else - jacobians[content.key] = dTdA; - } else if (type == Function) + if (kind == Leaf) + handleLeafCase(dTdA, jacobians, content.key); + else if (kind == Function) content.ptr->reverseAD2(dTdA, jacobians); } + + /// Define type so we can apply it as a meta-function + typedef ExecutionTrace type; }; /// Primary template calls the generic Matrix reverseAD pipeline -template +template struct Select { - typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix Jacobian; static void reverseAD(const ExecutionTrace& trace, const Jacobian& dTdA, JacobianMap& jacobians) { trace.reverseAD(dTdA, jacobians); @@ -177,208 +194,6 @@ struct Select<2, A> { } }; -//----------------------------------------------------------------------------- -/** - * Record the evaluation of a single argument in a functional expression - * Building block for Recursive Record Class - */ -template -struct Argument { - typedef Eigen::Matrix JacobianTA; - ExecutionTrace trace; - JacobianTA dTdA; -}; - -/** - * Recursive Record Class for Functional Expressions - * Abrahams, David; Gurtovoy, Aleksey (2004-12-10). - * C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost - * and Beyond. Pearson Education. - */ -template -struct Record: Argument, More { - - typedef T return_type; - typedef typename AN::type A; - const static size_t N = AN::value; - typedef Argument This; - - /// Print to std::cout - virtual void print(const std::string& indent) const { - More::print(indent); - static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); - std::cout << This::dTdA.format(matlab) << std::endl; - This::trace.print(indent); - } - - /// Start the reverse AD process - virtual void startReverseAD(JacobianMap& jacobians) const { - More::startReverseAD(jacobians); - Select::reverseAD(This::trace, This::dTdA, jacobians); - } - - /// Given df/dT, multiply in dT/dA and continue reverse AD process - virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { - More::reverseAD(dFdT, jacobians); - This::trace.reverseAD(dFdT * This::dTdA, jacobians); - } - - /// Version specialized to 2-dimensional output - typedef Eigen::Matrix Jacobian2T; - virtual void reverseAD2(const Jacobian2T& dFdT, - JacobianMap& jacobians) const { - More::reverseAD2(dFdT, jacobians); - This::trace.reverseAD2(dFdT * This::dTdA, jacobians); - } -}; - -/// Meta-function for generating a numbered type -template -struct Numbered { - typedef A type; - typedef size_t value_type; - static const size_t value = N; -}; - -/// Recursive Record class Generator -template -struct GenerateRecord { - typedef typename boost::mpl::fold, - Record >::type type; -}; - -/// Access Argument -template -Argument& argument(Record& record) { - return static_cast&>(record); -} - -/// Access Trace -template -ExecutionTrace& getTrace(Record* record) { - return argument(*record).trace; -} - -/// Access Jacobian -template -Eigen::Matrix& jacobian( - Record* record) { - return argument(*record).dTdA; -} - -//----------------------------------------------------------------------------- -/** - * Value and Jacobians - */ -template -class Augmented { - -private: - - T value_; - JacobianMap jacobians_; - - typedef std::pair Pair; - - /// Insert terms into jacobians_, adding if already exists - void add(const JacobianMap& terms) { - BOOST_FOREACH(const Pair& term, terms) { - JacobianMap::iterator it = jacobians_.find(term.first); - if (it != jacobians_.end()) - it->second += term.second; - else - jacobians_[term.first] = term.second; - } - } - - /// Insert terms into jacobians_, premultiplying by H, adding if already exists - void add(const Matrix& H, const JacobianMap& terms) { - BOOST_FOREACH(const Pair& term, terms) { - JacobianMap::iterator it = jacobians_.find(term.first); - if (it != jacobians_.end()) - it->second += H * term.second; - else - jacobians_[term.first] = H * term.second; - } - } - -public: - - /// Construct value that does not depend on anything - Augmented(const T& t) : - value_(t) { - } - - /// Construct value dependent on a single key - Augmented(const T& t, Key key) : - value_(t) { - size_t n = t.dim(); - jacobians_[key] = Eigen::MatrixXd::Identity(n, n); - } - - /// Construct value, pre-multiply jacobians by dTdA - Augmented(const T& t, const Matrix& dTdA, const JacobianMap& jacobians) : - value_(t) { - add(dTdA, jacobians); - } - - /// Construct value, pre-multiply jacobians - Augmented(const T& t, const Matrix& dTdA1, const JacobianMap& jacobians1, - const Matrix& dTdA2, const JacobianMap& jacobians2) : - value_(t) { - add(dTdA1, jacobians1); - add(dTdA2, jacobians2); - } - - /// Construct value, pre-multiply jacobians - Augmented(const T& t, const Matrix& dTdA1, const JacobianMap& jacobians1, - const Matrix& dTdA2, const JacobianMap& jacobians2, const Matrix& dTdA3, - const JacobianMap& jacobians3) : - value_(t) { - add(dTdA1, jacobians1); - add(dTdA2, jacobians2); - add(dTdA3, jacobians3); - } - - /// Return value - const T& value() const { - return value_; - } - - /// Return jacobians - const JacobianMap& jacobians() const { - return jacobians_; - } - - /// Return jacobians - JacobianMap& jacobians() { - return jacobians_; - } - - /// Not dependent on any key - bool constant() const { - return jacobians_.empty(); - } - - /// debugging - void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) { - BOOST_FOREACH(const Pair& term, jacobians_) - std::cout << "(" << keyFormatter(term.first) << ", " << term.second.rows() - << "x" << term.second.cols() << ") "; - std::cout << std::endl; - } - - /// Move terms to array, destroys content - void move(std::vector& H) { - assert(H.size()==jacobains.size()); - size_t j = 0; - JacobianMap::iterator it = jacobians_.begin(); - for (; it != jacobians_.end(); ++it) - it->second.swap(H[j++]); - } - -}; - //----------------------------------------------------------------------------- /** * Expression node. The superclass for objects that do the heavy lifting @@ -392,7 +207,11 @@ class ExpressionNode { protected: - ExpressionNode() { + size_t traceSize_; + + /// Constructor, traceSize is size of the execution trace of expression rooted here + ExpressionNode(size_t traceSize = 0) : + traceSize_(traceSize) { } public: @@ -402,19 +221,33 @@ public: } /// Return keys that play in this expression as a set - virtual std::set keys() const = 0; + virtual std::set keys() const { + std::set keys; + return keys; + } + + /// Return dimensions for each argument, as a map + virtual std::map dims() const { + std::map map; + return map; + } + + /// Return dimensions as vector, ordered as keys + std::vector dimensions() const { + std::map map = dims(); + std::vector dims(map.size()); + boost::copy(map | boost::adaptors::map_values, dims.begin()); + return dims; + } + + // Return size needed for memory buffer in traceExecution + size_t traceSize() const { + return traceSize_; + } /// Return value virtual T value(const Values& values) const = 0; - /// Return value and derivatives - virtual Augmented forward(const Values& values) const = 0; - - // Return size needed for memory buffer in traceExecution - virtual size_t traceSize() const { - return 0; - } - /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const = 0; @@ -437,22 +270,11 @@ class ConstantExpression: public ExpressionNode { public: - /// Return keys that play in this expression, i.e., the empty set - virtual std::set keys() const { - std::set keys; - return keys; - } - /// Return value virtual T value(const Values& values) const { return constant_; } - /// Return value and derivatives - virtual Augmented forward(const Values& values) const { - return Augmented(constant_); - } - /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const { @@ -484,16 +306,18 @@ public: return keys; } + /// Return dimensions for each argument + virtual std::map dims() const { + std::map map; + map[key_] = T::dimension; + return map; + } + /// Return value virtual T value(const Values& values) const { return values.at(key_); } - /// Return value and derivatives - virtual Augmented forward(const Values& values) const { - return Augmented(values.at(key_), key_); - } - /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const { @@ -504,68 +328,262 @@ public: }; //----------------------------------------------------------------------------- +// Below we use the "Class Composition" technique described in the book +// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost +// and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education. +// to recursively generate a class, that will be the base for function nodes. +// The class generated, for two arguments A1, A2, and A3 will be +// +// struct Base1 : Argument, FunctionalBase { +// ... storage related to A1 ... +// ... methods that work on A1 ... +// }; +// +// struct Base2 : Argument, Base1 { +// ... storage related to A2 ... +// ... methods that work on A2 and (recursively) on A2 ... +// }; +// +// struct Base2 : Argument, Base2 { +// ... storage related to A3 ... +// ... methods that work on A3 and (recursively) on A2 and A3 ... +// }; +// +// struct FunctionalNode : Base3 { +// Provides convenience access to storage in hierarchy by using +// static_cast &>(*this) +// } +// +// All this magic happens when we generate the Base3 base class of FunctionalNode +// by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode +//----------------------------------------------------------------------------- + +/// meta-function to generate fixed-size JacobianTA type +template +struct Jacobian { + typedef Eigen::Matrix type; +}; + +/// meta-function to generate JacobianTA optional reference +template +struct Optional { + typedef Eigen::Matrix Jacobian; + typedef boost::optional type; +}; + +/** + * Base case for recursive FunctionalNode class + */ +template +struct FunctionalBase: ExpressionNode { + static size_t const N = 0; // number of arguments + + typedef CallRecord Record; + + /// Construct an execution trace for reverse AD + void trace(const Values& values, Record* record, char*& raw) const { + } +}; + +/** + * Building block for recursive FunctionalNode class + * The integer argument N is to guarantee a unique type signature, + * so we are guaranteed to be able to extract their values by static cast. + */ +template +struct Argument { + /// Expression that will generate value/derivatives for argument + boost::shared_ptr > expression; +}; + +/** + * Building block for Recursive Record Class + * Records the evaluation of a single argument in a functional expression + */ +template +struct JacobianTrace { + A value; + ExecutionTrace trace; + typename Jacobian::type dTdA; +}; + +/** + * Recursive Definition of Functional ExpressionNode + */ +template +struct GenerateFunctionalNode: Argument, Base { + + static size_t const N = Base::N + 1; ///< Number of arguments in hierarchy + typedef Argument This; ///< The storage we have direct access to + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys = Base::keys(); + std::set myKeys = This::expression->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + return keys; + } + + /// Return dimensions for each argument + virtual std::map dims() const { + std::map map = Base::dims(); + std::map myMap = This::expression->dims(); + map.insert(myMap.begin(), myMap.end()); + return map; + } + + /// Recursive Record Class for Functional Expressions + struct Record: JacobianTrace, Base::Record { + + typedef T return_type; + typedef JacobianTrace This; + + /// Print to std::cout + virtual void print(const std::string& indent) const { + Base::Record::print(indent); + static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); + std::cout << This::dTdA.format(matlab) << std::endl; + This::trace.print(indent); + } + + /// Start the reverse AD process + virtual void startReverseAD(JacobianMap& jacobians) const { + Base::Record::startReverseAD(jacobians); + Select::reverseAD(This::trace, This::dTdA, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { + Base::Record::reverseAD(dFdT, jacobians); + This::trace.reverseAD(dFdT * This::dTdA, jacobians); + } + + /// Version specialized to 2-dimensional output + typedef Eigen::Matrix Jacobian2T; + virtual void reverseAD2(const Jacobian2T& dFdT, + JacobianMap& jacobians) const { + Base::Record::reverseAD2(dFdT, jacobians); + This::trace.reverseAD2(dFdT * This::dTdA, jacobians); + } + }; + + /// Construct an execution trace for reverse AD + void trace(const Values& values, Record* record, char*& raw) const { + Base::trace(values, record, raw); // recurse + // Write an Expression execution trace in record->trace + // Iff Constant or Leaf, this will not write to raw, only to trace. + // Iff the expression is functional, write all Records in raw buffer + // Return value of type T is recorded in record->value + record->Record::This::value = This::expression->traceExecution(values, + record->Record::This::trace, raw); + // raw is never modified by traceExecution, but if traceExecution has + // written in the buffer, the next caller expects we advance the pointer + raw += This::expression->traceSize(); + } +}; + +/** + * Recursive GenerateFunctionalNode class Generator + */ +template +struct FunctionalNode { + + typedef typename boost::mpl::fold, + GenerateFunctionalNode >::type Base; + + struct type: public Base { + + // Argument types and derived, note these are base 0 ! + typedef TYPES Arguments; + typedef typename boost::mpl::transform >::type Jacobians; + typedef typename boost::mpl::transform >::type Optionals; + + /// Reset expression shared pointer + template + void reset(const boost::shared_ptr >& ptr) { + static_cast &>(*this).expression = ptr; + } + + /// Access Expression shared pointer + template + boost::shared_ptr > expression() const { + return static_cast const &>(*this).expression; + } + + /// Provide convenience access to Record storage + struct Record: public Base::Record { + + /// Access Value + template + const A& value() const { + return static_cast const &>(*this).value; + } + + /// Access Jacobian + template + typename Jacobian::type& jacobian() { + return static_cast&>(*this).dTdA; + } + + }; + + /// Construct an execution trace for reverse AD + Record* trace(const Values& values, char* raw) const { + + // Create the record and advance the pointer + Record* record = new (raw) Record(); + raw = (char*) (record + 1); + + // Record the traces for all arguments + // After this, the raw pointer is set to after what was written + Base::trace(values, record, raw); + + // Return the record for this function evaluation + return record; + } + }; +}; +//----------------------------------------------------------------------------- + /// Unary Function Expression template -class UnaryExpression: public ExpressionNode { +class UnaryExpression: public FunctionalNode >::type { public: - typedef Eigen::Matrix JacobianTA; - typedef boost::function)> Function; + typedef boost::function::type)> Function; + typedef typename FunctionalNode >::type Base; + typedef typename Base::Record Record; private: Function function_; - boost::shared_ptr > expressionA1_; /// Constructor with a unary function f, and input argument e - UnaryExpression(Function f, const Expression& e) : - function_(f), expressionA1_(e.root()) { + UnaryExpression(Function f, const Expression& e1) : + function_(f) { + this->template reset(e1.root()); + ExpressionNode::traceSize_ = sizeof(Record) + e1.traceSize(); } friend class Expression ; public: - /// Return keys that play in this expression - virtual std::set keys() const { - return expressionA1_->keys(); - } - /// Return value virtual T value(const Values& values) const { - return function_(this->expressionA1_->value(values), boost::none); - } - - /// Return value and derivatives - virtual Augmented forward(const Values& values) const { - using boost::none; - Augmented argument = this->expressionA1_->forward(values); - JacobianTA dTdA; - T t = function_(argument.value(), - argument.constant() ? none : boost::optional(dTdA)); - return Augmented(t, dTdA, argument.jacobians()); - } - - /// CallRecord structure for reverse AD - typedef boost::mpl::vector > Arguments; - typedef typename GenerateRecord::type Record; - - // Return size needed for memory buffer in traceExecution - virtual size_t traceSize() const { - return sizeof(Record) + expressionA1_->traceSize(); + return function_(this->template expression()->value(values), boost::none); } /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const { - Record* record = new (raw) Record(); + + Record* record = Base::trace(values, raw); trace.setFunction(record); - raw = (char*) (record + 1); - A1 a1 = expressionA1_->traceExecution(values, getTrace(record), raw); - - return function_(a1, jacobian(record)); + return function_(record->template value(), + record->template jacobian()); } }; @@ -573,183 +591,109 @@ public: /// Binary Expression template -class BinaryExpression: public ExpressionNode { +class BinaryExpression: public FunctionalNode >::type { public: - typedef Eigen::Matrix JacobianTA1; - typedef Eigen::Matrix JacobianTA2; typedef boost::function< - T(const A1&, const A2&, boost::optional, - boost::optional)> Function; + T(const A1&, const A2&, typename Optional::type, + typename Optional::type)> Function; + typedef typename FunctionalNode >::type Base; + typedef typename Base::Record Record; private: Function function_; - boost::shared_ptr > expressionA1_; - boost::shared_ptr > expressionA2_; - /// Constructor with a binary function f, and two input arguments - BinaryExpression(Function f, // - const Expression& e1, const Expression& e2) : - function_(f), expressionA1_(e1.root()), expressionA2_(e2.root()) { + /// Constructor with a ternary function f, and three input arguments + BinaryExpression(Function f, const Expression& e1, + const Expression& e2) : + function_(f) { + this->template reset(e1.root()); + this->template reset(e2.root()); + ExpressionNode::traceSize_ = // + sizeof(Record) + e1.traceSize() + e2.traceSize(); } friend class Expression ; - friend struct ::TestBinaryExpression; + friend class ::ExpressionFactorBinaryTest; public: - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys1 = expressionA1_->keys(); - std::set keys2 = expressionA2_->keys(); - keys1.insert(keys2.begin(), keys2.end()); - return keys1; - } - /// Return value virtual T value(const Values& values) const { using boost::none; - return function_(this->expressionA1_->value(values), - this->expressionA2_->value(values), none, none); - } - - /// Return value and derivatives - virtual Augmented forward(const Values& values) const { - using boost::none; - Augmented a1 = this->expressionA1_->forward(values); - Augmented a2 = this->expressionA2_->forward(values); - JacobianTA1 dTdA1; - JacobianTA2 dTdA2; - T t = function_(a1.value(), a2.value(), - a1.constant() ? none : boost::optional(dTdA1), - a2.constant() ? none : boost::optional(dTdA2)); - return Augmented(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians()); - } - - /// CallRecord structure for reverse AD - typedef boost::mpl::vector, Numbered > Arguments; - typedef typename GenerateRecord::type Record; - - // Return size needed for memory buffer in traceExecution - virtual size_t traceSize() const { - return sizeof(Record) + expressionA1_->traceSize() - + expressionA2_->traceSize(); + return function_(this->template expression()->value(values), + this->template expression()->value(values), + none, none); } /// Construct an execution trace for reverse AD - /// The raw buffer is [Record | A1 raw | A2 raw] virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const { - Record* record = new (raw) Record(); + + Record* record = Base::trace(values, raw); trace.setFunction(record); - raw = (char*) (record + 1); - A1 a1 = expressionA1_->traceExecution(values, getTrace(record), raw); - raw = raw + expressionA1_->traceSize(); - A2 a2 = expressionA2_->traceExecution(values, getTrace(record), raw); - - return function_(a1, a2, jacobian(record), jacobian(record)); + return function_(record->template value(), + record->template value(), record->template jacobian(), + record->template jacobian()); } - }; //----------------------------------------------------------------------------- /// Ternary Expression template -class TernaryExpression: public ExpressionNode { +class TernaryExpression: public FunctionalNode >::type { public: - typedef Eigen::Matrix JacobianTA1; - typedef Eigen::Matrix JacobianTA2; - typedef Eigen::Matrix JacobianTA3; typedef boost::function< - T(const A1&, const A2&, const A3&, boost::optional, - boost::optional, boost::optional)> Function; + T(const A1&, const A2&, const A3&, typename Optional::type, + typename Optional::type, typename Optional::type)> Function; + typedef typename FunctionalNode >::type Base; + typedef typename Base::Record Record; private: Function function_; - boost::shared_ptr > expressionA1_; - boost::shared_ptr > expressionA2_; - boost::shared_ptr > expressionA3_; /// Constructor with a ternary function f, and three input arguments - TernaryExpression( - Function f, // - const Expression& e1, const Expression& e2, - const Expression& e3) : - function_(f), expressionA1_(e1.root()), expressionA2_(e2.root()), expressionA3_( - e3.root()) { + TernaryExpression(Function f, const Expression& e1, + const Expression& e2, const Expression& e3) : + function_(f) { + this->template reset(e1.root()); + this->template reset(e2.root()); + this->template reset(e3.root()); + ExpressionNode::traceSize_ = // + sizeof(Record) + e1.traceSize() + e2.traceSize() + e3.traceSize(); } friend class Expression ; public: - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys1 = expressionA1_->keys(); - std::set keys2 = expressionA2_->keys(); - std::set keys3 = expressionA3_->keys(); - keys2.insert(keys3.begin(), keys3.end()); - keys1.insert(keys2.begin(), keys2.end()); - return keys1; - } - /// Return value virtual T value(const Values& values) const { using boost::none; - return function_(this->expressionA1_->value(values), - this->expressionA2_->value(values), this->expressionA3_->value(values), - none, none, none); - } - - /// Return value and derivatives - virtual Augmented forward(const Values& values) const { - using boost::none; - Augmented a1 = this->expressionA1_->forward(values); - Augmented a2 = this->expressionA2_->forward(values); - Augmented a3 = this->expressionA3_->forward(values); - JacobianTA1 dTdA1; - JacobianTA2 dTdA2; - JacobianTA3 dTdA3; - T t = function_(a1.value(), a2.value(), a3.value(), - a1.constant() ? none : boost::optional(dTdA1), - a2.constant() ? none : boost::optional(dTdA2), - a3.constant() ? none : boost::optional(dTdA3)); - return Augmented(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians(), dTdA3, - a3.jacobians()); - } - - /// CallRecord structure for reverse AD - typedef boost::mpl::vector, Numbered, Numbered > Arguments; - typedef typename GenerateRecord::type Record; - - // Return size needed for memory buffer in traceExecution - virtual size_t traceSize() const { - return sizeof(Record) + expressionA1_->traceSize() - + expressionA2_->traceSize() + expressionA2_->traceSize(); + return function_(this->template expression()->value(values), + this->template expression()->value(values), + this->template expression()->value(values), + none, none, none); } /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, char* raw) const { - Record* record = new (raw) Record(); + + Record* record = Base::trace(values, raw); trace.setFunction(record); - raw = (char*) (record + 1); - A1 a1 = expressionA1_->traceExecution(values, getTrace(record), raw); - raw = raw + expressionA1_->traceSize(); - A2 a2 = expressionA2_->traceExecution(values, getTrace(record), raw); - raw = raw + expressionA2_->traceSize(); - A3 a3 = expressionA3_->traceExecution(values, getTrace(record), raw); - - return function_(a1, a2, a3, jacobian(record), - jacobian(record), jacobian(record)); + return function_( + record->template value(), record->template value(), + record->template value(), record->template jacobian(), + record->template jacobian(), record->template jacobian()); } }; diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index afd5a60e0..7556ea629 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -34,73 +34,83 @@ class Expression { private: // Paul's trick shared pointer, polymorphic root of entire expression tree - boost::shared_ptr > root_; + const boost::shared_ptr > root_; + + // Fixed dimensions: an Expression is assumed unmutable + const std::vector dimensions_; public: // Construct a constant expression Expression(const T& value) : - root_(new ConstantExpression(value)) { + root_(new ConstantExpression(value)), // + dimensions_(root_->dimensions()) { } // Construct a leaf expression, with Key Expression(const Key& key) : - root_(new LeafExpression(key)) { + root_(new LeafExpression(key)), // + dimensions_(root_->dimensions()) { } // Construct a leaf expression, with Symbol Expression(const Symbol& symbol) : - root_(new LeafExpression(symbol)) { + root_(new LeafExpression(symbol)), // + dimensions_(root_->dimensions()) { } // Construct a leaf expression, creating Symbol Expression(unsigned char c, size_t j) : - root_(new LeafExpression(Symbol(c, j))) { + root_(new LeafExpression(Symbol(c, j))), // + dimensions_(root_->dimensions()) { } /// Construct a nullary method expression template Expression(const Expression& expression, - T (A::*method)(boost::optional) const) { - root_.reset( - new UnaryExpression(boost::bind(method, _1, _2), expression)); + T (A::*method)(typename Optional::type) const) : + root_(new UnaryExpression(boost::bind(method, _1, _2), expression)), // + dimensions_(root_->dimensions()) { } /// Construct a unary function expression template Expression(typename UnaryExpression::Function function, - const Expression& expression) { - root_.reset(new UnaryExpression(function, expression)); + const Expression& expression) : + root_(new UnaryExpression(function, expression)), // + dimensions_(root_->dimensions()) { } /// Construct a unary method expression template Expression(const Expression& expression1, - T (A1::*method)(const A2&, - boost::optional::JacobianTA1&>, - boost::optional::JacobianTA2&>) const, - const Expression& expression2) { - root_.reset( - new BinaryExpression(boost::bind(method, _1, _2, _3, _4), - expression1, expression2)); + T (A1::*method)(const A2&, typename Optional::type, + typename Optional::type) const, + const Expression& expression2) : + root_( + new BinaryExpression(boost::bind(method, _1, _2, _3, _4), + expression1, expression2)), // + dimensions_(root_->dimensions()) { } /// Construct a binary function expression template Expression(typename BinaryExpression::Function function, - const Expression& expression1, const Expression& expression2) { - root_.reset( - new BinaryExpression(function, expression1, expression2)); + const Expression& expression1, const Expression& expression2) : + root_( + new BinaryExpression(function, expression1, expression2)), // + dimensions_(root_->dimensions()) { } /// Construct a ternary function expression template Expression(typename TernaryExpression::Function function, const Expression& expression1, const Expression& expression2, - const Expression& expression3) { - root_.reset( - new TernaryExpression(function, expression1, expression2, - expression3)); + const Expression& expression3) : + root_( + new TernaryExpression(function, expression1, + expression2, expression3)), // + dimensions_(root_->dimensions()) { } /// Return keys that play in this expression @@ -108,14 +118,9 @@ public: return root_->keys(); } - /// Return value and optional derivatives - T value(const Values& values) const { - return root_->value(values); - } - - /// Return value and derivatives, forward AD version - Augmented forward(const Values& values) const { - return root_->forward(values); + /// Return dimensions for each argument, must be same order as keys + const std::vector& dimensions() const { + return dimensions_; } // Return size needed for memory buffer in traceExecution @@ -130,29 +135,31 @@ public: } /// Return value and derivatives, reverse AD version - Augmented reverse(const Values& values) const { + T reverse(const Values& values, JacobianMap& jacobians) const { size_t size = traceSize(); char raw[size]; ExecutionTrace trace; T value(traceExecution(values, trace, raw)); - Augmented augmented(value); - trace.startReverseAD(augmented.jacobians()); - return augmented; + trace.startReverseAD(jacobians); + return value; + } + + /// Return value + T value(const Values& values) const { + return root_->value(values); } /// Return value and derivatives - Augmented augmented(const Values& values) const { -#ifdef EXPRESSION_FORWARD_AD - return forward(values); -#else - return reverse(values); -#endif + T value(const Values& values, JacobianMap& jacobians) const { + return reverse(values, jacobians); } const boost::shared_ptr >& root() const { return root_; } + /// Define type so we can apply it as a meta-function + typedef Expression type; }; // http://stackoverflow.com/questions/16260445/boost-bind-to-operator diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 14e9c54ba..a2e9fb273 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -39,6 +39,11 @@ public: const T& measurement, const Expression& expression) : NoiseModelFactor(noiseModel, expression.keys()), // measurement_(measurement), expression_(expression) { + if (!noiseModel) + throw std::invalid_argument("ExpressionFactor: no NoiseModel."); + if (noiseModel->dim() != T::dimension) + throw std::invalid_argument( + "ExpressionFactor was created with a NoiseModel of incorrect dimension."); } /** @@ -49,17 +54,71 @@ public: virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if (H) { + // H should be pre-allocated assert(H->size()==size()); - Augmented augmented = expression_.augmented(x); - // move terms to H, which is pre-allocated to correct size - augmented.move(*H); - return measurement_.localCoordinates(augmented.value()); + + // Get dimensions of Jacobian matrices + std::vector dims = expression_.dimensions(); + + // Create and zero out blocks to be passed to expression_ + JacobianMap blocks; + for(DenseIndex i=0;iat(i); + Hi.resize(T::dimension, dims[i]); + Hi.setZero(); // zero out + Eigen::Block block = Hi.block(0,0,T::dimension, dims[i]); + blocks.insert(std::make_pair(keys_[i], block)); + } + + T value = expression_.value(x, blocks); + return measurement_.localCoordinates(value); } else { const T& value = expression_.value(x); return measurement_.localCoordinates(value); } } + // Internal function to allocate a VerticalBlockMatrix and + // create Eigen::Block views into it + VerticalBlockMatrix prepareBlocks(JacobianMap& blocks) const { + + // Get dimensions of Jacobian matrices + std::vector dims = expression_.dimensions(); + + // Construct block matrix, is of right size but un-initialized + VerticalBlockMatrix Ab(dims, T::dimension, true); + Ab.matrix().setZero(); // zero out + + // Create blocks to be passed to expression_ + for(DenseIndex i=0;i linearize(const Values& x) const { + + // Construct VerticalBlockMatrix and views into it + JacobianMap blocks; + VerticalBlockMatrix Ab = prepareBlocks(blocks); + + // Evaluate error to get Jacobians and RHS vector b + T value = expression_.value(x, blocks); + Ab(size()).col(0) = -measurement_.localCoordinates(value); + + // Whiten the corresponding system now + // TODO ! this->noiseModel_->WhitenSystem(Ab); + + // TODO pass unwhitened + noise model to Gaussian factor + // For now, only linearized constrained factors have noise model at linear level!!! + noiseModel::Constrained::shared_ptr constrained = // + boost::dynamic_pointer_cast(this->noiseModel_); + if (constrained) { + return boost::make_shared(this->keys(), Ab, + constrained->unit()); + } else + return boost::make_shared(this->keys(), Ab); + } }; // ExpressionFactor diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index e14912a65..ad738d50c 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -26,6 +26,10 @@ #include +#include +using boost::assign::list_of; +using boost::assign::map_list_of; + using namespace std; using namespace gtsam; @@ -44,10 +48,11 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); TEST(Expression, constant) { Expression R(someR); Values values; - Augmented a = R.augmented(values); - EXPECT(assert_equal(someR, a.value())); + JacobianMap actualMap; + Rot3 actual = R.value(values, actualMap); + EXPECT(assert_equal(someR, actual)); JacobianMap expected; - EXPECT(a.jacobians() == expected); + EXPECT(actualMap == expected); } /* ************************************************************************* */ @@ -56,11 +61,16 @@ TEST(Expression, leaf) { Expression R(100); Values values; values.insert(100, someR); - Augmented a = R.augmented(values); - EXPECT(assert_equal(someR, a.value())); + JacobianMap expected; - expected[100] = eye(3); - EXPECT(a.jacobians() == expected); + Matrix H = eye(3); + expected.insert(make_pair(100, H.block(0, 0, 3, 3))); + + JacobianMap actualMap2; + actualMap2.insert(make_pair(100, H.block(0, 0, 3, 3))); + Rot3 actual2 = R.reverse(values, actualMap2); + EXPECT(assert_equal(someR, actual2)); + EXPECT(actualMap2 == expected); } /* ************************************************************************* */ @@ -86,13 +96,16 @@ Expression p_cam(x, &Pose3::transform_to, p); } /* ************************************************************************* */ // keys -TEST(Expression, keys_binary) { - - // Check keys - set expectedKeys; - expectedKeys.insert(1); - expectedKeys.insert(2); - EXPECT(expectedKeys == binary::p_cam.keys()); +TEST(Expression, BinaryKeys) { + set expected = list_of(1)(2); + EXPECT(expected == binary::p_cam.keys()); +} +/* ************************************************************************* */ +// dimensions +TEST(Expression, BinaryDimensions) { + vector expected = list_of(6)(3), // + actual = binary::p_cam.dimensions(); + EXPECT(expected==actual); } /* ************************************************************************* */ // Binary(Leaf,Unary(Binary(Leaf,Leaf))) @@ -107,24 +120,16 @@ Expression uv_hat(uncalibrate, K, projection); } /* ************************************************************************* */ // keys -TEST(Expression, keys_tree) { - - // Check keys - set expectedKeys; - expectedKeys.insert(1); - expectedKeys.insert(2); - expectedKeys.insert(3); - EXPECT(expectedKeys == tree::uv_hat.keys()); +TEST(Expression, TreeKeys) { + set expected = list_of(1)(2)(3); + EXPECT(expected == tree::uv_hat.keys()); } /* ************************************************************************* */ -// keys -TEST(Expression, block_tree) { -// // Check VerticalBlockMatrix -// size_t dimensions[3] = { 6, 3, 5 }; -// Matrix matrix(2, 14); -// VerticalBlockMatrix expected(dimensions, matrix), actual = -// tree::uv_hat.verticalBlockMatrix(); -// EXPECT( assert_equal(expected, *jf, 1e-9)); +// dimensions +TEST(Expression, TreeDimensions) { + vector expected = list_of(6)(3)(5), // + actual = tree::uv_hat.dimensions(); + EXPECT(expected==actual); } /* ************************************************************************* */ @@ -135,10 +140,8 @@ TEST(Expression, compose1) { Expression R3 = R1 * R2; // Check keys - set expectedKeys; - expectedKeys.insert(1); - expectedKeys.insert(2); - EXPECT(expectedKeys == R3.keys()); + set expected = list_of(1)(2); + EXPECT(expected == R3.keys()); } /* ************************************************************************* */ @@ -150,9 +153,8 @@ TEST(Expression, compose2) { Expression R3 = R1 * R2; // Check keys - set expectedKeys; - expectedKeys.insert(1); - EXPECT(expectedKeys == R3.keys()); + set expected = list_of(1); + EXPECT(expected == R3.keys()); } /* ************************************************************************* */ @@ -164,9 +166,8 @@ TEST(Expression, compose3) { Expression R3 = R1 * R2; // Check keys - set expectedKeys; - expectedKeys.insert(3); - EXPECT(expectedKeys == R3.keys()); + set expected = list_of(3); + EXPECT(expected == R3.keys()); } /* ************************************************************************* */ @@ -191,11 +192,8 @@ TEST(Expression, ternary) { Expression ABC(composeThree, A, B, C); // Check keys - set expectedKeys; - expectedKeys.insert(1); - expectedKeys.insert(2); - expectedKeys.insert(3); - EXPECT(expectedKeys == ABC.keys()); + set expected = list_of(1)(2)(3); + EXPECT(expected == ABC.keys()); } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 02d9b4e9d..93c2ecac1 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -33,85 +34,73 @@ using namespace gtsam; Point2 measured(-17, 30); SharedNoiseModel model = noiseModel::Unit::Create(2); +namespace leaf { +// Create some values +struct MyValues: public Values { + MyValues() { + insert(2, Point2(3, 5)); + } +} values; + +// Create leaf +Point2_ p(2); +} + /* ************************************************************************* */ // Leaf -TEST(ExpressionFactor, leaf) { +TEST(ExpressionFactor, Leaf) { + using namespace leaf; - // Create some values - Values values; - values.insert(2, Point2(3, 5)); - - JacobianFactor expected( // - 2, (Matrix(2, 2) << 1, 0, 0, 1), // - (Vector(2) << -3, -5)); - - // Create leaf - Point2_ p(2); + // Create old-style factor to create expected value and derivatives + PriorFactor old(2, Point2(0, 0), model); // Concise version ExpressionFactor f(model, Point2(0, 0), p); + EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf, 1e-9)); + boost::shared_ptr gf2 = f.linearize(values); + EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); } -/* ************************************************************************* */ -// non-zero noise model -TEST(ExpressionFactor, model) { - - SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); - - // Create some values - Values values; - values.insert(2, Point2(3, 5)); - - JacobianFactor expected( // - 2, (Matrix(2, 2) << 10, 0, 0, 100), // - (Vector(2) << -30, -500)); - - // Create leaf - Point2_ p(2); - - // Concise version - ExpressionFactor f(model, Point2(0, 0), p); - EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf, 1e-9)); -} - -/* ************************************************************************* */ -// Constrained noise model -TEST(ExpressionFactor, constrained) { - - SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2,0)); - - // Create some values - Values values; - values.insert(2, Point2(3, 5)); - - vector > terms; - terms.push_back(make_pair(2, (Matrix(2, 2) << 1, 0, 0, 1))); - JacobianFactor expected(terms, (Vector(2) << -3, -5), model); - - // Create leaf - Point2_ p(2); - - // Concise version - ExpressionFactor f(model, Point2(0, 0), p); - EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf, 1e-9)); -} +///* ************************************************************************* */ +//// non-zero noise model +//TEST(ExpressionFactor, Model) { +// using namespace leaf; +// +// SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); +// +// // Create old-style factor to create expected value and derivatives +// PriorFactor old(2, Point2(0, 0), model); +// +// // Concise version +// ExpressionFactor f(model, Point2(0, 0), p); +// EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); +// EXPECT_LONGS_EQUAL(2, f.dim()); +// boost::shared_ptr gf2 = f.linearize(values); +// EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); +//} +// +///* ************************************************************************* */ +//// Constrained noise model +//TEST(ExpressionFactor, Constrained) { +// using namespace leaf; +// +// SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0)); +// +// // Create old-style factor to create expected value and derivatives +// PriorFactor old(2, Point2(0, 0), model); +// +// // Concise version +// ExpressionFactor f(model, Point2(0, 0), p); +// EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); +// EXPECT_LONGS_EQUAL(2, f.dim()); +// boost::shared_ptr gf2 = f.linearize(values); +// EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); +//} /* ************************************************************************* */ // Unary(Leaf)) -TEST(ExpressionFactor, unary) { +TEST(ExpressionFactor, Unary) { // Create some values Values values; @@ -132,25 +121,21 @@ TEST(ExpressionFactor, unary) { boost::dynamic_pointer_cast(gf); EXPECT( assert_equal(expected, *jf, 1e-9)); } + /* ************************************************************************* */ -struct TestBinaryExpression { - static Point2 myUncal(const Cal3_S2& K, const Point2& p, - boost::optional Dcal, boost::optional Dp) { - return K.uncalibrate(p, Dcal, Dp); - } - Cal3_S2_ K_; - Point2_ p_; - BinaryExpression binary_; - TestBinaryExpression() : - K_(1), p_(2), binary_(myUncal, K_, p_) { - } -}; -/* ************************************************************************* */ +static Point2 myUncal(const Cal3_S2& K, const Point2& p, + boost::optional Dcal, boost::optional Dp) { + return K.uncalibrate(p, Dcal, Dp); +} + // Binary(Leaf,Leaf) -TEST(ExpressionFactor, binary) { +TEST(ExpressionFactor, Binary) { typedef BinaryExpression Binary; - TestBinaryExpression tester; + + Cal3_S2_ K_(1); + Point2_ p_(2); + Binary binary(myUncal, K_, p_); // Create some values Values values; @@ -159,22 +144,24 @@ TEST(ExpressionFactor, binary) { // traceRaw will fill raw with [Trace | Binary::Record] EXPECT_LONGS_EQUAL(8, sizeof(double)); + EXPECT_LONGS_EQUAL(24, sizeof(Point2)); + EXPECT_LONGS_EQUAL(48, sizeof(Cal3_S2)); EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); - EXPECT_LONGS_EQUAL(2*5*8, sizeof(Binary::JacobianTA1)); - EXPECT_LONGS_EQUAL(2*2*8, sizeof(Binary::JacobianTA2)); - size_t expectedRecordSize = 16 + 2 * 16 + 80 + 32; + EXPECT_LONGS_EQUAL(2*5*8, sizeof(Jacobian::type)); + EXPECT_LONGS_EQUAL(2*2*8, sizeof(Jacobian::type)); + size_t expectedRecordSize = 24 + 24 + 48 + 2 * 16 + 80 + 32; EXPECT_LONGS_EQUAL(expectedRecordSize, sizeof(Binary::Record)); // Check size - size_t size = tester.binary_.traceSize(); + size_t size = binary.traceSize(); CHECK(size); EXPECT_LONGS_EQUAL(expectedRecordSize, size); // Use Variable Length Array, allocated on stack by gcc // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla char raw[size]; ExecutionTrace trace; - Point2 value = tester.binary_.traceExecution(values, trace, raw); + Point2 value = binary.traceExecution(values, trace, raw); // trace.print(); // Expected Jacobians @@ -187,13 +174,12 @@ TEST(ExpressionFactor, binary) { boost::optional r = trace.record(); CHECK(r); EXPECT( - assert_equal(expected25, (Matrix) static_cast*> (*r)->dTdA, 1e-9)); - EXPECT( - assert_equal(expected22, (Matrix) static_cast*> (*r)->dTdA, 1e-9)); + assert_equal(expected25, (Matrix) (*r)-> jacobian(), 1e-9)); + EXPECT( assert_equal(expected22, (Matrix) (*r)->jacobian(), 1e-9)); } /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf)) -TEST(ExpressionFactor, shallow) { +TEST(ExpressionFactor, Shallow) { // Create some values Values values; @@ -216,10 +202,10 @@ TEST(ExpressionFactor, shallow) { // traceExecution of shallow tree typedef UnaryExpression Unary; typedef BinaryExpression Binary; - EXPECT_LONGS_EQUAL(80, sizeof(Unary::Record)); - EXPECT_LONGS_EQUAL(272, sizeof(Binary::Record)); + EXPECT_LONGS_EQUAL(112, sizeof(Unary::Record)); + EXPECT_LONGS_EQUAL(432, sizeof(Binary::Record)); size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record); - LONGS_EQUAL(352, expectedTraceSize); + LONGS_EQUAL(112+432, expectedTraceSize); size_t size = expression.traceSize(); CHECK(size); EXPECT_LONGS_EQUAL(expectedTraceSize, size); @@ -235,8 +221,7 @@ TEST(ExpressionFactor, shallow) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT( - assert_equal(expected23, (Matrix)static_cast*>(*r)->dTdA, 1e-9)); + EXPECT(assert_equal(expected23, (Matrix)(*r)->jacobian(), 1e-9)); // Linearization ExpressionFactor f2(model, measured, expression); @@ -298,7 +283,7 @@ TEST(ExpressionFactor, tree) { /* ************************************************************************* */ -TEST(ExpressionFactor, compose1) { +TEST(ExpressionFactor, Compose1) { // Create expression Rot3_ R1(1), R2(2); @@ -430,37 +415,6 @@ TEST(ExpressionFactor, composeTernary) { EXPECT( assert_equal(expected, *jf,1e-9)); } -/* ************************************************************************* */ - -namespace mpl = boost::mpl; - -#include -template struct Incomplete; - -typedef mpl::vector, Numbered, - Numbered > MyTypes; -typedef GenerateRecord::type Generated; -//Incomplete incomplete; -//BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >)); -BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Jacobian2T >)); - -Generated generated; - -typedef mpl::vector1 OneType; -typedef mpl::pop_front::type Empty; -typedef mpl::pop_front::type Bad; -//typedef ProtoTrace UnaryTrace; -//BOOST_MPL_ASSERT((boost::is_same< UnaryTrace::A, Point3 >)); - -#include -#include -#include -//#include - -typedef struct { -} Expected0; -BOOST_MPL_ASSERT((boost::is_same< Expected0, Expected0 >)); - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp new file mode 100644 index 000000000..19a39d52f --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp @@ -0,0 +1,158 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testExpressionMeta.cpp + * @date October 14, 2014 + * @author Frank Dellaert + * @brief Test meta-programming constructs for Expressions + */ + +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +namespace mpl = boost::mpl; + +#include +#include +template struct Incomplete; + +// Check generation of FunctionalNode +typedef mpl::vector MyTypes; +typedef FunctionalNode::type Generated; +//Incomplete incomplete; +BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Record::Jacobian2T >)); + +// Try generating vectors of ExecutionTrace +typedef mpl::vector, ExecutionTrace > ExpectedTraces; + +typedef mpl::transform >::type MyTraces; +BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces >)); + +template +struct MakeTrace { + typedef ExecutionTrace type; +}; +typedef mpl::transform >::type MyTraces1; +BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces1 >)); + +// Try generating vectors of Expression types +typedef mpl::vector, Expression > ExpectedExpressions; +typedef mpl::transform >::type Expressions; +BOOST_MPL_ASSERT((mpl::equal< ExpectedExpressions, Expressions >)); + +// Try generating vectors of Jacobian types +typedef mpl::vector ExpectedJacobians; +typedef mpl::transform >::type Jacobians; +BOOST_MPL_ASSERT((mpl::equal< ExpectedJacobians, Jacobians >)); + +// Try accessing a Jacobian +typedef mpl::at_c::type Jacobian23; // base zero ! +BOOST_MPL_ASSERT((boost::is_same< Matrix23, Jacobian23>)); + +/* ************************************************************************* */ +// Boost Fusion includes allow us to create/access values from MPL vectors +#include +#include + +// Create a value and access it +TEST(ExpressionFactor, JacobiansValue) { + using boost::fusion::at_c; + Matrix23 expected; + Jacobians jacobians; + + at_c<1>(jacobians) << 1, 2, 3, 4, 5, 6; + + Matrix23 actual = at_c<1>(jacobians); + CHECK(actual.cols() == expected.cols()); + CHECK(actual.rows() == expected.rows()); +} + +/* ************************************************************************* */ +// Test out polymorphic transform + +#include +#include +#include + +struct triple { + template struct result; // says we will provide result + + template + struct result { + typedef double type; // result for int argument + }; + + template + struct result { + typedef double type; // result for int argument + }; + + template + struct result { + typedef double type; // result for double argument + }; + + template + struct result { + typedef double type; // result for double argument + }; + + // actual function + template + typename result::type operator()(const T& x) const { + return (double) x; + } +}; + +TEST(ExpressionFactor, Triple) { + typedef boost::fusion::vector IntDouble; + IntDouble H = boost::fusion::make_vector(1, 2.0); + + // Only works if I use Double2 + typedef boost::fusion::result_of::transform::type Result; + typedef boost::fusion::vector Double2; + Double2 D = boost::fusion::transform(H, triple()); + + using boost::fusion::at_c; + DOUBLES_EQUAL(1.0, at_c<0>(D), 1e-9); + DOUBLES_EQUAL(2.0, at_c<1>(D), 1e-9); +} + +/* ************************************************************************* */ +#include +#include + +// Test out polymorphic transform +TEST(ExpressionFactor, Invoke) { + std::plus add; + assert(invoke(add,boost::fusion::make_vector(1,1)) == 2); + + // Creating a Pose3 (is there another way?) + boost::fusion::vector pair; + Pose3 pose = boost::fusion::invoke(boost::value_factory(), pair); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +