From cc0b7cfdc1ccd2fc9a6195f01b6a07ae691d6c50 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Tue, 9 Feb 2016 20:00:38 -0500 Subject: [PATCH 1/3] convert tabs to spaces. See https://bitbucket.org/gtborg/gtsam/wiki/C++%20Coding%20Conventions --- examples/RangeISAMExample_plaza2.cpp | 2 +- gtsam/base/Matrix.cpp | 2 +- .../inference/EliminateableFactorGraph-inst.h | 18 +++---- gtsam/inference/EliminateableFactorGraph.h | 18 +++---- gtsam/linear/JacobianFactor.cpp | 6 +-- gtsam/nonlinear/NonlinearOptimizer.cpp | 4 +- gtsam/nonlinear/NonlinearOptimizerParams.cpp | 54 ++++++++++--------- gtsam/nonlinear/NonlinearOptimizerParams.h | 6 +-- 8 files changed, 56 insertions(+), 54 deletions(-) diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 4d116c7ec..01ce8b08b 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -83,7 +83,7 @@ vector 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)); } diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index c6af89486..740699d4b 100644 --- a/gtsam/base/Matrix.cpp +++ b/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 diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index b4fc3b3a6..98a3545f6 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -28,8 +28,8 @@ namespace gtsam { template boost::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( - OptionalOrdering ordering, const Eliminate& function, - OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const + OptionalOrdering ordering, const Eliminate& function, + OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const { if(ordering && variableIndex) { gttic(eliminateSequential); @@ -65,8 +65,8 @@ namespace gtsam { template boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( - OptionalOrdering ordering, const Eliminate& function, - OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const + OptionalOrdering ordering, const Eliminate& function, + 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) - return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType); - else - return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); + if (orderingType == Ordering::METIS) + return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType); + else + return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index f5431db3d..891f22e61 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/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 OptionalVariableIndex; - /// Typedef for an optional ordering type - typedef boost::optional OptionalOrderingType; + /// Typedef for an optional ordering type + typedef boost::optional 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 result = graph.eliminateSequential(EliminateCholesky); * \endcode - * - * Example - METIS ordering for elimination - * \code - * boost::shared_ptr result = graph.eliminateSequential(OrderingType::METIS); + * + * Example - METIS ordering for elimination + * \code + * boost::shared_ptr result = graph.eliminateSequential(OrderingType::METIS); * * 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 eliminateMultifrontal( OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; + OptionalVariableIndex variableIndex = boost::none, + 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$, diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index d4df57298..47b6ec90b 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/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 - "", // rowSuffix + "\t", // rowPrefix + "", // rowSuffix "[\n", // matPrefix "\n ]" // matSuffix ); diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 77d26d361..2c752815e 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/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(), - boost::none, params.orderingType)->optimize(); + delta = gfg.eliminateSequential(*params.ordering, + params.getEliminationFunction(), boost::none, params.orderingType)->optimize(); } else if (params.isIterative()) { // Conjugate Gradient -> needs params.iterativeParams diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 5a163ffb9..91edd8f93 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/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"; - break; + std::cout << " ordering: COLAMD\n"; + break; case Ordering::METIS: - std::cout << " ordering: METIS\n"; - break; + std::cout << " ordering: METIS\n"; + break; default: - std::cout << " ordering: custom\n"; - break; + std::cout << " ordering: custom\n"; + break; } std::cout.flush(); @@ -165,29 +165,31 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve } /* ************************************************************************* */ -std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::OrderingType type) const{ - switch (type) { - case Ordering::METIS: - return "METIS"; - case Ordering::COLAMD: - return "COLAMD"; - default: - if (ordering) - return "CUSTOM"; - else - throw std::invalid_argument( - "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); - } +std::string NonlinearOptimizerParams::orderingTypeTranslator( + Ordering::OrderingType type) const { + switch (type) { + case Ordering::METIS: + return "METIS"; + case Ordering::COLAMD: + return "COLAMD"; + default: + if (ordering) + return "CUSTOM"; + else + 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{ - if (type == "METIS") - return Ordering::METIS; - if (type == "COLAMD") - return Ordering::COLAMD; - 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 { + if (type == "METIS") + return Ordering::METIS; + if (type == "COLAMD") + return Ordering::COLAMD; + throw std::invalid_argument( + "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); } diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 10de6994f..ca75bb02a 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/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: From 26d74a1f6e3133d923243bd26358868e837ca120 Mon Sep 17 00:00:00 2001 From: Frank Date: Thu, 11 Feb 2016 19:04:36 -0800 Subject: [PATCH 2/3] Modified ExpressionNode so avoids calling constructors altogether --- gtsam/nonlinear/internal/ExpressionNode.h | 84 ++++++++++++----------- 1 file changed, 43 insertions(+), 41 deletions(-) diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 928387eb9..d2a130830 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -269,9 +269,13 @@ public: // Inner Record Class struct Record: public CallRecordImplementor::dimension> { - A1 value1; - ExecutionTrace trace1; typename Jacobian::type dTdA1; + ExecutionTrace trace1; + A1 value1; + + /// Construct record by calling argument expression + Record(const Values& values, const ExpressionNode& 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(ptr) % TraceAlignment == 0); - // Create the record at the start of the traceStorage and advance the pointer - Record* record = new (ptr) Record(); - ptr += upAligned(sizeof(Record)); - - // Record the traces for all arguments - // After this, the traceStorage pointer is set to after what was written + // Create a Record in the memory pointed to by ptr + // Calling the construct will record the traces for all arguments // Write an Expression 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 - ptr += expression1_->traceSize(); + // Our trace parameter is set to point to the Record trace.setFunction(record); // Finally, the function call fills in the Jacobian dTdA1 @@ -384,14 +383,21 @@ public: // Inner Record Class struct Record: public CallRecordImplementor::dimension> { - A1 value1; - ExecutionTrace trace1; typename Jacobian::type dTdA1; - - A2 value2; - ExecutionTrace trace2; typename Jacobian::type dTdA2; + ExecutionTrace trace1; + ExecutionTrace trace2; + + A1 value1; + A2 value2; + + /// Construct record by calling argument expressions + Record(const Values& values, const ExpressionNode& expression1, + const ExpressionNode& 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& trace, ExecutionTraceStorage* ptr) const { assert(reinterpret_cast(ptr) % TraceAlignment == 0); - Record* record = new (ptr) Record(); - 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* record = new (ptr) Record(values, *expression1_, *expression2_, ptr); 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::dimension> { - A1 value1; - ExecutionTrace trace1; typename Jacobian::type dTdA1; - - A2 value2; - ExecutionTrace trace2; typename Jacobian::type dTdA2; - - A3 value3; - ExecutionTrace trace3; typename Jacobian::type dTdA3; + ExecutionTrace trace1; + ExecutionTrace trace2; + ExecutionTrace trace3; + + A1 value1; + A2 value2; + A3 value3; + + /// Construct record by calling 3 argument expressions + Record(const Values& values, const ExpressionNode& expression1, + const ExpressionNode& expression2, + const ExpressionNode& 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& trace, - ExecutionTraceStorage* ptr) const { + ExecutionTraceStorage* ptr) const { assert(reinterpret_cast(ptr) % TraceAlignment == 0); - Record* record = new (ptr) Record(); - 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(); + Record* record = new (ptr) Record(values, *expression1_, *expression2_, *expression3_, ptr); trace.setFunction(record); return function_(record->value1, record->value2, record->value3, - record->dTdA1, record->dTdA2, record->dTdA3); + record->dTdA1, record->dTdA2, record->dTdA3); } }; From 5c3dd7914b7ab93e6c70ff1cc282f2fabfed0c3f Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 11 Feb 2016 22:36:03 -0800 Subject: [PATCH 3/3] Removed checks on sizeof(Record) as non-portable. --- tests/testExpressionFactor.cpp | 28 ---------------------------- 1 file changed, 28 deletions(-) diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index bef69edb6..57f03cca6 100644 --- a/tests/testExpressionFactor.cpp +++ b/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 | 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)); - EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace)); - EXPECT_LONGS_EQUAL(2*5*8, sizeof(internal::Jacobian::type)); - EXPECT_LONGS_EQUAL(2*2*8, sizeof(internal::Jacobian::type)); - size_t expectedRecordSize = sizeof(Cal3_S2) - + sizeof(internal::ExecutionTrace) - + +sizeof(internal::Jacobian::type) + sizeof(Point2) - + sizeof(internal::ExecutionTrace) - + sizeof(internal::Jacobian::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 Unary; typedef internal::BinaryExpression 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 trace; Point2 value = expression.traceExecution(values, trace, traceStorage);