diff --git a/gtsam/geometry/tests/testSimilarity2.cpp b/gtsam/geometry/tests/testSimilarity2.cpp index dd4fd0efd..ca041fc7b 100644 --- a/gtsam/geometry/tests/testSimilarity2.cpp +++ b/gtsam/geometry/tests/testSimilarity2.cpp @@ -33,8 +33,6 @@ static const Point2 P(0.2, 0.7); static const Rot2 R = Rot2::fromAngle(0.3); static const double s = 4; -const double degree = M_PI / 180; - //****************************************************************************** TEST(Similarity2, Concepts) { BOOST_CONCEPT_ASSERT((IsGroup)); diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index a9fe62cf1..e9d64b283 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -52,7 +52,6 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, HybridFactor::HybridFactor(const KeyVector &keys) : Base(keys), isContinuous_(true), - nrContinuous_(keys.size()), continuousKeys_(keys) {} /* ************************************************************************ */ @@ -62,7 +61,6 @@ HybridFactor::HybridFactor(const KeyVector &continuousKeys, isDiscrete_((continuousKeys.size() == 0) && (discreteKeys.size() != 0)), isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)), isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)), - nrContinuous_(continuousKeys.size()), discreteKeys_(discreteKeys), continuousKeys_(continuousKeys) {} diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index b3cdc231b..165dfde29 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -47,9 +47,6 @@ class GTSAM_EXPORT HybridFactor : public Factor { bool isContinuous_ = false; bool isHybrid_ = false; - // TODO(Varun) remove - size_t nrContinuous_ = 0; - protected: // Set of DiscreteKeys for this factor. DiscreteKeys discreteKeys_; diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 7725742cf..422c200a4 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -31,9 +31,7 @@ template class EliminatableClusterTree; struct HybridConstructorTraversalData { - typedef - typename JunctionTree::Node - Node; + typedef HybridJunctionTree::Node Node; typedef typename JunctionTree::sharedNode sharedNode; @@ -62,6 +60,7 @@ struct HybridConstructorTraversalData { data.junctionTreeNode = boost::make_shared(node->key, node->factors); parentData.junctionTreeNode->addChild(data.junctionTreeNode); + // Add all the discrete keys in the hybrid factors to the current data for (HybridFactor::shared_ptr& f : node->factors) { for (auto& k : f->discreteKeys()) { data.discreteKeys.insert(k.first); @@ -72,8 +71,8 @@ struct HybridConstructorTraversalData { } // Post-order visitor function - static void ConstructorTraversalVisitorPostAlg2( - const boost::shared_ptr& ETreeNode, + static void ConstructorTraversalVisitorPost( + const boost::shared_ptr& node, const HybridConstructorTraversalData& data) { // In this post-order visitor, we combine the symbolic elimination results // from the elimination tree children and symbolically eliminate the current @@ -86,15 +85,15 @@ struct HybridConstructorTraversalData { // Do symbolic elimination for this node SymbolicFactors symbolicFactors; - symbolicFactors.reserve(ETreeNode->factors.size() + + symbolicFactors.reserve(node->factors.size() + data.childSymbolicFactors.size()); // Add ETree node factors - symbolicFactors += ETreeNode->factors; + symbolicFactors += node->factors; // Add symbolic factors passed up from children symbolicFactors += data.childSymbolicFactors; Ordering keyAsOrdering; - keyAsOrdering.push_back(ETreeNode->key); + keyAsOrdering.push_back(node->key); SymbolicConditional::shared_ptr conditional; SymbolicFactor::shared_ptr separatorFactor; boost::tie(conditional, separatorFactor) = @@ -105,19 +104,19 @@ struct HybridConstructorTraversalData { data.parentData->childSymbolicFactors.push_back(separatorFactor); data.parentData->discreteKeys.merge(data.discreteKeys); - sharedNode node = data.junctionTreeNode; + sharedNode jt_node = data.junctionTreeNode; const FastVector& childConditionals = data.childSymbolicConditionals; - node->problemSize_ = (int)(conditional->size() * symbolicFactors.size()); + jt_node->problemSize_ = (int)(conditional->size() * symbolicFactors.size()); // Merge our children if they are in our clique - if our conditional has // exactly one fewer parent than our child's conditional. const size_t nrParents = conditional->nrParents(); - const size_t nrChildren = node->nrChildren(); + const size_t nrChildren = jt_node->nrChildren(); assert(childConditionals.size() == nrChildren); // decide which children to merge, as index into children - std::vector nrChildrenFrontals = node->nrFrontalsOfChildren(); + std::vector nrChildrenFrontals = jt_node->nrFrontalsOfChildren(); std::vector merge(nrChildren, false); size_t nrFrontals = 1; for (size_t i = 0; i < nrChildren; i++) { @@ -137,7 +136,7 @@ struct HybridConstructorTraversalData { } // now really merge - node->mergeChildren(merge); + jt_node->mergeChildren(merge); } }; @@ -161,7 +160,7 @@ HybridJunctionTree::HybridJunctionTree( // the junction tree roots treeTraversal::DepthFirstForest(eliminationTree, rootData, Data::ConstructorTraversalVisitorPre, - Data::ConstructorTraversalVisitorPostAlg2); + Data::ConstructorTraversalVisitorPost); // Assign roots from the dummy node this->addChildrenAsRoots(rootData.junctionTreeNode); diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 6f41d06ea..ecd90e234 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -128,9 +128,6 @@ struct Switching { /// Create with given number of time steps. Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1) : K(K) { - using symbol_shorthand::M; - using symbol_shorthand::X; - // Create DiscreteKeys for binary K modes, modes[0] will not be used. for (size_t k = 0; k <= K; k++) { modes.emplace_back(M(k), 2); @@ -175,9 +172,6 @@ struct Switching { // Create motion models for a given time step static std::vector motionModels(size_t k, double sigma = 1.0) { - using symbol_shorthand::M; - using symbol_shorthand::X; - auto noise_model = noiseModel::Isotropic::Sigma(1, sigma); auto still = boost::make_shared(X(k), X(k + 1), 0.0, noise_model), diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 018b017a9..3723e6078 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -257,14 +257,6 @@ TEST(GaussianElimination, Eliminate_x1) { // Add first hybrid factor factors.push_back(self.linearizedFactorGraph[1]); - // TODO(Varun) remove this block since sum is no longer exposed. - // // Check that sum works: - // auto sum = factors.sum(); - // Assignment mode; - // mode[M(1)] = 1; - // auto actual = sum(mode); // Selects one of 2 modes. - // EXPECT_LONGS_EQUAL(2, actual.size()); // Prior and motion model. - // Eliminate x1 Ordering ordering; ordering += X(1); @@ -289,15 +281,6 @@ TEST(HybridsGaussianElimination, Eliminate_x2) { factors.push_back(self.linearizedFactorGraph[1]); // involves m1 factors.push_back(self.linearizedFactorGraph[2]); // involves m2 - // TODO(Varun) remove this block since sum is no longer exposed. - // // Check that sum works: - // auto sum = factors.sum(); - // Assignment mode; - // mode[M(1)] = 0; - // mode[M(2)] = 1; - // auto actual = sum(mode); // Selects one of 4 mode - // combinations. EXPECT_LONGS_EQUAL(2, actual.size()); // 2 motion models. - // Eliminate x2 Ordering ordering; ordering += X(2); @@ -364,51 +347,10 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { CHECK(discreteFactor); EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size()); EXPECT(discreteFactor->root_->isLeaf() == false); + + //TODO(Varun) Test emplace_discrete } -// /* -// ****************************************************************************/ -// /// Test the toDecisionTreeFactor method -// TEST(HybridFactorGraph, ToDecisionTreeFactor) { -// size_t K = 3; - -// // Provide tight sigma values so that the errors are visibly different. -// double between_sigma = 5e-8, prior_sigma = 1e-7; - -// Switching self(K, between_sigma, prior_sigma); - -// // Clear out discrete factors since sum() cannot hanldle those -// HybridGaussianFactorGraph linearizedFactorGraph( -// self.linearizedFactorGraph.gaussianGraph(), DiscreteFactorGraph(), -// self.linearizedFactorGraph.dcGraph()); - -// auto decisionTreeFactor = linearizedFactorGraph.toDecisionTreeFactor(); - -// auto allAssignments = -// DiscreteValues::CartesianProduct(linearizedFactorGraph.discreteKeys()); - -// // Get the error of the discrete assignment m1=0, m2=1. -// double actual = (*decisionTreeFactor)(allAssignments[1]); - -// /********************************************/ -// // Create equivalent factor graph for m1=0, m2=1 -// GaussianFactorGraph graph = linearizedFactorGraph.gaussianGraph(); - -// for (auto &p : linearizedFactorGraph.dcGraph()) { -// if (auto mixture = -// boost::dynamic_pointer_cast(p)) { -// graph.add((*mixture)(allAssignments[1])); -// } -// } - -// VectorValues values = graph.optimize(); -// double expected = graph.probPrime(values); -// /********************************************/ -// EXPECT_DOUBLES_EQUAL(expected, actual, 1e-12); -// // REGRESSION: -// EXPECT_DOUBLES_EQUAL(0.6125, actual, 1e-4); -// } - /**************************************************************************** * Test partial elimination */ @@ -428,7 +370,6 @@ TEST(HybridFactorGraph, Partial_Elimination) { linearizedFactorGraph.eliminatePartialSequential(ordering); CHECK(hybridBayesNet); - // GTSAM_PRINT(*hybridBayesNet); // HybridBayesNet EXPECT_LONGS_EQUAL(3, hybridBayesNet->size()); EXPECT(hybridBayesNet->at(0)->frontals() == KeyVector{X(1)}); EXPECT(hybridBayesNet->at(0)->parents() == KeyVector({X(2), M(1)})); @@ -438,7 +379,6 @@ TEST(HybridFactorGraph, Partial_Elimination) { EXPECT(hybridBayesNet->at(2)->parents() == KeyVector({M(1), M(2)})); CHECK(remainingFactorGraph); - // GTSAM_PRINT(*remainingFactorGraph); // HybridFactorGraph EXPECT_LONGS_EQUAL(3, remainingFactorGraph->size()); EXPECT(remainingFactorGraph->at(0)->keys() == KeyVector({M(1)})); EXPECT(remainingFactorGraph->at(1)->keys() == KeyVector({M(2), M(1)})); @@ -721,13 +661,8 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { moving = boost::make_shared(X(0), X(1), odometry, noise_model); std::vector motion_models = {still, moving}; - // TODO(Varun) Make a templated constructor for MixtureFactor which does this? - std::vector components; - for (auto&& f : motion_models) { - components.push_back(boost::dynamic_pointer_cast(f)); - } fg.emplace_hybrid( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models); // Add Range-Bearing measurements to from X0 to L0 and X1 to L1. // create a noise model for the landmark measurements diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 04472f7e3..1c68aa0da 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -33,7 +33,7 @@ struct ConstructorTraversalData { typedef typename JunctionTree::sharedNode sharedNode; ConstructorTraversalData* const parentData; - sharedNode myJTNode; + sharedNode junctionTreeNode; FastVector childSymbolicConditionals; FastVector childSymbolicFactors; @@ -53,8 +53,9 @@ struct ConstructorTraversalData { // a traversal data structure with its own JT node, and create a child // pointer in its parent. ConstructorTraversalData myData = ConstructorTraversalData(&parentData); - myData.myJTNode = boost::make_shared(node->key, node->factors); - parentData.myJTNode->addChild(myData.myJTNode); + myData.junctionTreeNode = + boost::make_shared(node->key, node->factors); + parentData.junctionTreeNode->addChild(myData.junctionTreeNode); return myData; } @@ -91,7 +92,7 @@ struct ConstructorTraversalData { myData.parentData->childSymbolicConditionals.push_back(myConditional); myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor); - sharedNode node = myData.myJTNode; + sharedNode node = myData.junctionTreeNode; const FastVector& childConditionals = myData.childSymbolicConditionals; node->problemSize_ = (int) (myConditional->size() * symbolicFactors.size()); @@ -138,14 +139,14 @@ JunctionTree::JunctionTree( typedef typename EliminationTree::Node ETreeNode; typedef ConstructorTraversalData Data; Data rootData(0); - rootData.myJTNode = boost::make_shared(); // Make a dummy node to gather - // the junction tree roots + // Make a dummy node to gather the junction tree roots + rootData.junctionTreeNode = boost::make_shared(); treeTraversal::DepthFirstForest(eliminationTree, rootData, Data::ConstructorTraversalVisitorPre, Data::ConstructorTraversalVisitorPostAlg2); // Assign roots from the dummy node - this->addChildrenAsRoots(rootData.myJTNode); + this->addChildrenAsRoots(rootData.junctionTreeNode); // Transfer remaining factors from elimination tree Base::remainingFactors_ = eliminationTree.remainingFactors();