Merge remote-tracking branch 'origin/feature/evenFasterExpressions' into feature/typedefPoint3

examples/RangeISAMExample_plaza2.cpp

@@ -83,7 +83,7 @@ vector<RangeTriple> readTriples() {
 
   while (is) {
     double t, sender, range;
-	size_t receiver;
+    size_t receiver;
     is >> t >> sender >> receiver >> range;
     triples.push_back(RangeTriple(t, receiver, range));
   }

gtsam/base/Matrix.cpp

@@ -189,7 +189,7 @@ void print(const Matrix& A, const string &s, ostream& stream) {
       0, // flags
       " ", // coeffSeparator
       ";\n", // rowSeparator
-	  " \t",  // rowPrefix
+      " \t",  // rowPrefix
       "", // rowSuffix
       "[\n", // matPrefix
       "\n  ]" // matSuffix

gtsam/inference/EliminateableFactorGraph-inst.h

@@ -28,8 +28,8 @@ namespace gtsam {
   template<class FACTORGRAPH>
   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
     EliminateableFactorGraph<FACTORGRAPH>::eliminateSequential(
-	OptionalOrdering ordering, const Eliminate& function,
+    OptionalOrdering ordering, const Eliminate& function,
-	OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
+    OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
   {
     if(ordering && variableIndex) {
       gttic(eliminateSequential);
@@ -65,8 +65,8 @@ namespace gtsam {
   template<class FACTORGRAPH>
   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
     EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
-	  OptionalOrdering ordering, const Eliminate& function,
+    OptionalOrdering ordering, const Eliminate& function,
-	  OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
+    OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
   {
     if(ordering && variableIndex) {
       gttic(eliminateMultifrontal);
@@ -86,16 +86,16 @@ namespace gtsam {
       // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
       // for no variable index first so that it's always computed if we need to call COLAMD because
       // no Ordering is provided.
-		  return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType);
+      return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType);
     }
     else /*if(!ordering)*/ {
       // If no Ordering provided, compute one and call this function again.  We are guaranteed to
       // have a VariableIndex already here because we computed one if needed in the previous 'else'
       // block.
-	    if (orderingType == Ordering::METIS)
+      if (orderingType == Ordering::METIS)
-		    return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType);
+        return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType);
-	    else
+      else
-		    return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType);
+        return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType);
     }
   }
 

gtsam/inference/EliminateableFactorGraph.h

@@ -94,8 +94,8 @@ namespace gtsam {
     /// Typedef for an optional variable index as an argument to elimination functions
     typedef boost::optional<const VariableIndex&> OptionalVariableIndex;
 
-	  /// Typedef for an optional ordering type
+    /// Typedef for an optional ordering type
-	  typedef boost::optional<Ordering::OrderingType> OptionalOrderingType;
+    typedef boost::optional<Ordering::OrderingType> OptionalOrderingType;
 
     /** Do sequential elimination of all variables to produce a Bayes net.  If an ordering is not
      *  provided, the ordering provided by COLAMD will be used.
@@ -104,10 +104,10 @@ namespace gtsam {
      *  \code
      *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateCholesky);
      *  \endcode
-	   *
+     *
-	   *  <b> Example - METIS ordering for elimination
+     *  <b> Example - METIS ordering for elimination
-	   *  \code
+     *  \code
-	   *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(OrderingType::METIS);
+     *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(OrderingType::METIS);
      *  
      *  <b> Example - Full QR elimination in specified order:
      *  \code
@@ -125,7 +125,7 @@ namespace gtsam {
       OptionalOrdering ordering = boost::none,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = boost::none,
-	    OptionalOrderingType orderingType = boost::none) const;
+      OptionalOrderingType orderingType = boost::none) const;
 
     /** Do multifrontal elimination of all variables to produce a Bayes tree.  If an ordering is not
      *  provided, the ordering will be computed using either COLAMD or METIS, dependeing on
@@ -151,8 +151,8 @@ namespace gtsam {
     boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
       OptionalOrdering ordering = boost::none,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
-	    OptionalVariableIndex variableIndex = boost::none,
+      OptionalVariableIndex variableIndex = boost::none,
-	    OptionalOrderingType orderingType = boost::none) const;
+      OptionalOrderingType orderingType = boost::none) const;
 
     /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net
      *  and a remaining factor graph.  This computes the factorization \f$ p(X) = p(A|B) p(B) \f$,

gtsam/linear/JacobianFactor.cpp

@@ -349,11 +349,11 @@ void JacobianFactor::print(const string& s,
     const KeyFormatter& formatter) const {
   static const Eigen::IOFormat matlab(
       Eigen::StreamPrecision, // precision
-	  0, // flags
+      0, // flags
       " ", // coeffSeparator
       ";\n", // rowSeparator
-	  "\t",  // rowPrefix
+      "\t",  // rowPrefix
-	  "", // rowSuffix
+      "", // rowSuffix
       "[\n", // matPrefix
       "\n  ]" // matSuffix
       );

gtsam/nonlinear/NonlinearOptimizer.cpp

@@ -110,8 +110,8 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg,
     delta = gfg.optimize(*params.ordering, params.getEliminationFunction());
   } else if (params.isSequential()) {
     // Sequential QR or Cholesky (decided by params.getEliminationFunction())
-    delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(), 
+    delta = gfg.eliminateSequential(*params.ordering,
-								     boost::none, params.orderingType)->optimize();
+            params.getEliminationFunction(), boost::none, params.orderingType)->optimize();
   } else if (params.isIterative()) {
 
     // Conjugate Gradient -> needs params.iterativeParams

gtsam/nonlinear/NonlinearOptimizerParams.cpp

@@ -110,14 +110,14 @@ void NonlinearOptimizerParams::print(const std::string& str) const {
 
   switch (orderingType){
   case Ordering::COLAMD:
-	  std::cout << "                   ordering: COLAMD\n";
+    std::cout << "                   ordering: COLAMD\n";
-	  break;
+    break;
   case Ordering::METIS:
-	  std::cout << "                   ordering: METIS\n";
+    std::cout << "                   ordering: METIS\n";
-	  break;
+    break;
   default:
-	  std::cout << "                   ordering: custom\n";
+    std::cout << "                   ordering: custom\n";
-	  break;
+    break;
   }
 
   std::cout.flush();
@@ -165,29 +165,31 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve
 }
 
 /* ************************************************************************* */
-std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::OrderingType type) const{
+std::string NonlinearOptimizerParams::orderingTypeTranslator(
-	switch (type) {
+    Ordering::OrderingType type) const {
-	case Ordering::METIS:
+  switch (type) {
-		return "METIS";
+  case Ordering::METIS:
-	case Ordering::COLAMD:
+    return "METIS";
-		return "COLAMD";
+  case Ordering::COLAMD:
-	default:
+    return "COLAMD";
-		if (ordering)
+  default:
-			return "CUSTOM";
+    if (ordering)
-		else
+      return "CUSTOM";
-			throw std::invalid_argument(
+    else
-			"Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
+      throw std::invalid_argument(
-	}
+          "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
+  }
 }
 
 /* ************************************************************************* */
-Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{
+Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(
-	if (type == "METIS")
+    const std::string& type) const {
-		return Ordering::METIS;
+  if (type == "METIS")
-	if (type == "COLAMD")
+    return Ordering::METIS;
-		return Ordering::COLAMD;
+  if (type == "COLAMD")
-	throw std::invalid_argument(
+    return Ordering::COLAMD;
-		"Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
+  throw std::invalid_argument(
+      "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
 }
 
 

gtsam/nonlinear/NonlinearOptimizerParams.h

@@ -154,16 +154,16 @@ public:
 
   void setOrdering(const Ordering& ordering) {
     this->ordering = ordering;
-	  this->orderingType = Ordering::CUSTOM;
+    this->orderingType = Ordering::CUSTOM;
   }
 
   std::string getOrderingType() const {
-	  return orderingTypeTranslator(orderingType);
+    return orderingTypeTranslator(orderingType);
   }
 
   // Note that if you want to use a custom ordering, you must set the ordering directly, this will switch to custom type
   void setOrderingType(const std::string& ordering){
-	  orderingType = orderingTypeTranslator(ordering);
+    orderingType = orderingTypeTranslator(ordering);
   }
 
 private:

gtsam/nonlinear/internal/ExpressionNode.h

@@ -269,9 +269,13 @@ public:
   // Inner Record Class
   struct Record: public CallRecordImplementor<Record, traits<T>::dimension> {
 
-    A1 value1;
-    ExecutionTrace<A1> trace1;
     typename Jacobian<T, A1>::type dTdA1;
+    ExecutionTrace<A1> trace1;
+    A1 value1;
+
+    /// Construct record by calling argument expression
+    Record(const Values& values, const ExpressionNode<A1>& expression1, ExecutionTraceStorage* ptr)
+        : value1(expression1.traceExecution(values, trace1, ptr + upAligned(sizeof(Record)))) {}
 
     /// Print to std::cout
     void print(const std::string& indent) const {
@@ -305,20 +309,15 @@ public:
       ExecutionTraceStorage* ptr) const {
     assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
 
-    // Create the record at the start of the traceStorage and advance the pointer
+    // Create a Record in the memory pointed to by ptr
-    Record* record = new (ptr) Record();
+    // Calling the construct will record the traces for all arguments
-    ptr += upAligned(sizeof(Record));
-
-    // Record the traces for all arguments
-    // After this, the traceStorage pointer is set to after what was written
     // Write an Expression<A> execution trace in record->trace
     // Iff Constant or Leaf, this will not write to traceStorage, only to trace.
     // Iff the expression is functional, write all Records in traceStorage buffer
     // Return value of type T is recorded in record->value
-    record->value1 = expression1_->traceExecution(values, record->trace1, ptr);
+    Record* record = new (ptr) Record(values, *expression1_, ptr);
 
-    //  We have written in the buffer, the next caller expects we advance the pointer
+    // Our trace parameter is set to point to the Record
-    ptr += expression1_->traceSize();
     trace.setFunction(record);
 
     // Finally, the function call fills in the Jacobian dTdA1
@@ -384,14 +383,21 @@ public:
   // Inner Record Class
   struct Record: public CallRecordImplementor<Record, traits<T>::dimension> {
 
-    A1 value1;
-    ExecutionTrace<A1> trace1;
     typename Jacobian<T, A1>::type dTdA1;
-
-    A2 value2;
-    ExecutionTrace<A2> trace2;
     typename Jacobian<T, A2>::type dTdA2;
 
+    ExecutionTrace<A1> trace1;
+    ExecutionTrace<A2> trace2;
+
+    A1 value1;
+    A2 value2;
+
+    /// Construct record by calling argument expressions
+    Record(const Values& values, const ExpressionNode<A1>& expression1,
+           const ExpressionNode<A2>& expression2, ExecutionTraceStorage* ptr)
+        : value1(expression1.traceExecution(values, trace1, ptr += upAligned(sizeof(Record)))),
+          value2(expression2.traceExecution(values, trace2, ptr += expression1.traceSize())) {}
+
     /// Print to std::cout
     void print(const std::string& indent) const {
       std::cout << indent << "BinaryExpression::Record {" << std::endl;
@@ -418,12 +424,7 @@ public:
   virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
       ExecutionTraceStorage* ptr) const {
     assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
-    Record* record = new (ptr) Record();
+    Record* record = new (ptr) Record(values, *expression1_, *expression2_, ptr);
-    ptr += upAligned(sizeof(Record));
-    record->value1 = expression1_->traceExecution(values, record->trace1, ptr);
-    ptr += expression1_->traceSize();
-    record->value2 = expression2_->traceExecution(values, record->trace2, ptr);
-    ptr += expression2_->traceSize();
     trace.setFunction(record);
     return function_(record->value1, record->value2, record->dTdA1, record->dTdA2);
   }
@@ -492,18 +493,26 @@ public:
   // Inner Record Class
   struct Record: public CallRecordImplementor<Record, traits<T>::dimension> {
 
-    A1 value1;
-    ExecutionTrace<A1> trace1;
     typename Jacobian<T, A1>::type dTdA1;
-
-    A2 value2;
-    ExecutionTrace<A2> trace2;
     typename Jacobian<T, A2>::type dTdA2;
-
-    A3 value3;
-    ExecutionTrace<A3> trace3;
     typename Jacobian<T, A3>::type dTdA3;
 
+    ExecutionTrace<A1> trace1;
+    ExecutionTrace<A2> trace2;
+    ExecutionTrace<A3> trace3;
+
+    A1 value1;
+    A2 value2;
+    A3 value3;
+
+    /// Construct record by calling 3 argument expressions
+    Record(const Values& values, const ExpressionNode<A1>& expression1,
+           const ExpressionNode<A2>& expression2,
+           const ExpressionNode<A3>& expression3, ExecutionTraceStorage* ptr)
+        : value1(expression1.traceExecution(values, trace1, ptr += upAligned(sizeof(Record)))),
+          value2(expression2.traceExecution(values, trace2, ptr += expression1.traceSize())),
+          value3(expression3.traceExecution(values, trace3, ptr += expression2.traceSize())) {}
+
     /// Print to std::cout
     void print(const std::string& indent) const {
       std::cout << indent << "TernaryExpression::Record {" << std::endl;
@@ -531,19 +540,12 @@ public:
 
   /// Construct an execution trace for reverse AD, see UnaryExpression for explanation
   virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* ptr) const {
+                           ExecutionTraceStorage* ptr) const {
     assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
-    Record* record = new (ptr) Record();
+    Record* record = new (ptr) Record(values, *expression1_, *expression2_, *expression3_, ptr);
-    ptr += upAligned(sizeof(Record));
-    record->value1 = expression1_->traceExecution(values, record->trace1, ptr);
-    ptr += expression1_->traceSize();
-    record->value2 = expression2_->traceExecution(values, record->trace2, ptr);
-    ptr += expression2_->traceSize();
-    record->value3 = expression3_->traceExecution(values, record->trace3, ptr);
-    ptr += expression3_->traceSize();
     trace.setFunction(record);
     return function_(record->value1, record->value2, record->value3,
-        record->dTdA1, record->dTdA2, record->dTdA3);
+                     record->dTdA1, record->dTdA2, record->dTdA3);
   }
 };
 

tests/testExpressionFactor.cpp

@@ -186,25 +186,8 @@ TEST(ExpressionFactor, Binary) {
   values.insert(1, Cal3_S2());
   values.insert(2, Point2(0, 0));
 
-  // traceRaw will fill raw with [Trace<Point2> | Binary::Record]
-  EXPECT_LONGS_EQUAL(8, sizeof(double));
-  EXPECT_LONGS_EQUAL(16, sizeof(Point2));
-  EXPECT_LONGS_EQUAL(40, sizeof(Cal3_S2));
-  EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace<Point2>));
-  EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace<Cal3_S2>));
-  EXPECT_LONGS_EQUAL(2*5*8, sizeof(internal::Jacobian<Point2,Cal3_S2>::type));
-  EXPECT_LONGS_EQUAL(2*2*8, sizeof(internal::Jacobian<Point2,Point2>::type));
-  size_t expectedRecordSize = sizeof(Cal3_S2)
-      + sizeof(internal::ExecutionTrace<Cal3_S2>)
-      + +sizeof(internal::Jacobian<Point2, Cal3_S2>::type) + sizeof(Point2)
-      + sizeof(internal::ExecutionTrace<Point2>)
-      + sizeof(internal::Jacobian<Point2, Point2>::type);
-  EXPECT_LONGS_EQUAL(expectedRecordSize + 8, sizeof(Binary::Record));
-
   // Check size
   size_t size = binary.traceSize();
-  CHECK(size);
-  EXPECT_LONGS_EQUAL(expectedRecordSize + 8, size);
   // Use Variable Length Array, allocated on stack by gcc
   // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla
   internal::ExecutionTraceStorage traceStorage[size];
@@ -261,18 +244,7 @@ TEST(ExpressionFactor, Shallow) {
   // traceExecution of shallow tree
   typedef internal::UnaryExpression<Point2, Point3> Unary;
   typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
-  size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record);
-  EXPECT_LONGS_EQUAL(96, sizeof(Unary::Record));
-#ifdef GTSAM_USE_QUATERNIONS
-  EXPECT_LONGS_EQUAL(352, sizeof(Binary::Record));
-  LONGS_EQUAL(96+352, expectedTraceSize);
-#else
-  EXPECT_LONGS_EQUAL(384, sizeof(Binary::Record));
-  LONGS_EQUAL(96+384, expectedTraceSize);
-#endif
   size_t size = expression.traceSize();
-  CHECK(size);
-  EXPECT_LONGS_EQUAL(expectedTraceSize, size);
   internal::ExecutionTraceStorage traceStorage[size];
   internal::ExecutionTrace<Point2> trace;
   Point2 value = expression.traceExecution(values, trace, traceStorage);