diff --git a/.cproject b/.cproject index b0ad23ee6..ca640b7b1 100644 --- a/.cproject +++ b/.cproject @@ -311,6 +311,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -337,7 +345,6 @@ make - tests/testBayesTree.run true false @@ -345,7 +352,6 @@ make - testBinaryBayesNet.run true false @@ -393,7 +399,6 @@ make - testSymbolicBayesNet.run true false @@ -401,7 +406,6 @@ make - tests/testSymbolicFactor.run true false @@ -409,7 +413,6 @@ make - testSymbolicFactorGraph.run true false @@ -425,20 +428,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -527,22 +521,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -559,6 +537,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -583,26 +577,26 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run true true true @@ -687,26 +681,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run + -j2 + clean true true true @@ -799,6 +793,14 @@ true true + + make + -j5 + testInference.run + true + true + true + make -j2 @@ -953,7 +955,6 @@ make - testGraph.run true false @@ -961,7 +962,6 @@ make - testJunctionTree.run true false @@ -969,7 +969,6 @@ make - testSymbolicBayesNetB.run true false @@ -1089,7 +1088,6 @@ make - testErrors.run true false @@ -1545,6 +1543,7 @@ make + testSimulated2DOriented.run true false @@ -1584,6 +1583,7 @@ make + testSimulated2D.run true false @@ -1591,6 +1591,7 @@ make + testSimulated3D.run true false @@ -1806,6 +1807,7 @@ make + tests/testGaussianISAM2 true false @@ -1827,6 +1829,102 @@ true true + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + make -j2 @@ -2028,7 +2126,6 @@ cpack - -G DEB true false @@ -2036,7 +2133,6 @@ cpack - -G RPM true false @@ -2044,7 +2140,6 @@ cpack - -G TGZ true false @@ -2052,7 +2147,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2146,98 +2240,42 @@ true true - + make - -j2 - testRot3.run + -j5 + testSpirit.run true true true - + make - -j2 - testRot2.run + -j5 + testWrap.run true true true - + make - -j2 - testPose3.run + -j5 + check.wrap true true true - + make - -j2 - timeRot3.run + -j5 + wrap_gtsam true true true - + make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run + -j5 + wrap true true true @@ -2281,46 +2319,6 @@ false true - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap_gtsam - true - true - true - - - make - -j5 - wrap - true - true - true - diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index dc3aa669a..5454dafec 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -40,6 +40,10 @@ namespace gtsam { * arguments supplied through the square-bracket [] operator through the * permutation. Note that this helper class stores a reference to the original * container. + * + * Permutations can be considered to a 1-1 mapping from an original set of indices + * to a different set of indices. Permutations can be composed and inverted + * in order to create new indexing for a structure. * \nosubgrouping */ class Permutation { diff --git a/gtsam/inference/inference.cpp b/gtsam/inference/inference.cpp index 7afad8330..3cddc4cfc 100644 --- a/gtsam/inference/inference.cpp +++ b/gtsam/inference/inference.cpp @@ -29,9 +29,9 @@ using namespace std; namespace gtsam { - namespace inference { +/* ************************************************************************* */ Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector& cmember) { size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); // Convert to compressed column major format colamd wants it in (== MATLAB format!) @@ -93,6 +93,6 @@ Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, s return permutation; } -} - -} +/* ************************************************************************* */ +} // \namespace inference +} // \namespace gtsam diff --git a/gtsam/inference/inference.h b/gtsam/inference/inference.h index 15373e62a..6ee278c68 100644 --- a/gtsam/inference/inference.h +++ b/gtsam/inference/inference.h @@ -38,7 +38,11 @@ namespace gtsam { const VariableIndex& variableIndex); /** - * Compute a permutation (variable ordering) using constrained colamd + * Compute a permutation (variable ordering) using constrained colamd to move + * a set of variables to the end of the ordering + * @param variableIndex is the variable index lookup from a graph + * @param constrainlast is a vector of keys that should be constrained + * @tparam constrainLast is a std::vector (or similar structure) */ template Permutation::shared_ptr PermutationCOLAMD( @@ -46,6 +50,13 @@ namespace gtsam { /** * Compute a CCOLAMD permutation using the constraint groups in cmember. + * + * @param variableIndex is the variable structure from a graph + * @param cmember is the constraint group list for each variable, where + * 0 is the default, unconstrained group, and higher numbers move further to + * the back of the list + * + * AGC: does cmember change? */ Permutation::shared_ptr PermutationCOLAMD_( const VariableIndex& variableIndex, std::vector& cmember); @@ -82,8 +93,8 @@ namespace gtsam { return eliminate(factorGraph, variables, eliminateFcn, variableIndex); } - } // namespace inference + } // \namespace inference -} // namespace gtsam +} // \namespace gtsam #include diff --git a/gtsam/inference/tests/testInference.cpp b/gtsam/inference/tests/testInference.cpp index 41c33a456..2523eb32b 100644 --- a/gtsam/inference/tests/testInference.cpp +++ b/gtsam/inference/tests/testInference.cpp @@ -11,8 +11,8 @@ /** * @file testInference.cpp - * @brief * @author Richard Roberts + * @author Alex Cunningham * @date Dec 6, 2010 */ @@ -35,8 +35,52 @@ TEST(inference, UnobservedVariables) { VariableIndex variableIndex(sfg); - Permutation::shared_ptr colamd(inference::PermutationCOLAMD(variableIndex)); + // Computes a permutation with known variables first, skipped variables last + // Actual 0 1 3 5 2 4 + Permutation::shared_ptr actual(inference::PermutationCOLAMD(variableIndex)); + Permutation expected(6); + expected[0] = 0; + expected[1] = 1; + expected[2] = 3; + expected[3] = 5; + expected[4] = 2; + expected[5] = 4; + EXPECT(assert_equal(expected, *actual)); +} + +/* ************************************************************************* */ +TEST(inference, constrained_ordering) { + SymbolicFactorGraph sfg; + + // create graph with wanted variable set = 2, 4 + sfg.push_factor(0,1); + sfg.push_factor(1,2); + sfg.push_factor(2,3); + sfg.push_factor(3,4); + sfg.push_factor(4,5); + + VariableIndex variableIndex(sfg); + + // unconstrained version + Permutation::shared_ptr actUnconstrained(inference::PermutationCOLAMD(variableIndex)); + Permutation expUnconstrained = Permutation::Identity(6); + EXPECT(assert_equal(expUnconstrained, *actUnconstrained)); + + // constrained version - push one set to the end + std::vector constrainLast; + constrainLast.push_back(2); + constrainLast.push_back(4); + Permutation::shared_ptr actConstrained(inference::PermutationCOLAMD(variableIndex, constrainLast)); + Permutation expConstrained(6); + expConstrained[0] = 0; + expConstrained[1] = 1; + expConstrained[2] = 5; + expConstrained[3] = 3; + expConstrained[4] = 4; + expConstrained[5] = 2; + EXPECT(assert_equal(expConstrained, *actConstrained)); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index a2ef3793b..984284ebd 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -232,8 +232,12 @@ struct ISAM2Result { boost::optional detail; }; -struct ISAM2Clique : public BayesTreeCliqueBase { - +/** + * Specialized Clique structure for ISAM2, incorporating caching and gradient contribution + * TODO: more documentation + */ +class ISAM2Clique : public BayesTreeCliqueBase { +public: typedef ISAM2Clique This; typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; @@ -268,7 +272,7 @@ struct ISAM2Clique : public BayesTreeCliqueBase @@ -308,7 +313,7 @@ private: ar & BOOST_SERIALIZATION_NVP(cachedFactor_); ar & BOOST_SERIALIZATION_NVP(gradientContribution_); } -}; +}; // \struct ISAM2Clique /** * @addtogroup ISAM2 @@ -442,6 +447,8 @@ public: * @param force_relinearize Relinearize any variables whose delta magnitude is sufficiently * large (Params::relinearizeThreshold), regardless of the relinearization interval * (Params::relinearizeSkip). + * @param constrainedKeys is an optional map of keys to group labels, such that a variable can + * be constrained to a particular grouping in the BayesTree * @return An ISAM2Result struct containing information about the update */ ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), @@ -450,7 +457,7 @@ public: bool force_relinearize = false); /** Access the current linearization point */ - const Values& getLinearizationPoint() const {return theta_;} + const Values& getLinearizationPoint() const { return theta_; } /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only @@ -496,7 +503,7 @@ public: mutable size_t lastBacksubVariableCount; size_t lastNnzTop; - ISAM2Params params() const { return params_; } + const ISAM2Params& params() const { return params_; } //@} diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index 393e4bacc..7033613ac 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -105,6 +105,7 @@ public: default: throw std::runtime_error("Nonlinear optimization parameter \"factorization\" is invalid"); + return EliminateQR; break; } }