diff --git a/examples/UnorderedLinear.cpp b/examples/UnorderedLinear.cpp index ff2fb0f52..f3d0014e4 100644 --- a/examples/UnorderedLinear.cpp +++ b/examples/UnorderedLinear.cpp @@ -129,15 +129,15 @@ ISAM2 solveWithOldISAM2(const NonlinearFactorGraph& measurements) } /* ************************************************************************* */ -GaussianFactorGraph convertToUnordered(const GaussianFactorGraphOrdered& gfg, const OrderingOrdered& ordering) +GaussianFactorGraph convertToUnordered(const GaussianFactorGraph& gfg, const Ordering& ordering) { GaussianFactorGraph gfgu; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, gfg) + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) { vector > terms; - const JacobianFactorOrdered& jacobian = dynamic_cast(*factor); - for(GaussianFactorOrdered::const_iterator term = jacobian.begin(); term != jacobian.end(); ++term) + const JacobianFactor& jacobian = dynamic_cast(*factor); + for(GaussianFactor::const_iterator term = jacobian.begin(); term != jacobian.end(); ++term) { terms.push_back(make_pair( ordering.key(*term), @@ -150,7 +150,7 @@ GaussianFactorGraph convertToUnordered(const GaussianFactorGraphOrdered& gfg, co } /* ************************************************************************* */ -void compareSolutions(const VectorValuesOrdered& orderedSoln, const OrderingOrdered& ordering, const VectorValues& unorderedSoln) +void compareSolutions(const VectorValues& orderedSoln, const Ordering& ordering, const VectorValues& unorderedSoln) { if(orderedSoln.size() != unorderedSoln.size()) { @@ -217,9 +217,9 @@ int main(int argc, char *argv[]) // Get linear graph cout << "Converting to unordered linear graph" << endl; - OrderingOrdered ordering = *isamsoln.orderingArbitrary(); - OrderingOrdered orderingCOLAMD = *nlfg.orderingCOLAMD(isamsoln); - GaussianFactorGraphOrdered gfg = *nlfg.linearize(isamsoln, ordering); + Ordering ordering = *isamsoln.orderingArbitrary(); + Ordering orderingCOLAMD = *nlfg.orderingCOLAMD(isamsoln); + GaussianFactorGraph gfg = *nlfg.linearize(isamsoln, ordering); GaussianFactorGraph gfgu = convertToUnordered(gfg, ordering); //Ordering orderingUnordered; @@ -233,9 +233,9 @@ int main(int argc, char *argv[]) gttic_(Solve_unordered); VectorValues unorderedSoln; for(size_t i = 0; i < 1; ++i) { - gttic_(VariableIndexOrdered); + gttic_(VariableIndex); VariableIndex vi(gfgu); - gttoc_(VariableIndexOrdered); + gttoc_(VariableIndex); gttic_(COLAMD); Ordering orderingUnordered = Ordering::COLAMD(vi); gttoc_(COLAMD); @@ -252,22 +252,22 @@ int main(int argc, char *argv[]) // Solve linear graph with old code cout << "Optimizing using old ordered code" << endl; - VectorValuesOrdered orderedSolnFinal; + VectorValues orderedSolnFinal; { - OrderingOrdered orderingToUse = ordering; - GaussianFactorGraphOrdered::shared_ptr orderedGraph = nlfg.linearize(isamsoln, *nlfg.orderingCOLAMD(isamsoln)); + Ordering orderingToUse = ordering; + GaussianFactorGraph::shared_ptr orderedGraph = nlfg.linearize(isamsoln, *nlfg.orderingCOLAMD(isamsoln)); gttic_(Solve_ordered); - VectorValuesOrdered orderedSoln; + VectorValues orderedSoln; for(size_t i = 0; i < 1; ++i) { - gttic_(VariableIndexOrdered); - boost::shared_ptr vi = boost::make_shared(gfg); - gttoc_(VariableIndexOrdered); + gttic_(VariableIndex); + boost::shared_ptr vi = boost::make_shared(gfg); + gttoc_(VariableIndex); gttic_(COLAMD); boost::shared_ptr permutation = inference::PermutationCOLAMD(*vi); orderingToUse.permuteInPlace(*permutation); gttoc_(COLAMD); gttic_(eliminate); - boost::shared_ptr bt = GaussianMultifrontalSolver(*orderedGraph, true).eliminate(); + boost::shared_ptr bt = GaussianMultifrontalSolver(*orderedGraph, true).eliminate(); gttoc_(eliminate); gttic_(optimize); orderedSoln = optimize(*bt); diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 69ef31931..3c18b220a 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -38,10 +38,10 @@ int main() { // [code below basically does SRIF with Cholesky] // Create a factor graph to perform the inference - GaussianFactorGraphOrdered::shared_ptr linearFactorGraph(new GaussianFactorGraphOrdered); + GaussianFactorGraph::shared_ptr linearFactorGraph(new GaussianFactorGraph); // Create the desired ordering - OrderingOrdered::shared_ptr ordering(new OrderingOrdered); + Ordering::shared_ptr ordering(new Ordering); // Create a structure to hold the linearization points Values linearizationPoints; @@ -110,10 +110,10 @@ int main() { // Solve the linear factor graph, converting it into a linear Bayes Network ( P(x0,x1) = P(x0|x1)*P(x1) ) GaussianSequentialSolver solver0(*linearFactorGraph); - GaussianBayesNetOrdered::shared_ptr linearBayesNet = solver0.eliminate(); + GaussianBayesNet::shared_ptr linearBayesNet = solver0.eliminate(); // Extract the current estimate of x1,P1 from the Bayes Network - VectorValuesOrdered result = optimize(*linearBayesNet); + VectorValues result = optimize(*linearBayesNet); Point2 x1_predict = linearizationPoints.at(x1).retract(result[ordering->at(x1)]); x1_predict.print("X1 Predict"); @@ -123,7 +123,7 @@ int main() { // Create a new, empty graph and add the prior from the previous step - linearFactorGraph = GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered); + linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph); // Convert the root conditional, P(x1) in this case, into a Prior for the next step // Some care must be done here, as the linearization point in future steps will be different @@ -138,13 +138,13 @@ int main() { // -> b'' = b' - F(dx1' - dx1'') // = || F*dx1'' - (b' - F(dx1' - dx1'')) ||^2 // = || F*dx1'' - (b' - F(x_predict - x_inital)) ||^2 - const GaussianConditionalOrdered::shared_ptr& cg0 = linearBayesNet->back(); + const GaussianConditional::shared_ptr& cg0 = linearBayesNet->back(); assert(cg0->nrFrontals() == 1); assert(cg0->nrParents() == 0); linearFactorGraph->add(0, cg0->get_R(), cg0->get_d() - cg0->get_R()*result[ordering->at(x1)], noiseModel::Diagonal::Sigmas(cg0->get_sigmas(), true)); // Create the desired ordering - ordering = OrderingOrdered::shared_ptr(new OrderingOrdered); + ordering = Ordering::shared_ptr(new Ordering); ordering->insert(x1, 0); // Now, a measurement, z1, has been received, and the Kalman Filter should be "Updated"/"Corrected" @@ -199,22 +199,22 @@ int main() { // Wash, rinse, repeat for another time step // Create a new, empty graph and add the prior from the previous step - linearFactorGraph = GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered); + linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph); // Convert the root conditional, P(x1) in this case, into a Prior for the next step // The linearization point of this prior must be moved to the new estimate of x, and the key/index needs to be reset to 0, // the first key in the next iteration - const GaussianConditionalOrdered::shared_ptr& cg1 = linearBayesNet->back(); + const GaussianConditional::shared_ptr& cg1 = linearBayesNet->back(); assert(cg1->nrFrontals() == 1); assert(cg1->nrParents() == 0); - JacobianFactorOrdered tmpPrior1 = JacobianFactorOrdered(*cg1); + JacobianFactor tmpPrior1 = JacobianFactor(*cg1); linearFactorGraph->add(0, tmpPrior1.getA(tmpPrior1.begin()), tmpPrior1.getb() - tmpPrior1.getA(tmpPrior1.begin()) * result[ordering->at(x1)], tmpPrior1.get_model()); // Create a key for the new state Symbol x2('x',2); // Create the desired ordering - ordering = OrderingOrdered::shared_ptr(new OrderingOrdered); + ordering = Ordering::shared_ptr(new Ordering); ordering->insert(x1, 0); ordering->insert(x2, 1); @@ -243,17 +243,17 @@ int main() { // Now add the next measurement // Create a new, empty graph and add the prior from the previous step - linearFactorGraph = GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered); + linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph); // Convert the root conditional, P(x1) in this case, into a Prior for the next step - const GaussianConditionalOrdered::shared_ptr& cg2 = linearBayesNet->back(); + const GaussianConditional::shared_ptr& cg2 = linearBayesNet->back(); assert(cg2->nrFrontals() == 1); assert(cg2->nrParents() == 0); - JacobianFactorOrdered tmpPrior2 = JacobianFactorOrdered(*cg2); + JacobianFactor tmpPrior2 = JacobianFactor(*cg2); linearFactorGraph->add(0, tmpPrior2.getA(tmpPrior2.begin()), tmpPrior2.getb() - tmpPrior2.getA(tmpPrior2.begin()) * result[ordering->at(x2)], tmpPrior2.get_model()); // Create the desired ordering - ordering = OrderingOrdered::shared_ptr(new OrderingOrdered); + ordering = Ordering::shared_ptr(new Ordering); ordering->insert(x2, 0); // And update using z2 ... @@ -290,20 +290,20 @@ int main() { // Wash, rinse, repeat for a third time step // Create a new, empty graph and add the prior from the previous step - linearFactorGraph = GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered); + linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph); // Convert the root conditional, P(x1) in this case, into a Prior for the next step - const GaussianConditionalOrdered::shared_ptr& cg3 = linearBayesNet->back(); + const GaussianConditional::shared_ptr& cg3 = linearBayesNet->back(); assert(cg3->nrFrontals() == 1); assert(cg3->nrParents() == 0); - JacobianFactorOrdered tmpPrior3 = JacobianFactorOrdered(*cg3); + JacobianFactor tmpPrior3 = JacobianFactor(*cg3); linearFactorGraph->add(0, tmpPrior3.getA(tmpPrior3.begin()), tmpPrior3.getb() - tmpPrior3.getA(tmpPrior3.begin()) * result[ordering->at(x2)], tmpPrior3.get_model()); // Create a key for the new state Symbol x3('x',3); // Create the desired ordering - ordering = OrderingOrdered::shared_ptr(new OrderingOrdered); + ordering = Ordering::shared_ptr(new Ordering); ordering->insert(x2, 0); ordering->insert(x3, 1); @@ -332,17 +332,17 @@ int main() { // Now add the next measurement // Create a new, empty graph and add the prior from the previous step - linearFactorGraph = GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered); + linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph); // Convert the root conditional, P(x1) in this case, into a Prior for the next step - const GaussianConditionalOrdered::shared_ptr& cg4 = linearBayesNet->back(); + const GaussianConditional::shared_ptr& cg4 = linearBayesNet->back(); assert(cg4->nrFrontals() == 1); assert(cg4->nrParents() == 0); - JacobianFactorOrdered tmpPrior4 = JacobianFactorOrdered(*cg4); + JacobianFactor tmpPrior4 = JacobianFactor(*cg4); linearFactorGraph->add(0, tmpPrior4.getA(tmpPrior4.begin()), tmpPrior4.getb() - tmpPrior4.getA(tmpPrior4.begin()) * result[ordering->at(x3)], tmpPrior4.get_model()); // Create the desired ordering - ordering = OrderingOrdered::shared_ptr(new OrderingOrdered); + ordering = Ordering::shared_ptr(new Ordering); ordering->insert(x3, 0); // And update using z3 ... diff --git a/gtsam/linear/GaussianBayesTree-inl.h b/gtsam/linear/GaussianBayesTree-inl.h index d6ddf6eb9..1f6c8e9f5 100644 --- a/gtsam/linear/GaussianBayesTree-inl.h +++ b/gtsam/linear/GaussianBayesTree-inl.h @@ -30,7 +30,7 @@ namespace gtsam { /* ************************************************************************* */ namespace internal { template -void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValuesOrdered& result) { +void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result) { // parents are assumed to already be solved and available in result clique->conditional()->solveInPlace(result); diff --git a/gtsam/linear/GaussianBayesTreeOrdered-inl.h b/gtsam/linear/GaussianBayesTreeOrdered-inl.h index d6ddf6eb9..1f6c8e9f5 100644 --- a/gtsam/linear/GaussianBayesTreeOrdered-inl.h +++ b/gtsam/linear/GaussianBayesTreeOrdered-inl.h @@ -30,7 +30,7 @@ namespace gtsam { /* ************************************************************************* */ namespace internal { template -void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValuesOrdered& result) { +void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result) { // parents are assumed to already be solved and available in result clique->conditional()->solveInPlace(result); diff --git a/gtsam/linear/GaussianConditional-inl.h b/gtsam/linear/GaussianConditional-inl.h index 5c0eb63a8..25dbe0e49 100644 --- a/gtsam/linear/GaussianConditional-inl.h +++ b/gtsam/linear/GaussianConditional-inl.h @@ -67,8 +67,8 @@ namespace gtsam { // Allocate combined conditional, has same keys as firstConditional Matrix tempCombined; VerticalBlockView tempBlockView(tempCombined, dims.begin(), dims.end(), 0); - GaussianConditionalOrdered::shared_ptr combinedConditional = - boost::make_shared( + GaussianConditional::shared_ptr combinedConditional = + boost::make_shared( (*firstConditional)->begin(), (*firstConditional)->end(), nFrontals, tempBlockView, Vector::Zero(nRows)); // Resize to correct number of rows diff --git a/gtsam/linear/tests/timeFactorOverhead.cpp b/gtsam/linear/tests/timeFactorOverhead.cpp index ea8ed8d12..6e5d6b9ad 100644 --- a/gtsam/linear/tests/timeFactorOverhead.cpp +++ b/gtsam/linear/tests/timeFactorOverhead.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -27,7 +27,7 @@ using namespace gtsam; using namespace std; -typedef EliminationTreeOrdered GaussianEliminationTree; +typedef EliminationTree GaussianEliminationTree; static boost::variate_generator > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0)); @@ -56,10 +56,10 @@ int main(int argc, char *argv[]) { cout.flush(); boost::timer timer; timer.restart(); - vector blockGfgs; + vector blockGfgs; blockGfgs.reserve(nTrials); for(size_t trial=0; trialeliminate(&EliminateQROrdered)); - VectorValuesOrdered soln(optimize(*gbn)); + GaussianBayesNet::shared_ptr gbn(GaussianEliminationTree::Create(blockGfgs[trial])->eliminate(&EliminateQR)); + VectorValues soln(optimize(*gbn)); } blocksolve = timer.elapsed(); cout << blocksolve << " s" << endl; @@ -99,9 +99,9 @@ int main(int argc, char *argv[]) { cout.flush(); boost::timer timer; timer.restart(); - vector combGfgs; + vector combGfgs; for(size_t trial=0; trialeliminate(&EliminateQROrdered)); - VectorValuesOrdered soln(optimize(*gbn)); + GaussianBayesNet::shared_ptr gbn(GaussianEliminationTree::Create(combGfgs[trial])->eliminate(&EliminateQR)); + VectorValues soln(optimize(*gbn)); } combsolve = timer.elapsed(); cout << combsolve << " s" << endl; diff --git a/gtsam/linear/tests/timeGaussianFactor.cpp b/gtsam/linear/tests/timeGaussianFactor.cpp index 833c01f71..3938abc8b 100644 --- a/gtsam/linear/tests/timeGaussianFactor.cpp +++ b/gtsam/linear/tests/timeGaussianFactor.cpp @@ -101,14 +101,14 @@ int main() b2(7) = -1; // time eliminate - JacobianFactorOrdered combined(_x2_, Ax2, _l1_, Al1, _x1_, Ax1, b2, noiseModel::Isotropic::Sigma(8,1)); + JacobianFactor combined(_x2_, Ax2, _l1_, Al1, _x1_, Ax1, b2, noiseModel::Isotropic::Sigma(8,1)); long timeLog = clock(); int n = 1000000; - GaussianConditionalOrdered::shared_ptr conditional; - JacobianFactorOrdered::shared_ptr factor; + GaussianConditional::shared_ptr conditional; + JacobianFactor::shared_ptr factor; for(int i = 0; i < n; i++) - conditional = JacobianFactorOrdered(combined).eliminateFirst(); + conditional = JacobianFactor(combined).eliminateFirst(); long timeLog2 = clock(); double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; diff --git a/gtsam/linear/tests/timeSLAMlike.cpp b/gtsam/linear/tests/timeSLAMlike.cpp index 96fa69b88..340e16a79 100644 --- a/gtsam/linear/tests/timeSLAMlike.cpp +++ b/gtsam/linear/tests/timeSLAMlike.cpp @@ -31,7 +31,7 @@ using namespace gtsam; using namespace std; using namespace boost::lambda; -typedef EliminationTreeOrdered GaussianEliminationTree; +typedef EliminationTree GaussianEliminationTree; static boost::variate_generator > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0)); @@ -65,10 +65,10 @@ int main(int argc, char *argv[]) { cout.flush(); boost::timer timer; timer.restart(); - vector blockGfgs; + vector blockGfgs; blockGfgs.reserve(nTrials); for(size_t trial=0; trial typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_( - const GaussianFactorGraphOrdered& linearFactorGraph, const OrderingOrdered& ordering, + const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering, const Values& linearizationPoints, Key lastKey, - JacobianFactorOrdered::shared_ptr& newPrior) const { + JacobianFactor::shared_ptr& newPrior) const { // Extract the Index of the provided last key gtsam::Index lastIndex = ordering.at(lastKey); @@ -39,23 +39,23 @@ namespace gtsam { // Solve the linear factor graph, converting it into a linear Bayes Network // P(x0,x1) = P(x0|x1)*P(x1) GaussianSequentialSolver solver(linearFactorGraph); - GaussianBayesNetOrdered::shared_ptr linearBayesNet = solver.eliminate(); + GaussianBayesNet::shared_ptr linearBayesNet = solver.eliminate(); // Extract the current estimate of x1,P1 from the Bayes Network - VectorValuesOrdered result = optimize(*linearBayesNet); + VectorValues result = optimize(*linearBayesNet); T x = linearizationPoints.at(lastKey).retract(result[lastIndex]); // Create a Jacobian Factor from the root node of the produced Bayes Net. // This will act as a prior for the next iteration. // The linearization point of this prior must be moved to the new estimate of x, // and the key/index needs to be reset to 0, the first key in the next iteration. - const GaussianConditionalOrdered::shared_ptr& cg = linearBayesNet->back(); + const GaussianConditional::shared_ptr& cg = linearBayesNet->back(); assert(cg->nrFrontals() == 1); assert(cg->nrParents() == 0); // TODO: Find a way to create the correct Jacobian Factor in a single pass - JacobianFactorOrdered tmpPrior = JacobianFactorOrdered(*cg); - newPrior = JacobianFactorOrdered::shared_ptr( - new JacobianFactorOrdered( + JacobianFactor tmpPrior = JacobianFactor(*cg); + newPrior = JacobianFactor::shared_ptr( + new JacobianFactor( 0, tmpPrior.getA(tmpPrior.begin()), tmpPrior.getb() @@ -76,8 +76,8 @@ namespace gtsam { // Create a Jacobian Prior Factor directly P_initial. // Since x0 is set to the provided mean, the b vector in the prior will be zero // TODO Frank asks: is there a reason why noiseModel is not simply P_initial ? - priorFactor_ = JacobianFactorOrdered::shared_ptr( - new JacobianFactorOrdered(0, P_initial->R(), Vector::Zero(x_initial.dim()), + priorFactor_ = JacobianFactor::shared_ptr( + new JacobianFactor(0, P_initial->R(), Vector::Zero(x_initial.dim()), noiseModel::Unit::Create(P_initial->dim()))); } @@ -95,7 +95,7 @@ namespace gtsam { Key x1 = motionFactor.key2(); // Create an elimination ordering - OrderingOrdered ordering; + Ordering ordering; ordering.insert(x0, 0); ordering.insert(x1, 1); @@ -105,7 +105,7 @@ namespace gtsam { linearizationPoints.insert(x1, x_); // TODO should this really be x_ ? // Create a Gaussian Factor Graph - GaussianFactorGraphOrdered linearFactorGraph; + GaussianFactorGraph linearFactorGraph; // Add in previous posterior as prior on the first state linearFactorGraph.push_back(priorFactor_); @@ -134,7 +134,7 @@ namespace gtsam { Key x0 = measurementFactor.key(); // Create an elimination ordering - OrderingOrdered ordering; + Ordering ordering; ordering.insert(x0, 0); // Create a set of linearization points @@ -142,7 +142,7 @@ namespace gtsam { linearizationPoints.insert(x0, x_); // Create a Gaussian Factor Graph - GaussianFactorGraphOrdered linearFactorGraph; + GaussianFactorGraph linearFactorGraph; // Add in the prior on the first state linearFactorGraph.push_back(priorFactor_); diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index a0ab1a327..a71a5332a 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -51,11 +51,11 @@ namespace gtsam { protected: T x_; // linearization point - JacobianFactorOrdered::shared_ptr priorFactor_; // density + JacobianFactor::shared_ptr priorFactor_; // density - T solve_(const GaussianFactorGraphOrdered& linearFactorGraph, - const OrderingOrdered& ordering, const Values& linearizationPoints, - Key x, JacobianFactorOrdered::shared_ptr& newPrior) const; + T solve_(const GaussianFactorGraph& linearFactorGraph, + const Ordering& ordering, const Values& linearizationPoints, + Key x, JacobianFactor::shared_ptr& newPrior) const; public: diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 5c860e037..8982ca9bd 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -31,9 +31,9 @@ namespace gtsam { /* ************************************************************************* */ void ISAM2::Impl::AddVariables( - const Values& newTheta, Values& theta, VectorValuesOrdered& delta, - VectorValuesOrdered& deltaNewton, VectorValuesOrdered& RgProd, vector& replacedKeys, - OrderingOrdered& ordering, const KeyFormatter& keyFormatter) { + const Values& newTheta, Values& theta, VectorValues& delta, + VectorValues& deltaNewton, VectorValues& RgProd, vector& replacedKeys, + Ordering& ordering, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); @@ -64,10 +64,10 @@ void ISAM2::Impl::AddVariables( /* ************************************************************************* */ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Clique::shared_ptr& root, - Values& theta, VariableIndexOrdered& variableIndex, - VectorValuesOrdered& delta, VectorValuesOrdered& deltaNewton, VectorValuesOrdered& RgProd, - std::vector& replacedKeys, OrderingOrdered& ordering, Base::Nodes& nodes, - GaussianFactorGraphOrdered& linearFactors, FastSet& fixedVariables) { + Values& theta, VariableIndex& variableIndex, + VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, + std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, + GaussianFactorGraph& linearFactors, FastSet& fixedVariables) { // Get indices of unused keys vector unusedIndices; unusedIndices.reserve(unusedKeys.size()); @@ -93,9 +93,9 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Cli // Copy from the old data structures to new ones, only iterating up to // the number of used variables, and applying the unusedToEnd permutation // in order to remove the unused variables. - VectorValuesOrdered newDelta(dims); - VectorValuesOrdered newDeltaNewton(dims); - VectorValuesOrdered newDeltaGradSearch(dims); + VectorValues newDelta(dims); + VectorValues newDeltaNewton(dims); + VectorValues newDeltaGradSearch(dims); std::vector newReplacedKeys(replacedKeys.size() - unusedIndices.size()); Base::Nodes newNodes(replacedKeys.size() - unusedIndices.size()); @@ -118,7 +118,7 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Cli // Reorder and remove from ordering, solution, and fixed keys ordering.permuteInPlace(unusedToEnd); BOOST_REVERSE_FOREACH(Key key, unusedKeys) { - OrderingOrdered::value_type removed = ordering.pop_back(); + Ordering::value_type removed = ordering.pop_back(); assert(removed.first == key); theta.erase(key); fixedVariables.erase(key); @@ -131,7 +131,7 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Cli } /* ************************************************************************* */ -FastSet ISAM2::Impl::IndicesFromFactors(const OrderingOrdered& ordering, const NonlinearFactorGraph& factors) { +FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors) { FastSet indices; BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) { BOOST_FOREACH(Key key, factor->keys()) { @@ -142,7 +142,7 @@ FastSet ISAM2::Impl::IndicesFromFactors(const OrderingOrdered& ordering, } /* ************************************************************************* */ -FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValuesOrdered& delta, const OrderingOrdered& ordering, +FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { FastSet relinKeys; @@ -156,7 +156,7 @@ FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValuesOrdered& } } else if(relinearizeThreshold.type() == typeid(FastMap)) { const FastMap& thresholds = boost::get >(relinearizeThreshold); - BOOST_FOREACH(const OrderingOrdered::value_type& key_index, ordering) { + BOOST_FOREACH(const Ordering::value_type& key_index, ordering) { const Vector& threshold = thresholds.find(Symbol(key_index.first).chr())->second; Index j = key_index.second; if(threshold.rows() != delta[j].rows()) @@ -170,7 +170,7 @@ FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValuesOrdered& } /* ************************************************************************* */ -void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double threshold, const VectorValuesOrdered& delta, const ISAM2Clique::shared_ptr& clique) { +void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double threshold, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) { // Check the current clique for relinearization bool relinearize = false; @@ -191,7 +191,7 @@ void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double thres } /* ************************************************************************* */ -void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, const VectorValuesOrdered& delta, const OrderingOrdered& ordering, const ISAM2Clique::shared_ptr& clique) { +void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, const VectorValues& delta, const Ordering& ordering, const ISAM2Clique::shared_ptr& clique) { // Check the current clique for relinearization bool relinearize = false; @@ -223,7 +223,7 @@ void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValuesOrdered& delta, const OrderingOrdered& ordering, +FastSet ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { FastSet relinKeys; @@ -260,8 +260,8 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, } /* ************************************************************************* */ -void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValuesOrdered& delta, const OrderingOrdered& ordering, - const vector& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) { +void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const Ordering& ordering, + const vector& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) { // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions // if we try to re-use them. @@ -272,7 +272,7 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValuesOrdered& delta, assert(values.size() == ordering.size()); assert(delta.size() == ordering.size()); Values::iterator key_value; - OrderingOrdered::const_iterator key_index; + Ordering::const_iterator key_index; for(key_value = values.begin(), key_index = ordering.begin(); key_value != values.end() && key_index != ordering.end(); ++key_value, ++key_index) { assert(key_value->key == key_index->first); @@ -297,7 +297,7 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValuesOrdered& delta, /* ************************************************************************* */ ISAM2::Impl::PartialSolveResult -ISAM2::Impl::PartialSolve(GaussianFactorGraphOrdered& factors, +ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode, bool useQR) { const bool debug = ISDEBUG("ISAM2 recalculate"); @@ -308,7 +308,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraphOrdered& factors, #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS // Debug check that all variables involved in the factors to be re-eliminated // are in affectedKeys, since we will use it to select a subset of variables. - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factors) { + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { BOOST_FOREACH(Index key, factor->keys()) { assert(find(keys.begin(), keys.end(), key) != keys.end()); } @@ -330,7 +330,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraphOrdered& factors, if(debug) factors.print("Factors to reorder/re-eliminate: "); gttoc(select_affected_variables); gttic(variable_index); - VariableIndexOrdered affectedFactorsIndex(factors); // Create a variable index for the factors to be re-eliminated + VariableIndex affectedFactorsIndex(factors); // Create a variable index for the factors to be re-eliminated if(debug) affectedFactorsIndex.print("affectedFactorsIndex: "); gttoc(variable_index); gttic(ccolamd); @@ -371,17 +371,17 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraphOrdered& factors, if(debug) factors.print("Colamd-ordered affected factors: "); #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - VariableIndexOrdered fromScratchIndex(factors); + VariableIndex fromScratchIndex(factors); assert(assert_equal(fromScratchIndex, affectedFactorsIndex)); #endif // eliminate into a Bayes net gttic(eliminate); - JunctionTreeOrdered jt(factors, affectedFactorsIndex); + JunctionTree jt(factors, affectedFactorsIndex); if(!useQR) - result.bayesTree = jt.eliminate(EliminatePreferCholeskyOrdered); + result.bayesTree = jt.eliminate(EliminatePreferCholesky); else - result.bayesTree = jt.eliminate(EliminateQROrdered); + result.bayesTree = jt.eliminate(EliminateQR); gttoc(eliminate); gttic(permute_eliminated); @@ -400,7 +400,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraphOrdered& factors, /* ************************************************************************* */ namespace internal { -inline static void optimizeInPlace(const boost::shared_ptr& clique, VectorValuesOrdered& result) { +inline static void optimizeInPlace(const boost::shared_ptr& clique, VectorValues& result) { // parents are assumed to already be solved and available in result clique->conditional()->solveInPlace(result); @@ -411,7 +411,7 @@ inline static void optimizeInPlace(const boost::shared_ptr& clique, } /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, VectorValuesOrdered& delta, double wildfireThreshold) { +size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, VectorValues& delta, double wildfireThreshold) { size_t lastBacksubVariableCount; @@ -439,7 +439,7 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std: /* ************************************************************************* */ namespace internal { void updateDoglegDeltas(const boost::shared_ptr& clique, const std::vector& replacedKeys, - const VectorValuesOrdered& grad, VectorValuesOrdered& deltaNewton, VectorValuesOrdered& RgProd, size_t& varsUpdated) { + const VectorValues& grad, VectorValues& deltaNewton, VectorValues& RgProd, size_t& varsUpdated) { // Check if any frontal or separator keys were recalculated, if so, we need // update deltas and recurse to children, but if not, we do not need to @@ -478,10 +478,10 @@ void updateDoglegDeltas(const boost::shared_ptr& clique, const std: /* ************************************************************************* */ size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector& replacedKeys, - VectorValuesOrdered& deltaNewton, VectorValuesOrdered& RgProd) { + VectorValues& deltaNewton, VectorValues& RgProd) { // Get gradient - VectorValuesOrdered grad = *allocateVectorValues(isam); + VectorValues grad = *allocateVectorValues(isam); gradientAtZero(isam, grad); // Update variables diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 38fdd9b4f..435a75a48 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -49,17 +49,17 @@ struct GTSAM_EXPORT ISAM2::Impl { * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables * @param keyFormatter Formatter for printing nonlinear keys during debugging */ - static void AddVariables(const Values& newTheta, Values& theta, VectorValuesOrdered& delta, - VectorValuesOrdered& deltaNewton, VectorValuesOrdered& RgProd, std::vector& replacedKeys, - OrderingOrdered& ordering, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta, + VectorValues& deltaNewton, VectorValues& RgProd, std::vector& replacedKeys, + Ordering& ordering, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** * Remove variables from the ISAM2 system. */ static void RemoveVariables(const FastSet& unusedKeys, const ISAM2Clique::shared_ptr& root, - Values& theta, VariableIndexOrdered& variableIndex, VectorValuesOrdered& delta, VectorValuesOrdered& deltaNewton, - VectorValuesOrdered& RgProd, std::vector& replacedKeys, OrderingOrdered& ordering, Base::Nodes& nodes, - GaussianFactorGraphOrdered& linearFactors, FastSet& fixedVariables); + Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, + VectorValues& RgProd, std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, + GaussianFactorGraph& linearFactors, FastSet& fixedVariables); /** * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol @@ -68,7 +68,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * @param factors The factors from which to extract the variables * @return The set of variables indices from the factors */ - static FastSet IndicesFromFactors(const OrderingOrdered& ordering, const NonlinearFactorGraph& factors); + static FastSet IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -79,7 +79,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationFull(const VectorValuesOrdered& delta, const OrderingOrdered& ordering, + static FastSet CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** @@ -93,7 +93,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValuesOrdered& delta, const OrderingOrdered& ordering, + static FastSet CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** @@ -126,9 +126,9 @@ struct GTSAM_EXPORT ISAM2::Impl { * recalculate its delta. * @param keyFormatter Formatter for printing nonlinear keys during debugging */ - static void ExpmapMasked(Values& values, const VectorValuesOrdered& delta, - const OrderingOrdered& ordering, const std::vector& mask, - boost::optional invalidateIfDebug = boost::none, + static void ExpmapMasked(Values& values, const VectorValues& delta, + const Ordering& ordering, const std::vector& mask, + boost::optional invalidateIfDebug = boost::none, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** @@ -145,13 +145,13 @@ struct GTSAM_EXPORT ISAM2::Impl { * \return The eliminated BayesTree and the permutation to be applied to the * rest of the ISAM2 data. */ - static PartialSolveResult PartialSolve(GaussianFactorGraphOrdered& factors, const FastSet& keys, + static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode, bool useQR); - static size_t UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, VectorValuesOrdered& delta, double wildfireThreshold); + static size_t UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, VectorValues& delta, double wildfireThreshold); static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector& replacedKeys, - VectorValuesOrdered& deltaNewton, VectorValuesOrdered& RgProd); + VectorValues& deltaNewton, VectorValues& RgProd); }; diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 388f4c9d7..4a9cf105a 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -37,7 +37,7 @@ VALUE ISAM2::calculateEstimate(Key key) const { namespace internal { template void optimizeWildfire(const boost::shared_ptr& clique, double threshold, - std::vector& changed, const std::vector& replaced, VectorValuesOrdered& delta, int& count) { + std::vector& changed, const std::vector& replaced, VectorValues& delta, int& count) { // if none of the variables in this clique (frontal and separator!) changed // significantly, then by the running intersection property, none of the // cliques in the children need to be processed @@ -67,7 +67,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, // Temporary copy of the original values, to check how much they change std::vector originalValues((*clique)->nrFrontals()); - GaussianConditionalOrdered::const_iterator it; + GaussianConditional::const_iterator it; for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { originalValues[it - (*clique)->beginFrontals()] = delta[*it]; } @@ -113,7 +113,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, template bool optimizeWildfireNode(const boost::shared_ptr& clique, double threshold, - std::vector& changed, const std::vector& replaced, VectorValuesOrdered& delta, int& count) { + std::vector& changed, const std::vector& replaced, VectorValues& delta, int& count) { // if none of the variables in this clique (frontal and separator!) changed // significantly, then by the running intersection property, none of the // cliques in the children need to be processed @@ -143,7 +143,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // Temporary copy of the original values, to check how much they change std::vector originalValues((*clique)->nrFrontals()); - GaussianConditionalOrdered::const_iterator it; + GaussianConditional::const_iterator it; for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { originalValues[it - (*clique)->beginFrontals()] = delta[*it]; } @@ -189,7 +189,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh /* ************************************************************************* */ template -int optimizeWildfire(const boost::shared_ptr& root, double threshold, const std::vector& keys, VectorValuesOrdered& delta) { +int optimizeWildfire(const boost::shared_ptr& root, double threshold, const std::vector& keys, VectorValues& delta) { std::vector changed(keys.size(), false); int count = 0; // starting from the root, call optimize on each conditional @@ -200,7 +200,7 @@ int optimizeWildfire(const boost::shared_ptr& root, double threshold, co /* ************************************************************************* */ template -int optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const std::vector& keys, VectorValuesOrdered& delta) { +int optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const std::vector& keys, VectorValues& delta) { std::vector changed(keys.size(), false); int count = 0; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 6ed9f8838..4be02f7ae 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -124,10 +124,10 @@ ISAM2& ISAM2::operator=(const ISAM2& rhs) { deltaReplacedMask_ = rhs.deltaReplacedMask_; nonlinearFactors_ = rhs.nonlinearFactors_; - linearFactors_ = GaussianFactorGraphOrdered(); + linearFactors_ = GaussianFactorGraph(); linearFactors_.reserve(rhs.linearFactors_.size()); - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& linearFactor, rhs.linearFactors_) { - linearFactors_.push_back(linearFactor ? linearFactor->clone() : GaussianFactorOrdered::shared_ptr()); } + BOOST_FOREACH(const GaussianFactor::shared_ptr& linearFactor, rhs.linearFactors_) { + linearFactors_.push_back(linearFactor ? linearFactor->clone() : GaussianFactor::shared_ptr()); } ordering_ = rhs.ordering_; params_ = rhs.params_; @@ -150,12 +150,12 @@ FastList ISAM2::getAffectedFactors(const FastList& keys) const { if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } } if(debug) cout << endl; - FactorGraphOrdered allAffected; + FactorGraph allAffected; FastList indices; BOOST_FOREACH(const Index key, keys) { // const list l = nonlinearFactors_.factors(key); // indices.insert(indices.begin(), l.begin(), l.end()); - const VariableIndexOrdered::Factors& factors(variableIndex_[key]); + const VariableIndex::Factors& factors(variableIndex_[key]); BOOST_FOREACH(size_t factor, factors) { if(debug) cout << "Variable " << key << " affects factor " << factor << endl; indices.push_back(factor); @@ -172,7 +172,8 @@ FastList ISAM2::getAffectedFactors(const FastList& keys) const { /* ************************************************************************* */ // retrieve all factors that ONLY contain the affected variables // (note that the remaining stuff is summarized in the cached factors) -FactorGraphOrdered::shared_ptr + +FactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const { gttic(getAffectedFactors); @@ -188,7 +189,7 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const Fas gttoc(affectedKeysSet); gttic(check_candidates_and_linearize); - FactorGraphOrdered::shared_ptr linearized = boost::make_shared >(); + FactorGraph::shared_ptr linearized = boost::make_shared >(); BOOST_FOREACH(size_t idx, candidates) { bool inside = true; bool useCachedLinear = params_.cacheLinearizedFactors; @@ -209,7 +210,7 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const Fas #endif linearized->push_back(linearFactors_[idx]); } else { - GaussianFactorOrdered::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_, ordering_); + GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_, ordering_); linearized->push_back(linearFactor); if(params_.cacheLinearizedFactors) { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS @@ -227,11 +228,12 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const Fas /* ************************************************************************* */ // find intermediate (linearized) factors from cache that are passed into the affected area -GaussianFactorGraphOrdered ISAM2::getCachedBoundaryFactors(Cliques& orphans) { + +GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { static const bool debug = false; - GaussianFactorGraphOrdered cachedBoundary; + GaussianFactorGraph cachedBoundary; BOOST_FOREACH(sharedClique orphan, orphans) { // find the last variable that was eliminated @@ -286,14 +288,14 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark // (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques. gttic(removetop); Cliques orphans; - BayesNetOrdered affectedBayesNet; + BayesNet affectedBayesNet; this->removeTop(markedKeys, affectedBayesNet, orphans); gttoc(removetop); if(debug) affectedBayesNet.print("Removed top: "); if(debug) orphans.print("Orphans: "); - // FactorGraph factors(affectedBayesNet); + // FactorGraph factors(affectedBayesNet); // bug was here: we cannot reuse the original factors, because then the cached factors get messed up // [all the necessary data is actually contained in the affectedBayesNet, including what was passed in from the boundaries, // so this would be correct; however, in the process we also generate new cached_ entries that will be wrong (ie. they don't @@ -316,7 +318,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark gttic(batch); gttic(add_keys); - BOOST_FOREACH(const OrderingOrdered::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); } + BOOST_FOREACH(const Ordering::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); } gttoc(add_keys); gttic(reorder); @@ -359,18 +361,18 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark gttoc(reorder); gttic(linearize); - GaussianFactorGraphOrdered linearized = *nonlinearFactors_.linearize(theta_, ordering_); + GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_, ordering_); if(params_.cacheLinearizedFactors) linearFactors_ = linearized; gttoc(linearize); gttic(eliminate); - JunctionTreeOrdered jt(linearized, variableIndex_); + JunctionTree jt(linearized, variableIndex_); sharedClique newRoot; if(params_.factorization == ISAM2Params::CHOLESKY) - newRoot = jt.eliminate(EliminatePreferCholeskyOrdered); + newRoot = jt.eliminate(EliminatePreferCholesky); else if(params_.factorization == ISAM2Params::QR) - newRoot = jt.eliminate(EliminateQROrdered); + newRoot = jt.eliminate(EliminateQR); else assert(false); if(debug) newRoot->print("Eliminated: "); gttoc(eliminate); @@ -405,7 +407,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end()); affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), observedKeys.end()); gttic(relinearizeAffected); - GaussianFactorGraphOrdered factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); + GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); if(debug) factors.print("Relinearized factors: "); gttoc(relinearizeAffected); @@ -433,7 +435,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark gttic(cached); // add the cached intermediate results from the boundary of the orphans ... - GaussianFactorGraphOrdered cachedBoundary = getCachedBoundaryFactors(orphans); + GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); if(debug) cachedBoundary.print("Boundary factors: "); factors.push_back(cachedBoundary); gttoc(cached); @@ -737,7 +739,7 @@ ISAM2Result ISAM2::update( // 7. Linearize new factors if(params_.cacheLinearizedFactors) { gttic(linearize); - FactorGraphOrdered::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); + FactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); linearFactors_.push_back(*linearFactors); assert(nonlinearFactors_.size() == linearFactors_.size()); gttoc(linearize); @@ -804,7 +806,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeys) // Keep track of marginal factors - map from clique to the marginal factors // that should be incorporated into it, passed up from it's children. - multimap marginalFactors; + multimap marginalFactors; // Remove each variable and its subtrees BOOST_REVERSE_FOREACH(Index j, indices) { @@ -821,7 +823,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeys) // Remove either the whole clique or part of it if(marginalizeEntireClique) { // Remove the whole clique and its subtree, and keep the marginal factor. - GaussianFactorOrdered::shared_ptr marginalFactor = clique->cachedFactor(); + GaussianFactor::shared_ptr marginalFactor = clique->cachedFactor(); // We do not need the marginal factors associated with this clique // because their information is already incorporated in the new // marginal factor. So, now associate this marginal factor with the @@ -845,7 +847,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeys) // subtrees already marginalized out. // Add child marginals and remove marginalized subtrees - GaussianFactorGraphOrdered graph; + GaussianFactorGraph graph; FastSet factorsInSubtreeRoot; Cliques subtreesToRemove; BOOST_FOREACH(const sharedClique& child, clique->children()) { @@ -897,30 +899,30 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeys) std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), indices.begin(), indices.end(), std::inserter(cliqueFrontalsToEliminate, cliqueFrontalsToEliminate.end())); vector cliqueFrontalsToEliminateV(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end()); - pair eliminationResult1 = + pair eliminationResult1 = graph.eliminate(cliqueFrontalsToEliminateV, - params_.factorization==ISAM2Params::QR ? EliminateQROrdered : EliminatePreferCholeskyOrdered); + params_.factorization==ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky); // Add the resulting marginal - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& marginal, eliminationResult1.second) { + BOOST_FOREACH(const GaussianFactor::shared_ptr& marginal, eliminationResult1.second) { if(marginal) marginalFactors.insert(make_pair(clique, marginal)); } // Recover the conditional on the remaining subset of frontal variables // of this clique being martially marginalized. size_t nToEliminate = std::find(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals(), j) - clique->conditional()->begin() + 1; - GaussianFactorGraphOrdered graph2; + GaussianFactorGraph graph2; graph2.push_back(clique->conditional()->toFactor()); - GaussianFactorGraphOrdered::EliminationResult eliminationResult2 = + GaussianFactorGraph::EliminationResult eliminationResult2 = params_.factorization == ISAM2Params::QR ? - EliminateQROrdered(graph2, nToEliminate) : - EliminatePreferCholeskyOrdered(graph2, nToEliminate); - GaussianFactorGraphOrdered graph3; + EliminateQR(graph2, nToEliminate) : + EliminatePreferCholesky(graph2, nToEliminate); + GaussianFactorGraph graph3; graph3.push_back(eliminationResult2.second); - GaussianFactorGraphOrdered::EliminationResult eliminationResult3 = + GaussianFactorGraph::EliminationResult eliminationResult3 = params_.factorization == ISAM2Params::QR ? - EliminateQROrdered(graph3, clique->conditional()->nrFrontals() - nToEliminate) : - EliminatePreferCholeskyOrdered(graph3, clique->conditional()->nrFrontals() - nToEliminate); + EliminateQR(graph3, clique->conditional()->nrFrontals() - nToEliminate) : + EliminatePreferCholesky(graph3, clique->conditional()->nrFrontals() - nToEliminate); sharedClique newClique = boost::make_shared(make_pair(eliminationResult3.first, clique->cachedFactor())); // Add the marginalized clique to the BayesTree @@ -936,8 +938,8 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeys) // At this point we have updated the BayesTree, now update the remaining iSAM2 data structures // Gather factors to add - the new marginal factors - GaussianFactorGraphOrdered factorsToAdd; - typedef pair Clique_Factor; + GaussianFactorGraph factorsToAdd; + typedef pair Clique_Factor; BOOST_FOREACH(const Clique_Factor& clique_factor, marginalFactors) { if(clique_factor.second) factorsToAdd.push_back(clique_factor.second); @@ -955,7 +957,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeys) BOOST_FOREACH(Index j, indices) { factorIndicesToRemove.insert(variableIndex_[j].begin(), variableIndex_[j].end()); } vector removedFactorIndices; - SymbolicFactorGraphOrdered removedFactors; + SymbolicFactorGraph removedFactors; BOOST_FOREACH(size_t i, factorIndicesToRemove) { removedFactorIndices.push_back(i); removedFactors.push_back(nonlinearFactors_[i]->symbolic(ordering_)); @@ -1011,7 +1013,7 @@ Values ISAM2::calculateEstimate() const { Values ret(theta_); gttoc(Copy_Values); gttic(getDelta); - const VectorValuesOrdered& delta(getDelta()); + const VectorValues& delta(getDelta()); gttoc(getDelta); gttic(Expmap); vector mask(ordering_.size(), true); @@ -1023,35 +1025,36 @@ Values ISAM2::calculateEstimate() const { /* ************************************************************************* */ Matrix ISAM2::marginalCovariance(Index key) const { return marginalFactor(ordering_[key], - params_.factorization == ISAM2Params::QR ? EliminateQROrdered : EliminatePreferCholeskyOrdered) + params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky) ->information().inverse(); } /* ************************************************************************* */ Values ISAM2::calculateBestEstimate() const { - VectorValuesOrdered delta(theta_.dims(ordering_)); + VectorValues delta(theta_.dims(ordering_)); internal::optimizeInPlace(this->root(), delta); return theta_.retract(delta, ordering_); } /* ************************************************************************* */ -const VectorValuesOrdered& ISAM2::getDelta() const { +const VectorValues& ISAM2::getDelta() const { if(!deltaUptodate_) updateDelta(); return delta_; } /* ************************************************************************* */ -VectorValuesOrdered optimize(const ISAM2& isam) { + +VectorValues optimize(const ISAM2& isam) { gttic(allocateVectorValues); - VectorValuesOrdered delta = *allocateVectorValues(isam); + VectorValues delta = *allocateVectorValues(isam); gttoc(allocateVectorValues); optimizeInPlace(isam, delta); return delta; } /* ************************************************************************* */ -void optimizeInPlace(const ISAM2& isam, VectorValuesOrdered& delta) { +void optimizeInPlace(const ISAM2& isam, VectorValues& delta) { // We may need to update the solution calculations if(!isam.deltaDoglegUptodate_) { gttic(UpdateDoglegDeltas); @@ -1073,9 +1076,10 @@ void optimizeInPlace(const ISAM2& isam, VectorValuesOrdered& delta) { } /* ************************************************************************* */ -VectorValuesOrdered optimizeGradientSearch(const ISAM2& isam) { + +VectorValues optimizeGradientSearch(const ISAM2& isam) { gttic(Allocate_VectorValues); - VectorValuesOrdered grad = *allocateVectorValues(isam); + VectorValues grad = *allocateVectorValues(isam); gttoc(Allocate_VectorValues); optimizeGradientSearchInPlace(isam, grad); @@ -1084,7 +1088,7 @@ VectorValuesOrdered optimizeGradientSearch(const ISAM2& isam) { } /* ************************************************************************* */ -void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValuesOrdered& grad) { +void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) { // We may need to update the solution calcaulations if(!isam.deltaDoglegUptodate_) { gttic(UpdateDoglegDeltas); @@ -1119,15 +1123,16 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValuesOrdered& grad) } /* ************************************************************************* */ -VectorValuesOrdered gradient(const ISAM2& bayesTree, const VectorValuesOrdered& x0) { - return gradient(FactorGraphOrdered(bayesTree), x0); + +VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0) { + return gradient(FactorGraph(bayesTree), x0); } /* ************************************************************************* */ -static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, VectorValuesOrdered& g) { +static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, VectorValues& g) { // Loop through variables in each clique, adding contributions int variablePosition = 0; - for(GaussianConditionalOrdered::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) { + for(GaussianConditional::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) { const int dim = root->conditional()->dim(jit); g[*jit] += root->gradientContribution().segment(variablePosition, dim); variablePosition += dim; @@ -1141,7 +1146,7 @@ static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, } /* ************************************************************************* */ -void gradientAtZero(const ISAM2& bayesTree, VectorValuesOrdered& g) { +void gradientAtZero(const ISAM2& bayesTree, VectorValues& g) { // Zero-out gradient g.setZero(); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 59ab2517f..bc17956c3 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -344,13 +344,13 @@ struct GTSAM_EXPORT ISAM2Result { * Specialized Clique structure for ISAM2, incorporating caching and gradient contribution * TODO: more documentation */ -class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBaseOrdered { +class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBase { public: typedef ISAM2Clique This; - typedef BayesTreeCliqueBaseOrdered Base; + typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; - typedef GaussianConditionalOrdered ConditionalType; + typedef GaussianConditional ConditionalType; typedef ConditionalType::shared_ptr sharedConditional; Base::FactorType::shared_ptr cachedFactor_; @@ -438,7 +438,7 @@ private: * estimate of all variables. * */ -class ISAM2: public BayesTreeOrdered { +class ISAM2: public BayesTree { protected: @@ -446,7 +446,7 @@ protected: Values theta_; /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */ - VariableIndexOrdered variableIndex_; + VariableIndex variableIndex_; /** The linear delta from the last linear solution, an update to the estimate in theta * @@ -454,10 +454,10 @@ protected: * until either requested with getDelta() or calculateEstimate(), or needed * during update() to evaluate whether to relinearize variables. */ - mutable VectorValuesOrdered delta_; + mutable VectorValues delta_; - mutable VectorValuesOrdered deltaNewton_; - mutable VectorValuesOrdered RgProd_; + mutable VectorValues deltaNewton_; + mutable VectorValues RgProd_; mutable bool deltaDoglegUptodate_; /** Indicates whether the current delta is up-to-date, only used @@ -487,11 +487,11 @@ protected: NonlinearFactorGraph nonlinearFactors_; /** The current linear factors, which are only updated as needed */ - mutable GaussianFactorGraphOrdered linearFactors_; + mutable GaussianFactorGraph linearFactors_; /** The current elimination ordering Symbols to Index (integer) keys. * We keep it up to date as we add and reorder variables. */ - OrderingOrdered ordering_; + Ordering ordering_; /** The current parameters */ ISAM2Params params_; @@ -506,7 +506,7 @@ protected: public: typedef ISAM2 This; ///< This class - typedef BayesTreeOrdered Base; ///< The BayesTree base class + typedef BayesTree Base; ///< The BayesTree base class /** Create an empty ISAM2 instance */ GTSAM_EXPORT ISAM2(const ISAM2Params& params); @@ -601,16 +601,16 @@ public: GTSAM_EXPORT Values calculateBestEstimate() const; /** Access the current delta, computed during the last call to update */ - GTSAM_EXPORT const VectorValuesOrdered& getDelta() const; + GTSAM_EXPORT const VectorValues& getDelta() const; /** Access the set of nonlinear factors */ GTSAM_EXPORT const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } /** Access the current ordering */ - GTSAM_EXPORT const OrderingOrdered& getOrdering() const { return ordering_; } + GTSAM_EXPORT const Ordering& getOrdering() const { return ordering_; } /** Access the nonlinear variable index */ - GTSAM_EXPORT const VariableIndexOrdered& getVariableIndex() const { return variableIndex_; } + GTSAM_EXPORT const VariableIndex& getVariableIndex() const { return variableIndex_; } size_t lastAffectedVariableCount; size_t lastAffectedFactorCount; @@ -629,24 +629,24 @@ public: private: GTSAM_EXPORT FastList getAffectedFactors(const FastList& keys) const; - GTSAM_EXPORT FactorGraphOrdered::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const; - GTSAM_EXPORT GaussianFactorGraphOrdered getCachedBoundaryFactors(Cliques& orphans); + GTSAM_EXPORT FactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const; + GTSAM_EXPORT GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); GTSAM_EXPORT boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, const FastVector& observedKeys, const FastSet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); // void linear_update(const GaussianFactorGraph& newFactors); GTSAM_EXPORT void updateDelta(bool forceFullSolve = false) const; - GTSAM_EXPORT friend void optimizeInPlace(const ISAM2&, VectorValuesOrdered&); - GTSAM_EXPORT friend void optimizeGradientSearchInPlace(const ISAM2&, VectorValuesOrdered&); + GTSAM_EXPORT friend void optimizeInPlace(const ISAM2&, VectorValues&); + GTSAM_EXPORT friend void optimizeGradientSearchInPlace(const ISAM2&, VectorValues&); }; // ISAM2 /** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ -GTSAM_EXPORT VectorValuesOrdered optimize(const ISAM2& isam); +GTSAM_EXPORT VectorValues optimize(const ISAM2& isam); /** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ -GTSAM_EXPORT void optimizeInPlace(const ISAM2& isam, VectorValuesOrdered& delta); +GTSAM_EXPORT void optimizeInPlace(const ISAM2& isam, VectorValues& delta); /// Optimize the BayesTree, starting from the root. /// @param replaced Needs to contain @@ -661,11 +661,11 @@ GTSAM_EXPORT void optimizeInPlace(const ISAM2& isam, VectorValuesOrdered& delta) /// @return The number of variables that were solved for template int optimizeWildfire(const boost::shared_ptr& root, - double threshold, const std::vector& replaced, VectorValuesOrdered& delta); + double threshold, const std::vector& replaced, VectorValues& delta); template int optimizeWildfireNonRecursive(const boost::shared_ptr& root, - double threshold, const std::vector& replaced, VectorValuesOrdered& delta); + double threshold, const std::vector& replaced, VectorValues& delta); /** * Optimize along the gradient direction, with a closed-form computation to @@ -692,10 +692,10 @@ int optimizeWildfireNonRecursive(const boost::shared_ptr& root, * * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] */ -GTSAM_EXPORT VectorValuesOrdered optimizeGradientSearch(const ISAM2& isam); +GTSAM_EXPORT VectorValues optimizeGradientSearch(const ISAM2& isam); /** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ -GTSAM_EXPORT void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValuesOrdered& grad); +GTSAM_EXPORT void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad); /// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) template @@ -712,7 +712,7 @@ int calculate_nnz(const boost::shared_ptr& clique); * @param x0 The center about which to compute the gradient * @return The gradient as a VectorValues */ -GTSAM_EXPORT VectorValuesOrdered gradient(const ISAM2& bayesTree, const VectorValuesOrdered& x0); +GTSAM_EXPORT VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0); /** * Compute the gradient of the energy function, @@ -725,7 +725,7 @@ GTSAM_EXPORT VectorValuesOrdered gradient(const ISAM2& bayesTree, const VectorVa * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues * @return The gradient as a VectorValues */ -GTSAM_EXPORT void gradientAtZero(const ISAM2& bayesTree, VectorValuesOrdered& g); +GTSAM_EXPORT void gradientAtZero(const ISAM2& bayesTree, VectorValues& g); } /// namespace gtsam diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index d4a70f324..a4cc90bc4 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -68,11 +68,11 @@ Matrix Marginals::marginalInformation(Key variable) const { Index index = ordering_[variable]; // Compute marginal - GaussianFactorOrdered::shared_ptr marginalFactor; + GaussianFactor::shared_ptr marginalFactor; if(factorization_ == CHOLESKY) - marginalFactor = bayesTree_.marginalFactor(index, EliminatePreferCholeskyOrdered); + marginalFactor = bayesTree_.marginalFactor(index, EliminatePreferCholesky); else if(factorization_ == QR) - marginalFactor = bayesTree_.marginalFactor(index, EliminateQROrdered); + marginalFactor = bayesTree_.marginalFactor(index, EliminateQR); // Get information matrix (only store upper-right triangle) gttic(AsMatrix); @@ -108,7 +108,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab Matrix info = marginalInformation(variables.front()); std::vector dims; dims.push_back(info.rows()); - OrderingOrdered indices; + Ordering indices; indices.insert(variables.front(), 0); return JointMarginal(info, dims, indices); @@ -118,12 +118,12 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab for(size_t i=0; i& variab // Build map from variable keys to position in factor graph variables, // which are sorted in index order. - OrderingOrdered variableConversion; + Ordering variableConversion; { // First build map from index to key FastMap usedIndices; @@ -166,7 +166,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) const { cout << s << "Joint marginal on keys "; bool first = true; - BOOST_FOREACH(const OrderingOrdered::value_type& key_index, indices_) { + BOOST_FOREACH(const Ordering::value_type& key_index, indices_) { if(!first) cout << ", "; else diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 1fea64b3a..a82536283 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -44,11 +44,11 @@ public: protected: - GaussianFactorGraphOrdered graph_; - OrderingOrdered ordering_; + GaussianFactorGraph graph_; + Ordering ordering_; Values values_; Factorization factorization_; - GaussianBayesTreeOrdered bayesTree_; + GaussianBayesTree bayesTree_; public: @@ -88,7 +88,7 @@ protected: Matrix fullMatrix_; BlockView blockView_; - OrderingOrdered indices_; + Ordering indices_; public: /** A block view of the joint marginal - this stores a reference to the @@ -135,7 +135,7 @@ public: void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; protected: - JointMarginal(const Matrix& fullMatrix, const std::vector& dims, const OrderingOrdered& indices) : + JointMarginal(const Matrix& fullMatrix, const std::vector& dims, const Ordering& indices) : fullMatrix_(fullMatrix), blockView_(fullMatrix_, dims.begin(), dims.end()), indices_(indices) {} friend class Marginals; diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 3bd7df529..faee280b1 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -153,12 +153,12 @@ namespace gtsam { } // Linearize is over-written, because base linearization tries to whiten - virtual GaussianFactorOrdered::shared_ptr linearize(const Values& x, const OrderingOrdered& ordering) const { + virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const { const T& xj = x.at(this->key()); Matrix A; Vector b = evaluateError(xj, A); SharedDiagonal model = noiseModel::Constrained::All(b.size()); - return GaussianFactorOrdered::shared_ptr(new JacobianFactorOrdered(ordering[this->key()], A, b, model)); + return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key()], A, b, model)); } /// @return a deep copy of this factor diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 74cb75649..c73d9aa9f 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -130,11 +130,11 @@ public: * Create a symbolic factor using the given ordering to determine the * variable indices. */ - //virtual IndexFactorOrdered::shared_ptr symbolic(const OrderingOrdered& ordering) const { + //virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { // std::vector indices(this->size()); // for(size_t j=0; jsize(); ++j) // indices[j] = ordering[this->keys()[j]]; - // return IndexFactorOrdered::shared_ptr(new IndexFactorOrdered(indices)); + // return IndexFactor::shared_ptr(new IndexFactor(indices)); //} /** diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 0cc330e5e..1732da885 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -221,31 +221,31 @@ Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap } /* ************************************************************************* */ -//SymbolicFactorGraphOrdered::shared_ptr NonlinearFactorGraph::symbolic(const OrderingOrdered& ordering) const { +//SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const { // gttic(NonlinearFactorGraph_symbolic_from_Ordering); // // // Generate the symbolic factor graph -// SymbolicFactorGraphOrdered::shared_ptr symbolicfg(new SymbolicFactorGraphOrdered); +// SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph); // symbolicfg->reserve(this->size()); // // BOOST_FOREACH(const sharedFactor& factor, this->factors_) { // if(factor) // symbolicfg->push_back(factor->symbolic(ordering)); // else -// symbolicfg->push_back(SymbolicFactorGraphOrdered::sharedFactor()); +// symbolicfg->push_back(SymbolicFactorGraph::sharedFactor()); // } // // return symbolicfg; //} /* ************************************************************************* */ -//pair NonlinearFactorGraph::symbolic( +//pair NonlinearFactorGraph::symbolic( // const Values& config) const //{ // gttic(NonlinearFactorGraph_symbolic_from_Values); // // // Generate an initial key ordering in iterator order -// OrderingOrdered::shared_ptr ordering(config.orderingArbitrary()); +// Ordering::shared_ptr ordering(config.orderingArbitrary()); // return make_pair(symbolic(*ordering), ordering); //} diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp index 32124ef2a..62d4b76a3 100644 --- a/gtsam/nonlinear/NonlinearISAM.cpp +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -60,7 +60,7 @@ void NonlinearISAM::update(const NonlinearFactorGraph& newFactors, BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialValues) ordering_.insert(key_value.key, ordering_.size()); - boost::shared_ptr linearizedNewFactors = newFactors.linearize(linPoint_, ordering_); + boost::shared_ptr linearizedNewFactors = newFactors.linearize(linPoint_, ordering_); // Update ISAM isam_.update(*linearizedNewFactors); @@ -84,7 +84,7 @@ void NonlinearISAM::reorder_relinearize() { // Create a linear factor graph at the new linearization point // TODO: decouple relinearization and reordering to avoid - boost::shared_ptr gfg = factors_.linearize(newLinPoint, ordering_); + boost::shared_ptr gfg = factors_.linearize(newLinPoint, ordering_); // Just recreate the whole BayesTree isam_.update(*gfg); diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 54042727f..2246d0a7a 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -30,13 +30,13 @@ class GTSAM_EXPORT NonlinearISAM { protected: /** The internal iSAM object */ - gtsam::GaussianISAMOrdered isam_; + gtsam::GaussianISAM isam_; /** The current linearization point */ Values linPoint_; /** The ordering */ - gtsam::OrderingOrdered ordering_; + gtsam::Ordering ordering_; /** The original factors, used when relinearizing */ NonlinearFactorGraph factors_; @@ -72,13 +72,13 @@ public: // access /** access the underlying bayes tree */ - const GaussianISAMOrdered& bayesTree() const { return isam_; } + const GaussianISAM& bayesTree() const { return isam_; } /** Return the current linearization point */ const Values& getLinearizationPoint() const { return linPoint_; } /** Get the ordering */ - const gtsam::OrderingOrdered& getOrdering() const { return ordering_; } + const gtsam::Ordering& getOrdering() const { return ordering_; } /** get underlying nonlinear graph */ const NonlinearFactorGraph& getFactorsUnsafe() const { return factors_; } @@ -110,7 +110,7 @@ public: void addKey(Key key) { ordering_.push_back(key); } /** replace the current ordering */ - void setOrdering(const OrderingOrdered& new_ordering) { ordering_ = new_ordering; } + void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; } /// @} diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index 4e473ae3d..910444cf8 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -71,7 +71,7 @@ namespace gtsam { * @f] * So f = 2 f(x), g = -df(x), G = ddf(x) */ - static HessianFactorOrdered::shared_ptr linearize(double z, double u, double p, + static HessianFactor::shared_ptr linearize(double z, double u, double p, Index j1, Index j2) { double e = u - z, e2 = e * e; double c = 2 * logSqrt2PI - log(p) + e2 * p; @@ -80,8 +80,8 @@ namespace gtsam { Matrix G11 = Matrix_(1, 1, p); Matrix G12 = Matrix_(1, 1, e); Matrix G22 = Matrix_(1, 1, 0.5 / (p * p)); - return HessianFactorOrdered::shared_ptr( - new HessianFactorOrdered(j1, j2, G11, G12, g1, G22, g2, c)); + return HessianFactor::shared_ptr( + new HessianFactor(j1, j2, G11, G12, g1, G22, g2, c)); } /// @name Standard Constructors @@ -144,9 +144,9 @@ namespace gtsam { * Create a symbolic factor using the given ordering to determine the * variable indices. */ - virtual IndexFactorOrdered::shared_ptr symbolic(const OrderingOrdered& ordering) const { + virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { const Index j1 = ordering[meanKey_], j2 = ordering[precisionKey_]; - return IndexFactorOrdered::shared_ptr(new IndexFactorOrdered(j1, j2)); + return IndexFactor::shared_ptr(new IndexFactor(j1, j2)); } /// @} @@ -154,8 +154,8 @@ namespace gtsam { /// @{ /// linearize returns a Hessianfactor that is an approximation of error(p) - virtual boost::shared_ptr linearize(const Values& x, - const OrderingOrdered& ordering) const { + virtual boost::shared_ptr linearize(const Values& x, + const Ordering& ordering) const { double u = x.at(meanKey_); double p = x.at(precisionKey_); Index j1 = ordering[meanKey_]; diff --git a/gtsam/nonlinear/summarization.cpp b/gtsam/nonlinear/summarization.cpp index 9a2dad1b1..3da3951de 100644 --- a/gtsam/nonlinear/summarization.cpp +++ b/gtsam/nonlinear/summarization.cpp @@ -16,15 +16,15 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -std::pair +std::pair summarize(const NonlinearFactorGraph& graph, const Values& values, const KeySet& saved_keys, SummarizationMode mode) { const size_t nrEliminatedKeys = values.size() - saved_keys.size(); // If we aren't eliminating anything, linearize and return if (!nrEliminatedKeys || saved_keys.empty()) { - OrderingOrdered ordering = *values.orderingArbitrary(); - GaussianFactorGraphOrdered linear_graph = *graph.linearize(values, ordering); + Ordering ordering = *values.orderingArbitrary(); + GaussianFactorGraph linear_graph = *graph.linearize(values, ordering); return make_pair(linear_graph, ordering); } @@ -35,11 +35,11 @@ summarize(const NonlinearFactorGraph& graph, const Values& values, BOOST_FOREACH(const gtsam::Key& key, saved_keys) ordering_constraints.insert(make_pair(key, 1)); - OrderingOrdered ordering = *graph.orderingCOLAMDConstrained(values, ordering_constraints); + Ordering ordering = *graph.orderingCOLAMDConstrained(values, ordering_constraints); // Linearize the system - GaussianFactorGraphOrdered full_graph = *graph.linearize(values, ordering); - GaussianFactorGraphOrdered summarized_system; + GaussianFactorGraph full_graph = *graph.linearize(values, ordering); + GaussianFactorGraph summarized_system; std::vector indices; BOOST_FOREACH(const Key& k, saved_keys) @@ -52,11 +52,11 @@ summarize(const NonlinearFactorGraph& graph, const Values& values, switch (mode) { case PARTIAL_QR: { - summarized_system.push_back(EliminateQROrdered(full_graph, nrEliminatedKeys).second); + summarized_system.push_back(EliminateQR(full_graph, nrEliminatedKeys).second); break; } case PARTIAL_CHOLESKY: { - summarized_system.push_back(EliminateCholeskyOrdered(full_graph, nrEliminatedKeys).second); + summarized_system.push_back(EliminateCholesky(full_graph, nrEliminatedKeys).second); break; } case SEQUENTIAL_QR: { @@ -77,8 +77,8 @@ summarize(const NonlinearFactorGraph& graph, const Values& values, NonlinearFactorGraph summarizeAsNonlinearContainer( const NonlinearFactorGraph& graph, const Values& values, const KeySet& saved_keys, SummarizationMode mode) { - GaussianFactorGraphOrdered summarized_graph; - OrderingOrdered ordering; + GaussianFactorGraph summarized_graph; + Ordering ordering; boost::tie(summarized_graph, ordering) = summarize(graph, values, saved_keys, mode); return LinearContainerFactor::convertLinearGraph(summarized_graph, ordering); } diff --git a/gtsam/nonlinear/summarization.h b/gtsam/nonlinear/summarization.h index 599a90bd1..75f09074d 100644 --- a/gtsam/nonlinear/summarization.h +++ b/gtsam/nonlinear/summarization.h @@ -40,7 +40,7 @@ typedef enum { * @param mode controls what elimination technique and requirements to use * @return a pair of the remaining graph and the ordering used for linearization */ -std::pair GTSAM_EXPORT +std::pair GTSAM_EXPORT summarize(const NonlinearFactorGraph& graph, const Values& values, const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR); diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index eba330ec8..fe1b93bde 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -94,11 +94,11 @@ namespace gtsam { * returns a Jacobian instead of a full Hessian), but with the opposite sign. This * effectively cancels the effect of the original factor on the factor graph. */ - boost::shared_ptr - linearize(const Values& c, const OrderingOrdered& ordering) const { + boost::shared_ptr + linearize(const Values& c, const Ordering& ordering) const { // Generate the linearized factor from the contained nonlinear factor - GaussianFactorOrdered::shared_ptr gaussianFactor = factor_->linearize(c, ordering); + GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c, ordering); // return the negated version of the factor return gaussianFactor->negate(); diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index fb3b65059..d918dbd99 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -46,7 +46,7 @@ TEST( AntiFactor, NegativeHessian) values->insert(2, pose2); // Define an elimination ordering - OrderingOrdered::shared_ptr ordering(new OrderingOrdered()); + Ordering::shared_ptr ordering(new Ordering()); ordering->insert(1, 0); ordering->insert(2, 1); @@ -55,17 +55,17 @@ TEST( AntiFactor, NegativeHessian) BetweenFactor::shared_ptr originalFactor(new BetweenFactor(1, 2, z, sigma)); // Linearize it into a Jacobian Factor - GaussianFactorOrdered::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering); + GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering); // Convert it to a Hessian Factor - HessianFactorOrdered::shared_ptr originalHessian = HessianFactorOrdered::shared_ptr(new HessianFactorOrdered(*originalJacobian)); + HessianFactor::shared_ptr originalHessian = HessianFactor::shared_ptr(new HessianFactor(*originalJacobian)); // Create the AntiFactor version of the original nonlinear factor AntiFactor::shared_ptr antiFactor(new AntiFactor(originalFactor)); // Linearize the AntiFactor into a Hessian Factor - GaussianFactorOrdered::shared_ptr antiGaussian = antiFactor->linearize(*values, *ordering); - HessianFactorOrdered::shared_ptr antiHessian = boost::dynamic_pointer_cast(antiGaussian); + GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(*values, *ordering); + HessianFactor::shared_ptr antiHessian = boost::dynamic_pointer_cast(antiGaussian); // Compare Hessian blocks @@ -107,14 +107,14 @@ TEST( AntiFactor, EquivalentBayesNet) values->insert(2, pose2); // Define an elimination ordering - OrderingOrdered::shared_ptr ordering = graph->orderingCOLAMD(*values); + Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); // Eliminate into a BayesNet GaussianSequentialSolver solver1(*graph->linearize(*values, *ordering)); - GaussianBayesNetOrdered::shared_ptr expectedBayesNet = solver1.eliminate(); + GaussianBayesNet::shared_ptr expectedBayesNet = solver1.eliminate(); // Back-substitute to find the optimal deltas - VectorValuesOrdered expectedDeltas = optimize(*expectedBayesNet); + VectorValues expectedDeltas = optimize(*expectedBayesNet); // Add an additional factor between Pose1 and Pose2 BetweenFactor::shared_ptr f1(new BetweenFactor(1, 2, z, sigma)); @@ -126,10 +126,10 @@ TEST( AntiFactor, EquivalentBayesNet) // Again, Eliminate into a BayesNet GaussianSequentialSolver solver2(*graph->linearize(*values, *ordering)); - GaussianBayesNetOrdered::shared_ptr actualBayesNet = solver2.eliminate(); + GaussianBayesNet::shared_ptr actualBayesNet = solver2.eliminate(); // Back-substitute to find the optimal deltas - VectorValuesOrdered actualDeltas = optimize(*actualBayesNet); + VectorValues actualDeltas = optimize(*actualBayesNet); // Verify the BayesNets are identical CHECK(assert_equal(*expectedBayesNet, *actualBayesNet, 1e-5)); diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 0fc5dacfd..0aeadfa6a 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -149,8 +149,8 @@ static vector genCameraVariableCalibration() { return X ; } -static boost::shared_ptr getOrdering(const vector& cameras, const vector& landmarks) { - boost::shared_ptr ordering(new OrderingOrdered); +static boost::shared_ptr getOrdering(const vector& cameras, const vector& landmarks) { + boost::shared_ptr ordering(new Ordering); for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ; for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ; return ordering ; @@ -190,7 +190,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { graph.addCameraConstraint(0, cameras[0]); // Create an ordering of the variables - OrderingOrdered ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); @@ -233,7 +233,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { graph.addCameraConstraint(0, cameras[0]); const double reproj_error = 1e-5; - OrderingOrdered ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -274,7 +274,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { const double reproj_error = 1e-5 ; - OrderingOrdered ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -331,7 +331,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { const double reproj_error = 1e-5 ; - OrderingOrdered ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -375,7 +375,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { const double reproj_error = 1e-5 ; - OrderingOrdered ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index a79d45a11..6d8f36634 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -152,8 +152,8 @@ static vector genCameraVariableCalibration() { return cameras ; } -static boost::shared_ptr getOrdering(const std::vector& cameras, const std::vector& landmarks) { - boost::shared_ptr ordering(new OrderingOrdered); +static boost::shared_ptr getOrdering(const std::vector& cameras, const std::vector& landmarks) { + boost::shared_ptr ordering(new Ordering); for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ; for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ; return ordering ; @@ -192,7 +192,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_defaultK ) { graph.addCameraConstraint(0, cameras[0]); // Create an ordering of the variables - OrderingOrdered ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); @@ -235,7 +235,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_SingleMeasurementError ) { graph.addCameraConstraint(0, cameras[0]); const double reproj_error = 1e-5; - OrderingOrdered ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -276,7 +276,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixCameras ) { const double reproj_error = 1e-5 ; - OrderingOrdered ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -329,7 +329,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) { const double reproj_error = 1e-5 ; - OrderingOrdered ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -373,7 +373,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) { const double reproj_error = 1e-5 ; - OrderingOrdered ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras,landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp index e71076f77..537733286 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -38,7 +38,7 @@ static SymbolicConditional::shared_ptr L(new SymbolicConditional(_L_, _B_)); /* ************************************************************************* */ -TEST( SymbolicBayesNetOrdered, equals ) +TEST( SymbolicBayesNet, equals ) { SymbolicBayesNet f1; f1.push_back(B); @@ -51,7 +51,7 @@ TEST( SymbolicBayesNetOrdered, equals ) } /* ************************************************************************* */ -TEST( SymbolicBayesNetOrdered, combine ) +TEST( SymbolicBayesNet, combine ) { SymbolicConditional::shared_ptr A(new SymbolicConditional(_A_,_B_,_C_)), @@ -79,7 +79,7 @@ TEST( SymbolicBayesNetOrdered, combine ) } /* ************************************************************************* */ -TEST(SymbolicBayesNetOrdered, saveGraph) { +TEST(SymbolicBayesNet, saveGraph) { SymbolicBayesNet bn; bn += SymbolicConditional(_A_, _B_); std::vector keys; diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 3d3116e27..793d2d5dc 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -67,7 +67,7 @@ namespace { } /* ************************************************************************* */ -TEST(SymbolicBayesTreeOrdered, clear) +TEST(SymbolicBayesTree, clear) { SymbolicBayesTree bayesTree = asiaBayesTree; bayesTree.clear(); @@ -85,7 +85,7 @@ Bayes Tree for testing conversion to a forest of orphans needed for incremental. D|C F|E */ /* ************************************************************************* */ -TEST( BayesTreeOrdered, removePath ) +TEST( BayesTree, removePath ) { const Key _A_=A(0), _B_=B(0), _C_=C(0), _D_=D(0), _E_=E(0), _F_=F(0); @@ -133,7 +133,7 @@ TEST( BayesTreeOrdered, removePath ) } /* ************************************************************************* */ -TEST( BayesTreeOrdered, removePath2 ) +TEST( BayesTree, removePath2 ) { SymbolicBayesTree bayesTree = asiaBayesTree; @@ -153,7 +153,7 @@ TEST( BayesTreeOrdered, removePath2 ) } /* ************************************************************************* */ -TEST( BayesTreeOrdered, removePath3 ) +TEST( BayesTree, removePath3 ) { SymbolicBayesTree bayesTree = asiaBayesTree; @@ -185,7 +185,7 @@ void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, SymbolicBayes } /* ************************************************************************* */ -TEST( BayesTreeOrdered, shortcutCheck ) +TEST( BayesTree, shortcutCheck ) { const Key _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0; SymbolicFactorGraph chain = list_of @@ -234,7 +234,7 @@ TEST( BayesTreeOrdered, shortcutCheck ) } /* ************************************************************************* */ -TEST( BayesTreeOrdered, removeTop ) +TEST( BayesTree, removeTop ) { SymbolicBayesTree bayesTree = asiaBayesTree; @@ -268,7 +268,7 @@ TEST( BayesTreeOrdered, removeTop ) } /* ************************************************************************* */ -TEST( BayesTreeOrdered, removeTop2 ) +TEST( BayesTree, removeTop2 ) { SymbolicBayesTree bayesTree = asiaBayesTree; @@ -294,7 +294,7 @@ TEST( BayesTreeOrdered, removeTop2 ) } /* ************************************************************************* */ -TEST( BayesTreeOrdered, removeTop3 ) +TEST( BayesTree, removeTop3 ) { SymbolicFactorGraph graph = list_of (SymbolicFactor(L(5))) @@ -318,7 +318,7 @@ TEST( BayesTreeOrdered, removeTop3 ) } /* ************************************************************************* */ -TEST( BayesTreeOrdered, removeTop4 ) +TEST( BayesTree, removeTop4 ) { SymbolicFactorGraph graph = list_of (SymbolicFactor(L(5))) @@ -342,7 +342,7 @@ TEST( BayesTreeOrdered, removeTop4 ) } /* ************************************************************************* */ -TEST( BayesTreeOrdered, removeTop5 ) +TEST( BayesTree, removeTop5 ) { // Remove top called with variables that are not in the Bayes tree SymbolicFactorGraph graph = list_of diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index b6340e93c..095e84a18 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -71,7 +71,7 @@ public: /* ************************************************************************* */ -TEST(EliminationTreeOrdered, Create) +TEST(EliminationTree, Create) { SymbolicEliminationTree expected = EliminationTreeTester::buildHardcodedTree(simpleTestGraph1); diff --git a/gtsam/symbolic/tests/testSymbolicFactor.cpp b/gtsam/symbolic/tests/testSymbolicFactor.cpp index 7596f9017..a9f01e2d9 100644 --- a/gtsam/symbolic/tests/testSymbolicFactor.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactor.cpp @@ -34,17 +34,17 @@ using namespace boost::assign; #ifdef TRACK_ELIMINATE TEST(SymbolicFactor, eliminate) { vector keys; keys += 2, 3, 4, 6, 7, 9, 10, 11; - IndexFactorOrdered actual(keys.begin(), keys.end()); - BayesNetOrdered fragment = *actual.eliminate(3); + IndexFactor actual(keys.begin(), keys.end()); + BayesNet fragment = *actual.eliminate(3); - IndexFactorOrdered expected(keys.begin()+3, keys.end()); - IndexConditionalOrdered::shared_ptr expected0 = IndexConditionalOrdered::FromRange(keys.begin(), keys.end(), 1); - IndexConditionalOrdered::shared_ptr expected1 = IndexConditionalOrdered::FromRange(keys.begin()+1, keys.end(), 1); - IndexConditionalOrdered::shared_ptr expected2 = IndexConditionalOrdered::FromRange(keys.begin()+2, keys.end(), 1); + IndexFactor expected(keys.begin()+3, keys.end()); + IndexConditional::shared_ptr expected0 = IndexConditional::FromRange(keys.begin(), keys.end(), 1); + IndexConditional::shared_ptr expected1 = IndexConditional::FromRange(keys.begin()+1, keys.end(), 1); + IndexConditional::shared_ptr expected2 = IndexConditional::FromRange(keys.begin()+2, keys.end(), 1); CHECK(assert_equal(fragment.size(), size_t(3))); CHECK(assert_equal(expected, actual)); - BayesNetOrdered::const_iterator fragmentCond = fragment.begin(); + BayesNet::const_iterator fragmentCond = fragment.begin(); CHECK(assert_equal(**fragmentCond++, *expected0)); CHECK(assert_equal(**fragmentCond++, *expected1)); CHECK(assert_equal(**fragmentCond++, *expected2)); diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index 6ac15d148..668d4e9d0 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -29,7 +29,7 @@ using namespace gtsam; using namespace boost::assign; /* ************************************************************************* */ -TEST(SymbolicFactorGraphOrdered, eliminateFullSequential) +TEST(SymbolicFactorGraph, eliminateFullSequential) { // Test with simpleTestGraph1 Ordering order; @@ -43,7 +43,7 @@ TEST(SymbolicFactorGraphOrdered, eliminateFullSequential) } /* ************************************************************************* */ -TEST(SymbolicFactorGraphOrdered, eliminatePartialSequential) +TEST(SymbolicFactorGraph, eliminatePartialSequential) { // Eliminate 0 and 1 const Ordering order = list_of(0)(1); @@ -67,7 +67,7 @@ TEST(SymbolicFactorGraphOrdered, eliminatePartialSequential) } /* ************************************************************************* */ -TEST(SymbolicFactorGraphOrdered, eliminateFullMultifrontal) +TEST(SymbolicFactorGraph, eliminateFullMultifrontal) { Ordering ordering; ordering += 0,1,2,3; SymbolicBayesTree actual1 = @@ -80,7 +80,7 @@ TEST(SymbolicFactorGraphOrdered, eliminateFullMultifrontal) } /* ************************************************************************* */ -TEST(SymbolicFactorGraphOrdered, eliminatePartialMultifrontal) +TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) { SymbolicBayesTree expectedBayesTree; SymbolicConditional::shared_ptr root = boost::make_shared( @@ -104,7 +104,7 @@ TEST(SymbolicFactorGraphOrdered, eliminatePartialMultifrontal) } /* ************************************************************************* */ -TEST(SymbolicFactorGraphOrdered, marginalMultifrontalBayesNet) +TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) { SymbolicBayesNet expectedBayesNet = list_of (SymbolicConditional(0, 1, 2)) @@ -118,7 +118,7 @@ TEST(SymbolicFactorGraphOrdered, marginalMultifrontalBayesNet) } /* ************************************************************************* */ -TEST(SymbolicFactorGraphOrdered, eliminate_disconnected_graph) { +TEST(SymbolicFactorGraph, eliminate_disconnected_graph) { SymbolicFactorGraph fg; fg.push_factor(0, 1); fg.push_factor(0, 2); @@ -141,10 +141,10 @@ TEST(SymbolicFactorGraphOrdered, eliminate_disconnected_graph) { } /* ************************************************************************* */ -//TEST(SymbolicFactorGraphOrdered, marginals) +//TEST(SymbolicFactorGraph, marginals) //{ // // Create factor graph -// SymbolicFactorGraphOrdered fg; +// SymbolicFactorGraph fg; // fg.push_factor(0, 1); // fg.push_factor(0, 2); // fg.push_factor(1, 4); @@ -176,12 +176,12 @@ TEST(SymbolicFactorGraphOrdered, eliminate_disconnected_graph) { // EXPECT( assert_equal(expectedBN,*actualBN)); // // // jointFactorGraph -// SymbolicFactorGraphOrdered::shared_ptr actualFG = solver.jointFactorGraph(js); -// SymbolicFactorGraphOrdered expectedFG; +// SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); +// SymbolicFactorGraph expectedFG; // expectedFG.push_factor(0, 4); // expectedFG.push_factor(4, 3); // expectedFG.push_factor(3); -// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraphOrdered)(*actualFG))); +// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); // } // // { @@ -198,12 +198,12 @@ TEST(SymbolicFactorGraphOrdered, eliminate_disconnected_graph) { // EXPECT( assert_equal(expectedBN,*actualBN)); // // // jointFactorGraph -// SymbolicFactorGraphOrdered::shared_ptr actualFG = solver.jointFactorGraph(js); -// SymbolicFactorGraphOrdered expectedFG; +// SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); +// SymbolicFactorGraph expectedFG; // expectedFG.push_factor(0, 3, 2); // expectedFG.push_factor(3, 2); // expectedFG.push_factor(2); -// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraphOrdered)(*actualFG))); +// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); // } // // { @@ -223,7 +223,7 @@ TEST(SymbolicFactorGraphOrdered, eliminate_disconnected_graph) { //} /* ************************************************************************* */ -TEST( SymbolicFactorGraphOrdered, constructFromBayesNet ) +TEST( SymbolicFactorGraph, constructFromBayesNet ) { // create expected factor graph SymbolicFactorGraph expected; @@ -244,7 +244,7 @@ TEST( SymbolicFactorGraphOrdered, constructFromBayesNet ) } /* ************************************************************************* */ -TEST( SymbolicFactorGraphOrdered, constructFromBayesTree ) +TEST( SymbolicFactorGraph, constructFromBayesTree ) { // create expected factor graph SymbolicFactorGraph expected; @@ -259,7 +259,7 @@ TEST( SymbolicFactorGraphOrdered, constructFromBayesTree ) } /* ************************************************************************* */ -TEST( SymbolicFactorGraphOrdered, push_back ) +TEST( SymbolicFactorGraph, push_back ) { // Create two factor graphs and expected combined graph SymbolicFactorGraph fg1, fg2, expected; diff --git a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp index e94919107..4893b4756 100644 --- a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp @@ -38,7 +38,7 @@ using namespace std; * 2 3 * 0 1 : 2 ****************************************************************************/ -TEST( JunctionTreeOrdered, constructor ) +TEST( JunctionTree, constructor ) { Ordering order; order += 0, 1, 2, 3; diff --git a/gtsam_unstable/linear/bayesTreeOperations.cpp b/gtsam_unstable/linear/bayesTreeOperations.cpp index 42808c318..c2e84dec3 100644 --- a/gtsam_unstable/linear/bayesTreeOperations.cpp +++ b/gtsam_unstable/linear/bayesTreeOperations.cpp @@ -14,7 +14,7 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -std::set keysToIndices(const KeySet& keys, const OrderingOrdered& ordering) { +std::set keysToIndices(const KeySet& keys, const Ordering& ordering) { std::set result; BOOST_FOREACH(const Key& key, keys) result.insert(ordering[key]); @@ -22,22 +22,22 @@ std::set keysToIndices(const KeySet& keys, const OrderingOrdered& orderin } /* ************************************************************************* */ -GaussianFactorGraphOrdered splitFactors(const GaussianFactorGraphOrdered& fullgraph) { - GaussianFactorGraphOrdered result; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, fullgraph) { - GaussianFactorGraphOrdered split = splitFactor(factor); +GaussianFactorGraph splitFactors(const GaussianFactorGraph& fullgraph) { + GaussianFactorGraph result; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fullgraph) { + GaussianFactorGraph split = splitFactor(factor); result.push_back(split); } return result; } /* ************************************************************************* */ -GaussianFactorGraphOrdered splitFactor(const GaussianFactorOrdered::shared_ptr& factor) { - GaussianFactorGraphOrdered result; +GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor) { + GaussianFactorGraph result; if (!factor) return result; // Needs to be a jacobian factor - just pass along hessians - JacobianFactorOrdered::shared_ptr jf = boost::dynamic_pointer_cast(factor); + JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(factor); if (!jf) { result.push_back(factor); return result; @@ -45,7 +45,7 @@ GaussianFactorGraphOrdered splitFactor(const GaussianFactorOrdered::shared_ptr& // Loop over variables and strip off factors using split conditionals // Assumes upper triangular structure - JacobianFactorOrdered::const_iterator rowIt, colIt; + JacobianFactor::const_iterator rowIt, colIt; const size_t totalRows = jf->rows(); size_t rowsRemaining = totalRows; for (rowIt = jf->begin(); rowIt != jf->end() && rowsRemaining > 0; ++rowIt) { @@ -80,10 +80,10 @@ GaussianFactorGraphOrdered splitFactor(const GaussianFactorOrdered::shared_ptr& } /* ************************************************************************* */ -GaussianFactorGraphOrdered removePriors(const GaussianFactorGraphOrdered& fullgraph) { - GaussianFactorGraphOrdered result; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, fullgraph) { - JacobianFactorOrdered::shared_ptr jf = boost::dynamic_pointer_cast(factor); +GaussianFactorGraph removePriors(const GaussianFactorGraph& fullgraph) { + GaussianFactorGraph result; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fullgraph) { + JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(factor); if (factor && (!jf || jf->size() > 1)) result.push_back(factor); } @@ -91,8 +91,8 @@ GaussianFactorGraphOrdered removePriors(const GaussianFactorGraphOrdered& fullgr } /* ************************************************************************* */ -void findCliques(const GaussianBayesTreeOrdered::sharedClique& current_clique, - std::set& result) { +void findCliques(const GaussianBayesTree::sharedClique& current_clique, + std::set& result) { // Add the current clique result.insert(current_clique->conditional()); @@ -102,12 +102,12 @@ void findCliques(const GaussianBayesTreeOrdered::sharedClique& current_clique, } /* ************************************************************************* */ -std::set findAffectedCliqueConditionals( - const GaussianBayesTreeOrdered& bayesTree, const std::set& savedIndices) { - std::set affected_cliques; +std::set findAffectedCliqueConditionals( + const GaussianBayesTree& bayesTree, const std::set& savedIndices) { + std::set affected_cliques; // FIXME: track previously found keys more efficiently BOOST_FOREACH(const Index& index, savedIndices) { - GaussianBayesTreeOrdered::sharedClique clique = bayesTree.nodes()[index]; + GaussianBayesTree::sharedClique clique = bayesTree.nodes()[index]; // add path back to root to affected set findCliques(clique, affected_cliques); @@ -116,9 +116,9 @@ std::set findAffectedCliqueConditionals( } /* ************************************************************************* */ -std::deque -findPathCliques(const GaussianBayesTreeOrdered::sharedClique& initial) { - std::deque result, parents; +std::deque +findPathCliques(const GaussianBayesTree::sharedClique& initial) { + std::deque result, parents; if (initial->isRoot()) return result; result.push_back(initial->parent()); diff --git a/gtsam_unstable/linear/bayesTreeOperations.h b/gtsam_unstable/linear/bayesTreeOperations.h index 5d2788f7b..3f85dc12c 100644 --- a/gtsam_unstable/linear/bayesTreeOperations.h +++ b/gtsam_unstable/linear/bayesTreeOperations.h @@ -19,7 +19,7 @@ namespace gtsam { // Managing orderings /** Converts sets of keys to indices by way of orderings */ -GTSAM_UNSTABLE_EXPORT std::set keysToIndices(const KeySet& keys, const OrderingOrdered& ordering); +GTSAM_UNSTABLE_EXPORT std::set keysToIndices(const KeySet& keys, const Ordering& ordering); // Linear Graph Operations @@ -27,16 +27,16 @@ GTSAM_UNSTABLE_EXPORT std::set keysToIndices(const KeySet& keys, const Or * Given a graph, splits each factor into factors where the dimension is * that of the first variable. */ -GTSAM_UNSTABLE_EXPORT GaussianFactorGraphOrdered splitFactors(const GaussianFactorGraphOrdered& fullgraph); +GTSAM_UNSTABLE_EXPORT GaussianFactorGraph splitFactors(const GaussianFactorGraph& fullgraph); /** * Splits a factor into factors where the dimension is * that of the first variable. */ -GTSAM_UNSTABLE_EXPORT GaussianFactorGraphOrdered splitFactor(const GaussianFactorOrdered::shared_ptr& factor); +GTSAM_UNSTABLE_EXPORT GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor); /** Removes prior jacobian factors from the graph */ -GTSAM_UNSTABLE_EXPORT GaussianFactorGraphOrdered removePriors(const GaussianFactorGraphOrdered& fullgraph); +GTSAM_UNSTABLE_EXPORT GaussianFactorGraph removePriors(const GaussianFactorGraph& fullgraph); // Bayes Tree / Conditional operations @@ -46,8 +46,8 @@ GTSAM_UNSTABLE_EXPORT GaussianFactorGraphOrdered removePriors(const GaussianFact * * @return the set of conditionals extracted from cliques. */ -GTSAM_UNSTABLE_EXPORT std::set findAffectedCliqueConditionals( - const GaussianBayesTreeOrdered& bayesTree, const std::set& savedIndices); +GTSAM_UNSTABLE_EXPORT std::set findAffectedCliqueConditionals( + const GaussianBayesTree& bayesTree, const std::set& savedIndices); /** * Recursively traverses from a given clique in a Bayes Tree and collects all of the conditionals @@ -56,15 +56,15 @@ GTSAM_UNSTABLE_EXPORT std::set findAffec * Note the use of a set of shared_ptr: this will sort/filter on unique *pointer* locations, * which ensures unique cliques, but the order of the cliques is meaningless */ -GTSAM_UNSTABLE_EXPORT void findCliqueConditionals(const GaussianBayesTreeOrdered::sharedClique& current_clique, - std::set& result); +GTSAM_UNSTABLE_EXPORT void findCliqueConditionals(const GaussianBayesTree::sharedClique& current_clique, + std::set& result); /** * Given a clique, returns a sequence of clique parents to the root, not including the * given clique. */ -GTSAM_UNSTABLE_EXPORT std::deque -findPathCliques(const GaussianBayesTreeOrdered::sharedClique& initial); +GTSAM_UNSTABLE_EXPORT std::deque +findPathCliques(const GaussianBayesTree::sharedClique& initial); /** * Liquefies a BayesTree into a GaussianFactorGraph recursively, given a @@ -73,10 +73,10 @@ findPathCliques(const GaussianBayesTreeOrdered::sharedClique& initial); * @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors */ template -GaussianFactorGraphOrdered liquefy(const typename BAYESTREE::sharedClique& root, bool splitConditionals = false) { - GaussianFactorGraphOrdered result; +GaussianFactorGraph liquefy(const typename BAYESTREE::sharedClique& root, bool splitConditionals = false) { + GaussianFactorGraph result; if (root && root->conditional()) { - GaussianConditionalOrdered::shared_ptr conditional = root->conditional(); + GaussianConditional::shared_ptr conditional = root->conditional(); if (!splitConditionals) result.push_back(conditional->toFactor()); else @@ -93,7 +93,7 @@ GaussianFactorGraphOrdered liquefy(const typename BAYESTREE::sharedClique& root, * @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors */ template -GaussianFactorGraphOrdered liquefy(const BAYESTREE& bayesTree, bool splitConditionals = false) { +GaussianFactorGraph liquefy(const BAYESTREE& bayesTree, bool splitConditionals = false) { return liquefy(bayesTree.root(), splitConditionals); } diff --git a/gtsam_unstable/linear/conditioning.cpp b/gtsam_unstable/linear/conditioning.cpp index b346729c2..c02f4d48c 100644 --- a/gtsam_unstable/linear/conditioning.cpp +++ b/gtsam_unstable/linear/conditioning.cpp @@ -13,8 +13,8 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -GaussianConditionalOrdered::shared_ptr conditionDensity(const GaussianConditionalOrdered::shared_ptr& initConditional, - const std::set& saved_indices, const VectorValuesOrdered& solution) { +GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shared_ptr& initConditional, + const std::set& saved_indices, const VectorValues& solution) { const bool verbose = false; if (!initConditional) @@ -41,7 +41,7 @@ GaussianConditionalOrdered::shared_ptr conditionDensity(const GaussianConditiona // If none of the frontal variables are to be saved, return empty pointer if (frontalsToRemove.size() == initConditional->nrFrontals()) - return GaussianConditionalOrdered::shared_ptr(); + return GaussianConditional::shared_ptr(); // Collect dimensions of the new conditional if (verbose) cout << " Collecting dimensions" << endl; @@ -54,7 +54,7 @@ GaussianConditionalOrdered::shared_ptr conditionDensity(const GaussianConditiona vector newIdxToOldIdx; // Access to arrays, maps from new var to old var const vector& oldIndices = initConditional->keys(); const size_t oldNrFrontals = initConditional->nrFrontals(); - GaussianConditionalOrdered::const_iterator varIt = initConditional->beginFrontals(); + GaussianConditional::const_iterator varIt = initConditional->beginFrontals(); size_t oldIdx = 0; for (; varIt != initConditional->endFrontals(); ++varIt) { size_t varDim = initConditional->dim(varIt); @@ -99,7 +99,7 @@ GaussianConditionalOrdered::shared_ptr conditionDensity(const GaussianConditiona size_t oldColOffset = oldColOffsets.at(newfrontalIdx); // get R block, sliced by row - Eigen::Block rblock = + Eigen::Block rblock = initConditional->get_R().block(oldColOffset, oldColOffset, dim, oldRNrCols-oldColOffset); if (verbose) cout << " rblock\n" << rblock << endl; @@ -142,7 +142,7 @@ GaussianConditionalOrdered::shared_ptr conditionDensity(const GaussianConditiona } // add parents (looping over original block structure), while updating rhs - GaussianConditionalOrdered::const_iterator oldParent = initConditional->beginParents(); + GaussianConditional::const_iterator oldParent = initConditional->beginParents(); for (; oldParent != initConditional->endParents(); ++oldParent) { Index parentKey = *oldParent; size_t parentDim = initConditional->dim(oldParent); @@ -169,26 +169,26 @@ GaussianConditionalOrdered::shared_ptr conditionDensity(const GaussianConditiona // Construct a new conditional if (verbose) cout << "backsubSummarize() Complete!" << endl; - GaussianConditionalOrdered::rsd_type matrices(full_matrix, newDims.begin(), newDims.end()); - return GaussianConditionalOrdered::shared_ptr(new - GaussianConditionalOrdered(newIndices.begin(), newIndices.end(), newNrFrontals, matrices, sigmas)); + GaussianConditional::rsd_type matrices(full_matrix, newDims.begin(), newDims.end()); + return GaussianConditional::shared_ptr(new + GaussianConditional(newIndices.begin(), newIndices.end(), newNrFrontals, matrices, sigmas)); } /* ************************************************************************* */ -GaussianFactorGraphOrdered conditionDensity(const GaussianBayesTreeOrdered& bayesTree, +GaussianFactorGraph conditionDensity(const GaussianBayesTree& bayesTree, const std::set& saved_indices) { const bool verbose = false; - VectorValuesOrdered solution = optimize(bayesTree); + VectorValues solution = optimize(bayesTree); // FIXME: set of conditionals does not manage possibility of solving out whole separators - std::set affected_cliques = findAffectedCliqueConditionals(bayesTree, saved_indices); + std::set affected_cliques = findAffectedCliqueConditionals(bayesTree, saved_indices); // Summarize conditionals separately - GaussianFactorGraphOrdered summarized_graph; - BOOST_FOREACH(const GaussianConditionalOrdered::shared_ptr& conditional, affected_cliques) { + GaussianFactorGraph summarized_graph; + BOOST_FOREACH(const GaussianConditional::shared_ptr& conditional, affected_cliques) { if (verbose) conditional->print("Initial conditional"); - GaussianConditionalOrdered::shared_ptr reducedConditional = conditionDensity(conditional, saved_indices, solution); + GaussianConditional::shared_ptr reducedConditional = conditionDensity(conditional, saved_indices, solution); if (reducedConditional) { if (verbose) reducedConditional->print("Final conditional"); summarized_graph.push_back(reducedConditional->toFactor()); diff --git a/gtsam_unstable/linear/conditioning.h b/gtsam_unstable/linear/conditioning.h index 41c9d1318..fbd5d6918 100644 --- a/gtsam_unstable/linear/conditioning.h +++ b/gtsam_unstable/linear/conditioning.h @@ -27,15 +27,15 @@ namespace gtsam { * @param saved_indices is the set of indices that should appear in the result * @param solution is a full solution for the system */ -GTSAM_UNSTABLE_EXPORT gtsam::GaussianConditionalOrdered::shared_ptr conditionDensity(const gtsam::GaussianConditionalOrdered::shared_ptr& initConditional, - const std::set& saved_indices, const gtsam::VectorValuesOrdered& solution); +GTSAM_UNSTABLE_EXPORT gtsam::GaussianConditional::shared_ptr conditionDensity(const gtsam::GaussianConditional::shared_ptr& initConditional, + const std::set& saved_indices, const gtsam::VectorValues& solution); /** * Backsubstitution-based conditioning for a complete Bayes Tree - reduces * conditionals by solving out variables to eliminate. Traverses the tree to * add the correct dummy factors whenever a separator is eliminated. */ -GTSAM_UNSTABLE_EXPORT gtsam::GaussianFactorGraphOrdered conditionDensity(const gtsam::GaussianBayesTreeOrdered& bayesTree, +GTSAM_UNSTABLE_EXPORT gtsam::GaussianFactorGraph conditionDensity(const gtsam::GaussianBayesTree& bayesTree, const std::set& saved_indices); diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 236c3cfad..0a84d307a 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -189,7 +189,7 @@ void BatchFixedLagSmoother::eraseKeys(const std::set& keys) { void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { // Calculate a variable index - VariableIndexOrdered variableIndex(*factors_.symbolic(ordering_), ordering_.size()); + VariableIndex variableIndex(*factors_.symbolic(ordering_), ordering_.size()); // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 int group0 = 0; @@ -236,7 +236,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { // Use a custom optimization loop so the linearization points can be controlled double previousError; - VectorValuesOrdered newDelta; + VectorValues newDelta; do { previousError = result.error; @@ -244,13 +244,13 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { gttic(optimizer_iteration); { // Linearize graph around the linearization point - GaussianFactorGraphOrdered linearFactorGraph = *factors_.linearize(theta_, ordering_); + GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); // Keep increasing lambda until we make make progress while(true) { // Add prior factors at the current solution gttic(damp); - GaussianFactorGraphOrdered dampedFactorGraph(linearFactorGraph); + GaussianFactorGraph dampedFactorGraph(linearFactorGraph); dampedFactorGraph.reserve(linearFactorGraph.size() + delta_.size()); { // for each of the variables, add a prior at the current solution @@ -260,7 +260,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { Matrix A = eye(dim); Vector b = delta_[j]; SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); - GaussianFactorOrdered::shared_ptr prior(new JacobianFactorOrdered(j, A, b, model)); + GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model)); dampedFactorGraph.push_back(prior); } } @@ -269,7 +269,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { gttic(solve); // Solve Damped Gaussian Factor Graph - newDelta = GaussianJunctionTreeOrdered(dampedFactorGraph).optimize(parameters_.getEliminationFunction()); + newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters_.getEliminationFunction()); // update the evalpoint with the new delta evalpoint = theta_.retract(newDelta, ordering_); gttoc(solve); @@ -334,10 +334,10 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { // Calculate marginal factors on the remaining variables (after marginalizing 'marginalizeKeys') // Note: It is assumed the ordering already has these keys first // Create the linear factor graph - GaussianFactorGraphOrdered linearFactorGraph = *factors_.linearize(theta_, ordering_); + GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); // Create a variable index - VariableIndexOrdered variableIndex(linearFactorGraph, ordering_.size()); + VariableIndex variableIndex(linearFactorGraph, ordering_.size()); // Use the variable Index to mark the factors that will be marginalized std::set removedFactorSlots; @@ -364,7 +364,7 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { // Add the marginal factor variables to the separator NonlinearFactorGraph marginalFactors; BOOST_FOREACH(Index index, indicesToEliminate) { - GaussianFactorOrdered::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); + GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); if(gaussianFactor->size() > 0) { LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_)); marginalFactors.push_back(marginalFactor); @@ -409,7 +409,7 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_pt } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering) { +void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering) { std::cout << "f("; BOOST_FOREACH(Index index, factor->keys()) { std::cout << " " << index << "[" << gtsam::DefaultKeyFormatter(ordering.key(index)) << "]"; @@ -426,15 +426,15 @@ void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraphOrdered& graph, const OrderingOrdered& ordering, const std::string& label) { +void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const Ordering& ordering, const std::string& label) { std::cout << label << std::endl; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, graph) { + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { PrintSymbolicFactor(factor, ordering); } } /* ************************************************************************* */ -std::vector BatchFixedLagSmoother::EliminationForest::ComputeParents(const VariableIndexOrdered& structure) { +std::vector BatchFixedLagSmoother::EliminationForest::ComputeParents(const VariableIndex& structure) { // Number of factors and variables const size_t m = structure.nFactors(); const size_t n = structure.size(); @@ -465,7 +465,7 @@ std::vector BatchFixedLagSmoother::EliminationForest::ComputeParents(cons } /* ************************************************************************* */ -std::vector BatchFixedLagSmoother::EliminationForest::Create(const GaussianFactorGraphOrdered& factorGraph, const VariableIndexOrdered& structure) { +std::vector BatchFixedLagSmoother::EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) { // Compute the tree structure std::vector parents(ComputeParents(structure)); @@ -484,7 +484,7 @@ std::vector BatchFixedLagS } // Hang factors in right places - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factorGraph) { + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorGraph) { if(factor && factor->size() > 0) { Index j = *std::min_element(factor->begin(), factor->end()); if(j < structure.size()) @@ -496,10 +496,10 @@ std::vector BatchFixedLagS } /* ************************************************************************* */ -GaussianFactorOrdered::shared_ptr BatchFixedLagSmoother::EliminationForest::eliminateRecursive(GaussianFactorGraphOrdered::Eliminate function) { +GaussianFactor::shared_ptr BatchFixedLagSmoother::EliminationForest::eliminateRecursive(GaussianFactorGraph::Eliminate function) { // Create the list of factors to be eliminated, initially empty, and reserve space - GaussianFactorGraphOrdered factors; + GaussianFactorGraph factors; factors.reserve(this->factors_.size() + this->subTrees_.size()); // Add all factors associated with the current node @@ -510,7 +510,7 @@ GaussianFactorOrdered::shared_ptr BatchFixedLagSmoother::EliminationForest::elim factors.push_back(child->eliminateRecursive(function)); // Combine all factors (from this node and from subtrees) into a joint factor - GaussianFactorGraphOrdered::EliminationResult eliminated(function(factors, 1)); + GaussianFactorGraph::EliminationResult eliminated(function(factors, 1)); return eliminated.second; } diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 4c832f85b..dd22939bf 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -92,12 +92,12 @@ public: } /** Access the current ordering */ - const OrderingOrdered& getOrdering() const { + const Ordering& getOrdering() const { return ordering_; } /** Access the current set of deltas to the linearization point */ - const VectorValuesOrdered& getDelta() const { + const VectorValues& getDelta() const { return delta_; } @@ -124,10 +124,10 @@ protected: Values linearKeys_; /** The current ordering */ - OrderingOrdered ordering_; + Ordering ordering_; /** The current set of linear deltas */ - VectorValuesOrdered delta_; + VectorValues delta_; /** The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes. **/ std::queue availableSlots_; @@ -162,9 +162,9 @@ protected: typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class private: - typedef FastList Factors; + typedef FastList Factors; typedef FastList SubTrees; - typedef std::vector Conditionals; + typedef std::vector Conditionals; Index key_; ///< index associated with root Factors factors_; ///< factors associated with root @@ -177,10 +177,10 @@ protected: * Static internal function to build a vector of parent pointers using the * algorithm of Gilbert et al., 2001, BIT. */ - static std::vector ComputeParents(const VariableIndexOrdered& structure); + static std::vector ComputeParents(const VariableIndex& structure); /** add a factor, for Create use only */ - void add(const GaussianFactorOrdered::shared_ptr& factor) { factors_.push_back(factor); } + void add(const GaussianFactor::shared_ptr& factor) { factors_.push_back(factor); } /** add a subtree, for Create use only */ void add(const shared_ptr& child) { subTrees_.push_back(child); } @@ -197,10 +197,10 @@ protected: const Factors& factors() const { return factors_; } /** Create an elimination tree from a factor graph */ - static std::vector Create(const GaussianFactorGraphOrdered& factorGraph, const VariableIndexOrdered& structure); + static std::vector Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure); /** Recursive routine that eliminates the factors arranged in an elimination tree */ - GaussianFactorOrdered::shared_ptr eliminateRecursive(GaussianFactorGraphOrdered::Eliminate function); + GaussianFactor::shared_ptr eliminateRecursive(GaussianFactorGraph::Eliminate function); /** Recursive function that helps find the top of each tree */ static void removeChildrenIndices(std::set& indices, const EliminationForest::shared_ptr& tree); @@ -210,9 +210,9 @@ private: /** Private methods for printing debug information */ static void PrintKeySet(const std::set& keys, const std::string& label); static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor); - static void PrintSymbolicFactor(const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering); + static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering); static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label); - static void PrintSymbolicGraph(const GaussianFactorGraphOrdered& graph, const OrderingOrdered& ordering, const std::string& label); + static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const Ordering& ordering, const std::string& label); }; // BatchFixedLagSmoother } /// namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 22485c011..1e1e39e10 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -128,8 +128,8 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFa // Perform an optional optimization on the to-be-sent-to-the-smoother factors if(relin_) { // Create ordering and delta - OrderingOrdered ordering = *graph.orderingCOLAMD(values); - VectorValuesOrdered delta = values.zeroVectors(ordering); + Ordering ordering = *graph.orderingCOLAMD(values); + VectorValues delta = values.zeroVectors(ordering); // Optimize this graph using a modified version of L-M optimize(graph, values, ordering, delta, separatorValues, parameters_); // Update filter theta and delta @@ -162,8 +162,8 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFa // Generate separate orderings that place the filter keys or the smoother keys first // TODO: This is convenient, but it recalculates the variable index each time - OrderingOrdered filterOrdering = *graph.orderingCOLAMDConstrained(values, filterConstraints); - OrderingOrdered smootherOrdering = *graph.orderingCOLAMDConstrained(values, smootherConstraints); + Ordering filterOrdering = *graph.orderingCOLAMDConstrained(values, filterConstraints); + Ordering smootherOrdering = *graph.orderingCOLAMDConstrained(values, smootherConstraints); // Extract the set of filter keys and smoother keys std::set filterKeys; @@ -276,7 +276,7 @@ void ConcurrentBatchFilter::removeFactors(const std::vector& slots) { gttic(remove_factors); // For each factor slot to delete... - SymbolicFactorGraphOrdered factors; + SymbolicFactorGraph factors; BOOST_FOREACH(size_t slot, slots) { // Create a symbolic version for the variable index factors.push_back(factors_.at(slot)->symbolic(ordering_)); @@ -295,7 +295,7 @@ void ConcurrentBatchFilter::removeFactors(const std::vector& slots) { void ConcurrentBatchFilter::reorder(const boost::optional >& keysToMove) { // Calculate the variable index - VariableIndexOrdered variableIndex(*factors_.symbolic(ordering_), ordering_.size()); + VariableIndex variableIndex(*factors_.symbolic(ordering_), ordering_.size()); // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 int group0 = 0; @@ -320,8 +320,8 @@ void ConcurrentBatchFilter::reorder(const boost::optional >& keysT } /* ************************************************************************* */ -ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values& theta, const OrderingOrdered& ordering, - VectorValuesOrdered& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters) { +ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering, + VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters) { // Create output result structure Result result; @@ -344,7 +344,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac // Use a custom optimization loop so the linearization points can be controlled double previousError; - VectorValuesOrdered newDelta; + VectorValues newDelta; do { previousError = result.error; @@ -352,13 +352,13 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac gttic(optimizer_iteration); { // Linearize graph around the linearization point - GaussianFactorGraphOrdered linearFactorGraph = *factors.linearize(theta, ordering); + GaussianFactorGraph linearFactorGraph = *factors.linearize(theta, ordering); // Keep increasing lambda until we make make progress while(true) { // Add prior factors at the current solution gttic(damp); - GaussianFactorGraphOrdered dampedFactorGraph(linearFactorGraph); + GaussianFactorGraph dampedFactorGraph(linearFactorGraph); dampedFactorGraph.reserve(linearFactorGraph.size() + delta.size()); { // for each of the variables, add a prior at the current solution @@ -366,7 +366,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac Matrix A = lambda * eye(delta[j].size()); Vector b = lambda * delta[j]; SharedDiagonal model = noiseModel::Unit::Create(delta[j].size()); - GaussianFactorOrdered::shared_ptr prior(new JacobianFactorOrdered(j, A, b, model)); + GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model)); dampedFactorGraph.push_back(prior); } } @@ -375,7 +375,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac gttic(solve); // Solve Damped Gaussian Factor Graph - newDelta = GaussianJunctionTreeOrdered(dampedFactorGraph).optimize(parameters.getEliminationFunction()); + newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters.getEliminationFunction()); // update the evalpoint with the new delta evalpoint = theta.retract(newDelta, ordering); gttoc(solve); @@ -442,10 +442,10 @@ void ConcurrentBatchFilter::marginalize(const FastList& keysToMove) { // Note: It is assumed the ordering already has these keys first // Create the linear factor graph - GaussianFactorGraphOrdered linearFactorGraph = *factors_.linearize(theta_, ordering_); + GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); // Calculate the variable index - VariableIndexOrdered variableIndex(linearFactorGraph, ordering_.size()); + VariableIndex variableIndex(linearFactorGraph, ordering_.size()); // Use the variable Index to mark the factors that will be marginalized std::set removedFactorSlots; @@ -472,7 +472,7 @@ void ConcurrentBatchFilter::marginalize(const FastList& keysToMove) { // Add the marginal factor variables to the separator NonlinearFactorGraph marginalFactors; BOOST_FOREACH(Index index, indicesToEliminate) { - GaussianFactorOrdered::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); + GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); if(gaussianFactor->size() > 0) { LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_)); marginalFactors.push_back(marginalFactor); @@ -545,16 +545,16 @@ void ConcurrentBatchFilter::marginalize(const FastList& keysToMove) { /* ************************************************************************* */ NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGraph& graph, const Values& values, - const OrderingOrdered& ordering, const std::set& marginalizeKeys, const GaussianFactorGraphOrdered::Eliminate& function) { + const Ordering& ordering, const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& function) { // Calculate marginal factors on the remaining variables (after marginalizing 'marginalizeKeys') // Note: It is assumed the ordering already has these keys first // Create the linear factor graph - GaussianFactorGraphOrdered linearFactorGraph = *graph.linearize(values, ordering); + GaussianFactorGraph linearFactorGraph = *graph.linearize(values, ordering); // Construct a variable index - VariableIndexOrdered variableIndex(linearFactorGraph, ordering.size()); + VariableIndex variableIndex(linearFactorGraph, ordering.size()); // Construct an elimination tree to perform sparse elimination std::vector forest( EliminationForest::Create(linearFactorGraph, variableIndex) ); @@ -574,7 +574,7 @@ NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGra // Add the marginal factor variables to the separator NonlinearFactorGraph marginalFactors; BOOST_FOREACH(Index index, indicesToEliminate) { - GaussianFactorOrdered::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(function); + GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(function); if(gaussianFactor->size() > 0) { LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values)); marginalFactors.push_back(marginalFactor); @@ -604,7 +604,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p } /* ************************************************************************* */ -void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering, +void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, const std::string& indent, const KeyFormatter& keyFormatter) { std::cout << indent; if(factor) { @@ -619,7 +619,7 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactorOrdered::share } /* ************************************************************************* */ -std::vector ConcurrentBatchFilter::EliminationForest::ComputeParents(const VariableIndexOrdered& structure) { +std::vector ConcurrentBatchFilter::EliminationForest::ComputeParents(const VariableIndex& structure) { // Number of factors and variables const size_t m = structure.nFactors(); const size_t n = structure.size(); @@ -650,7 +650,7 @@ std::vector ConcurrentBatchFilter::EliminationForest::ComputeParents(cons } /* ************************************************************************* */ -std::vector ConcurrentBatchFilter::EliminationForest::Create(const GaussianFactorGraphOrdered& factorGraph, const VariableIndexOrdered& structure) { +std::vector ConcurrentBatchFilter::EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) { // Compute the tree structure std::vector parents(ComputeParents(structure)); @@ -669,7 +669,7 @@ std::vector ConcurrentBatc } // Hang factors in right places - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factorGraph) { + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorGraph) { if(factor && factor->size() > 0) { Index j = *std::min_element(factor->begin(), factor->end()); if(j < structure.size()) @@ -681,10 +681,10 @@ std::vector ConcurrentBatc } /* ************************************************************************* */ -GaussianFactorOrdered::shared_ptr ConcurrentBatchFilter::EliminationForest::eliminateRecursive(GaussianFactorGraphOrdered::Eliminate function) { +GaussianFactor::shared_ptr ConcurrentBatchFilter::EliminationForest::eliminateRecursive(GaussianFactorGraph::Eliminate function) { // Create the list of factors to be eliminated, initially empty, and reserve space - GaussianFactorGraphOrdered factors; + GaussianFactorGraph factors; factors.reserve(this->factors_.size() + this->subTrees_.size()); // Add all factors associated with the current node @@ -695,7 +695,7 @@ GaussianFactorOrdered::shared_ptr ConcurrentBatchFilter::EliminationForest::elim factors.push_back(child->eliminateRecursive(function)); // Combine all factors (from this node and from subtrees) into a joint factor - GaussianFactorGraphOrdered::EliminationResult eliminated(function(factors, 1)); + GaussianFactorGraph::EliminationResult eliminated(function(factors, 1)); return eliminated.second; } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 31122f2e7..9a7690786 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -76,12 +76,12 @@ public: } /** Access the current ordering */ - const OrderingOrdered& getOrdering() const { + const Ordering& getOrdering() const { return ordering_; } /** Access the current set of deltas to the linearization point */ - const VectorValuesOrdered& getDelta() const { + const VectorValues& getDelta() const { return delta_; } @@ -125,8 +125,8 @@ protected: bool relin_; NonlinearFactorGraph factors_; ///< The set of all factors currently in the filter Values theta_; ///< Current linearization point of all variables in the filter - OrderingOrdered ordering_; ///< The current ordering used to calculate the linear deltas - VectorValuesOrdered delta_; ///< The current set of linear deltas from the linearization point + Ordering ordering_; ///< The current ordering used to calculate the linear deltas + VectorValues delta_; ///< The current set of linear deltas from the linearization point std::queue availableSlots_; ///< The set of available factor graph slots caused by deleting factors Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization. std::vector smootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization factors @@ -194,8 +194,8 @@ private: void reorder(const boost::optional >& keysToMove = boost::none); /** Use a modified version of L-M to update the linearization point and delta */ - static Result optimize(const NonlinearFactorGraph& factors, Values& theta, const OrderingOrdered& ordering, - VectorValuesOrdered& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters); + static Result optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering, + VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters); /** Marginalize out the set of requested variables from the filter, caching them for the smoother * This effectively moves the separator. @@ -206,14 +206,14 @@ private: * This effectively moves the separator. */ static NonlinearFactorGraph marginalize(const NonlinearFactorGraph& graph, const Values& values, - const OrderingOrdered& ordering, const std::set& marginalizeKeys, const GaussianFactorGraphOrdered::Eliminate& function = EliminateQROrdered); + const Ordering& ordering, const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& function = EliminateQR); /** Print just the nonlinear keys in a nonlinear factor */ static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor, const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** Print just the nonlinear keys in a linear factor */ - static void PrintLinearFactor(const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering, + static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); // A custom elimination tree that supports forests and partial elimination @@ -222,9 +222,9 @@ private: typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class private: - typedef FastList Factors; + typedef FastList Factors; typedef FastList SubTrees; - typedef std::vector Conditionals; + typedef std::vector Conditionals; Index key_; ///< index associated with root Factors factors_; ///< factors associated with root @@ -237,10 +237,10 @@ private: * Static internal function to build a vector of parent pointers using the * algorithm of Gilbert et al., 2001, BIT. */ - static std::vector ComputeParents(const VariableIndexOrdered& structure); + static std::vector ComputeParents(const VariableIndex& structure); /** add a factor, for Create use only */ - void add(const GaussianFactorOrdered::shared_ptr& factor) { factors_.push_back(factor); } + void add(const GaussianFactor::shared_ptr& factor) { factors_.push_back(factor); } /** add a subtree, for Create use only */ void add(const shared_ptr& child) { subTrees_.push_back(child); } @@ -257,10 +257,10 @@ private: const Factors& factors() const { return factors_; } /** Create an elimination tree from a factor graph */ - static std::vector Create(const GaussianFactorGraphOrdered& factorGraph, const VariableIndexOrdered& structure); + static std::vector Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure); /** Recursive routine that eliminates the factors arranged in an elimination tree */ - GaussianFactorOrdered::shared_ptr eliminateRecursive(GaussianFactorGraphOrdered::Eliminate function); + GaussianFactor::shared_ptr eliminateRecursive(GaussianFactorGraph::Eliminate function); /** Recursive function that helps find the top of each tree */ static void removeChildrenIndices(std::set& indices, const EliminationForest::shared_ptr& tree); diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index f25f9b294..1e9d4a2a9 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -217,7 +217,7 @@ void ConcurrentBatchSmoother::removeFactors(const std::vector& slots) { gttic(remove_factors); // For each factor slot to delete... - SymbolicFactorGraphOrdered factors; + SymbolicFactorGraph factors; BOOST_FOREACH(size_t slot, slots) { // Create a symbolic version for the variable index factors.push_back(factors_.at(slot)->symbolic(ordering_)); @@ -236,7 +236,7 @@ void ConcurrentBatchSmoother::removeFactors(const std::vector& slots) { void ConcurrentBatchSmoother::reorder() { // Recalculate the variable index - variableIndex_ = VariableIndexOrdered(*factors_.symbolic(ordering_)); + variableIndex_ = VariableIndex(*factors_.symbolic(ordering_)); // Initialize all variables to group0 std::vector cmember(variableIndex_.size(), 0); @@ -281,7 +281,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { // Use a custom optimization loop so the linearization points can be controlled double previousError; - VectorValuesOrdered newDelta; + VectorValues newDelta; do { previousError = result.error; @@ -289,13 +289,13 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { gttic(optimizer_iteration); { // Linearize graph around the linearization point - GaussianFactorGraphOrdered linearFactorGraph = *factors_.linearize(theta_, ordering_); + GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); // Keep increasing lambda until we make make progress while(true) { // Add prior factors at the current solution gttic(damp); - GaussianFactorGraphOrdered dampedFactorGraph(linearFactorGraph); + GaussianFactorGraph dampedFactorGraph(linearFactorGraph); dampedFactorGraph.reserve(linearFactorGraph.size() + delta_.size()); { // for each of the variables, add a prior at the current solution @@ -303,7 +303,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { Matrix A = lambda * eye(delta_[j].size()); Vector b = lambda * delta_[j]; SharedDiagonal model = noiseModel::Unit::Create(delta_[j].size()); - GaussianFactorOrdered::shared_ptr prior(new JacobianFactorOrdered(j, A, b, model)); + GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model)); dampedFactorGraph.push_back(prior); } } @@ -312,7 +312,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { gttic(solve); // Solve Damped Gaussian Factor Graph - newDelta = GaussianJunctionTreeOrdered(dampedFactorGraph).optimize(parameters_.getEliminationFunction()); + newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters_.getEliminationFunction()); // update the evalpoint with the new delta evalpoint = theta_.retract(newDelta, ordering_); gttoc(solve); @@ -382,7 +382,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() { reorder(); // Create the linear factor graph - GaussianFactorGraphOrdered linearFactorGraph = *factors_.linearize(theta_, ordering_); + GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); // Construct an elimination tree to perform sparse elimination std::vector forest( EliminationForest::Create(linearFactorGraph, variableIndex_) ); @@ -404,7 +404,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() { // Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables // Convert the marginal factors into Linear Container Factors and store BOOST_FOREACH(Index index, indicesToEliminate) { - GaussianFactorOrdered::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); + GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); if(gaussianFactor->size() > 0) { LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_)); smootherSummarization_.push_back(marginalFactor); @@ -432,7 +432,7 @@ void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared } /* ************************************************************************* */ -void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering, const std::string& indent, const KeyFormatter& keyFormatter) { +void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, const std::string& indent, const KeyFormatter& keyFormatter) { std::cout << indent; if(factor) { std::cout << "g( "; @@ -446,7 +446,7 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactorOrdered::sha } /* ************************************************************************* */ -std::vector ConcurrentBatchSmoother::EliminationForest::ComputeParents(const VariableIndexOrdered& structure) { +std::vector ConcurrentBatchSmoother::EliminationForest::ComputeParents(const VariableIndex& structure) { // Number of factors and variables const size_t m = structure.nFactors(); const size_t n = structure.size(); @@ -477,7 +477,7 @@ std::vector ConcurrentBatchSmoother::EliminationForest::ComputeParents(co } /* ************************************************************************* */ -std::vector ConcurrentBatchSmoother::EliminationForest::Create(const GaussianFactorGraphOrdered& factorGraph, const VariableIndexOrdered& structure) { +std::vector ConcurrentBatchSmoother::EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) { // Compute the tree structure std::vector parents(ComputeParents(structure)); @@ -496,7 +496,7 @@ std::vector ConcurrentBa } // Hang factors in right places - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factorGraph) { + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorGraph) { if(factor && factor->size() > 0) { Index j = *std::min_element(factor->begin(), factor->end()); if(j < structure.size()) @@ -508,10 +508,10 @@ std::vector ConcurrentBa } /* ************************************************************************* */ -GaussianFactorOrdered::shared_ptr ConcurrentBatchSmoother::EliminationForest::eliminateRecursive(GaussianFactorGraphOrdered::Eliminate function) { +GaussianFactor::shared_ptr ConcurrentBatchSmoother::EliminationForest::eliminateRecursive(GaussianFactorGraph::Eliminate function) { // Create the list of factors to be eliminated, initially empty, and reserve space - GaussianFactorGraphOrdered factors; + GaussianFactorGraph factors; factors.reserve(this->factors_.size() + this->subTrees_.size()); // Add all factors associated with the current node @@ -522,7 +522,7 @@ GaussianFactorOrdered::shared_ptr ConcurrentBatchSmoother::EliminationForest::el factors.push_back(child->eliminateRecursive(function)); // Combine all factors (from this node and from subtrees) into a joint factor - GaussianFactorGraphOrdered::EliminationResult eliminated(function(factors, 1)); + GaussianFactorGraph::EliminationResult eliminated(function(factors, 1)); return eliminated.second; } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index 80ce9724b..f54c1f60d 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -76,12 +76,12 @@ public: } /** Access the current ordering */ - const OrderingOrdered& getOrdering() const { + const Ordering& getOrdering() const { return ordering_; } /** Access the current set of deltas to the linearization point */ - const VectorValuesOrdered& getDelta() const { + const VectorValues& getDelta() const { return delta_; } @@ -126,9 +126,9 @@ protected: LevenbergMarquardtParams parameters_; ///< LM parameters NonlinearFactorGraph factors_; ///< The set of all factors currently in the smoother Values theta_; ///< Current linearization point of all variables in the smoother - OrderingOrdered ordering_; ///< The current ordering used to calculate the linear deltas - VectorValuesOrdered delta_; ///< The current set of linear deltas from the linearization point - VariableIndexOrdered variableIndex_; ///< The current variable index, which allows efficient factor lookup by variable + Ordering ordering_; ///< The current ordering used to calculate the linear deltas + VectorValues delta_; ///< The current set of linear deltas from the linearization point + VariableIndex variableIndex_; ///< The current variable index, which allows efficient factor lookup by variable std::queue availableSlots_; ///< The set of available factor graph slots caused by deleting factors Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization. std::vector filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors @@ -197,7 +197,7 @@ private: const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** Print just the nonlinear keys in a linear factor */ - static void PrintLinearFactor(const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering, + static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); // A custom elimination tree that supports forests and partial elimination @@ -206,9 +206,9 @@ private: typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class private: - typedef FastList Factors; + typedef FastList Factors; typedef FastList SubTrees; - typedef std::vector Conditionals; + typedef std::vector Conditionals; Index key_; ///< index associated with root Factors factors_; ///< factors associated with root @@ -221,10 +221,10 @@ private: * Static internal function to build a vector of parent pointers using the * algorithm of Gilbert et al., 2001, BIT. */ - static std::vector ComputeParents(const VariableIndexOrdered& structure); + static std::vector ComputeParents(const VariableIndex& structure); /** add a factor, for Create use only */ - void add(const GaussianFactorOrdered::shared_ptr& factor) { factors_.push_back(factor); } + void add(const GaussianFactor::shared_ptr& factor) { factors_.push_back(factor); } /** add a subtree, for Create use only */ void add(const shared_ptr& child) { subTrees_.push_back(child); } @@ -241,10 +241,10 @@ private: const Factors& factors() const { return factors_; } /** Create an elimination tree from a factor graph */ - static std::vector Create(const GaussianFactorGraphOrdered& factorGraph, const VariableIndexOrdered& structure); + static std::vector Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure); /** Recursive routine that eliminates the factors arranged in an elimination tree */ - GaussianFactorOrdered::shared_ptr eliminateRecursive(GaussianFactorGraphOrdered::Eliminate function); + GaussianFactor::shared_ptr eliminateRecursive(GaussianFactorGraph::Eliminate function); /** Recursive function that helps find the top of each tree */ static void removeChildrenIndices(std::set& indices, const EliminationForest::shared_ptr& tree); diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 8b8927661..2341fc449 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -25,7 +25,7 @@ namespace gtsam { /* ************************************************************************* */ -void recursiveMarkAffectedKeys(const Index& index, const ISAM2Clique::shared_ptr& clique, const OrderingOrdered& ordering, std::set& additionalKeys) { +void recursiveMarkAffectedKeys(const Index& index, const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, std::set& additionalKeys) { // Check if the separator keys of the current clique contain the specified key if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), index) != clique->conditional()->endParents()) { @@ -184,7 +184,7 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, const s } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactorOrdered::shared_ptr& factor, const gtsam::OrderingOrdered& ordering) { +void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering) { std::cout << "f("; BOOST_FOREACH(Index index, factor->keys()) { std::cout << " " << index << "[" << gtsam::DefaultKeyFormatter(ordering.key(index)) << "]"; @@ -193,9 +193,9 @@ void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactorOrdere } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraphOrdered& graph, const gtsam::OrderingOrdered& ordering, const std::string& label) { +void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const std::string& label) { std::cout << label << std::endl; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, graph) { + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { PrintSymbolicFactor(factor, ordering); } } @@ -210,7 +210,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, co } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::OrderingOrdered& ordering, const std::string indent) { +void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string indent) { // Print the current clique std::cout << indent << "P( "; diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 4c108c81f..b46a08074 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -93,10 +93,10 @@ protected: private: /** Private methods for printing debug information */ static void PrintKeySet(const std::set& keys, const std::string& label = "Keys:"); - static void PrintSymbolicFactor(const GaussianFactorOrdered::shared_ptr& factor, const gtsam::OrderingOrdered& ordering); - static void PrintSymbolicGraph(const GaussianFactorGraphOrdered& graph, const gtsam::OrderingOrdered& ordering, const std::string& label = "Factor Graph:"); + static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering); + static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const std::string& label = "Factor Graph:"); static void PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label = "Bayes Tree:"); - static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::OrderingOrdered& ordering, const std::string indent = ""); + static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string indent = ""); }; // IncrementalFixedLagSmoother diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index fe0eb9328..eb5a83c79 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -22,7 +22,7 @@ namespace gtsam { /* ************************************************************************* */ -LinearizedGaussianFactor::LinearizedGaussianFactor(const GaussianFactorOrdered::shared_ptr& gaussian, const OrderingOrdered& ordering, const Values& lin_points) { +LinearizedGaussianFactor::LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Ordering& ordering, const Values& lin_points) { // Extract the keys and linearization points BOOST_FOREACH(const Index& idx, gaussian->keys()) { // find full symbol @@ -48,8 +48,8 @@ LinearizedJacobianFactor::LinearizedJacobianFactor() : Ab_(matrix_) { } /* ************************************************************************* */ -LinearizedJacobianFactor::LinearizedJacobianFactor(const JacobianFactorOrdered::shared_ptr& jacobian, - const OrderingOrdered& ordering, const Values& lin_points) +LinearizedJacobianFactor::LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian, + const Ordering& ordering, const Values& lin_points) : Base(jacobian, ordering, lin_points), Ab_(matrix_) { // Get the Ab matrix from the Jacobian factor, with any covariance baked in @@ -58,7 +58,7 @@ LinearizedJacobianFactor::LinearizedJacobianFactor(const JacobianFactorOrdered:: // Create the dims array size_t *dims = (size_t *)alloca(sizeof(size_t) * (jacobian->size() + 1)); size_t index = 0; - for(JacobianFactorOrdered::const_iterator iter = jacobian->begin(); iter != jacobian->end(); ++iter) { + for(JacobianFactor::const_iterator iter = jacobian->begin(); iter != jacobian->end(); ++iter) { dims[index++] = jacobian->getDim(iter); } dims[index] = 1; @@ -109,8 +109,8 @@ double LinearizedJacobianFactor::error(const Values& c) const { } /* ************************************************************************* */ -boost::shared_ptr -LinearizedJacobianFactor::linearize(const Values& c, const OrderingOrdered& ordering) const { +boost::shared_ptr +LinearizedJacobianFactor::linearize(const Values& c, const Ordering& ordering) const { // Create the 'terms' data structure for the Jacobian constructor std::vector > terms; @@ -121,7 +121,7 @@ LinearizedJacobianFactor::linearize(const Values& c, const OrderingOrdered& orde // compute rhs Vector b = -error_vector(c); - return boost::shared_ptr(new JacobianFactorOrdered(terms, b, noiseModel::Unit::Create(dim()))); + return boost::shared_ptr(new JacobianFactor(terms, b, noiseModel::Unit::Create(dim()))); } /* ************************************************************************* */ @@ -150,8 +150,8 @@ LinearizedHessianFactor::LinearizedHessianFactor() : info_(matrix_) { } /* ************************************************************************* */ -LinearizedHessianFactor::LinearizedHessianFactor(const HessianFactorOrdered::shared_ptr& hessian, - const OrderingOrdered& ordering, const Values& lin_points) +LinearizedHessianFactor::LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian, + const Ordering& ordering, const Values& lin_points) : Base(hessian, ordering, lin_points), info_(matrix_) { // Copy the augmented matrix holding G, g, and f @@ -160,7 +160,7 @@ LinearizedHessianFactor::LinearizedHessianFactor(const HessianFactorOrdered::sha // Create the dims array size_t *dims = (size_t*)alloca(sizeof(size_t)*(hessian->size() + 1)); size_t index = 0; - for(HessianFactorOrdered::const_iterator iter = hessian->begin(); iter != hessian->end(); ++iter) { + for(HessianFactor::const_iterator iter = hessian->begin(); iter != hessian->end(); ++iter) { dims[index++] = hessian->getDim(iter); } dims[index] = 1; @@ -227,8 +227,8 @@ double LinearizedHessianFactor::error(const Values& c) const { } /* ************************************************************************* */ -boost::shared_ptr -LinearizedHessianFactor::linearize(const Values& c, const OrderingOrdered& ordering) const { +boost::shared_ptr +LinearizedHessianFactor::linearize(const Values& c, const Ordering& ordering) const { // Use the ordering to convert the keys into indices; std::vector js; @@ -274,8 +274,8 @@ LinearizedHessianFactor::linearize(const Values& c, const OrderingOrdered& order } // Create a Hessian Factor from the modified info matrix - //return boost::shared_ptr(new HessianFactorOrdered(js, newInfo)); - return boost::shared_ptr(new HessianFactorOrdered(js, Gs, gs, f)); + //return boost::shared_ptr(new HessianFactor(js, newInfo)); + return boost::shared_ptr(new HessianFactor(js, Gs, gs, f)); } } // \namespace aspn diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 0d11eb7e0..330df0c6a 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -54,7 +54,7 @@ public: * @param ordering: The full ordering used to linearize this factor * @param lin_points: The linearization points for, at least, the variables used by this factor */ - LinearizedGaussianFactor(const GaussianFactorOrdered::shared_ptr& gaussian, const OrderingOrdered& ordering, const Values& lin_points); + LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Ordering& ordering, const Values& lin_points); virtual ~LinearizedGaussianFactor() {}; @@ -114,8 +114,8 @@ public: * @param ordering: The ordering used to linearize this factor * @param lin_points: The linearization points for, at least, the variables used by this factor */ - LinearizedJacobianFactor(const JacobianFactorOrdered::shared_ptr& jacobian, - const OrderingOrdered& ordering, const Values& lin_points); + LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian, + const Ordering& ordering, const Values& lin_points); virtual ~LinearizedJacobianFactor() {} @@ -148,8 +148,8 @@ public: * Reimplemented from NoiseModelFactor to directly copy out the * matrices and only update the RHS b with an updated residual */ - boost::shared_ptr linearize( - const Values& c, const OrderingOrdered& ordering) const; + boost::shared_ptr linearize( + const Values& c, const Ordering& ordering) const; /** (A*x-b) */ Vector error_vector(const Values& c) const; @@ -207,8 +207,8 @@ public: * @param ordering: The ordering used to linearize this factor * @param lin_points: The linearization points for, at least, the variables used by this factor */ - LinearizedHessianFactor(const HessianFactorOrdered::shared_ptr& hessian, - const OrderingOrdered& ordering, const Values& lin_points); + LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian, + const Ordering& ordering, const Values& lin_points); virtual ~LinearizedHessianFactor() {} @@ -270,8 +270,8 @@ public: * Reimplemented from NoiseModelFactor to directly copy out the * matrices and only update the RHS b with an updated residual */ - boost::shared_ptr linearize( - const Values& c, const OrderingOrdered& ordering) const; + boost::shared_ptr linearize( + const Values& c, const Ordering& ordering) const; private: /** Serialization function */ diff --git a/gtsam_unstable/nonlinear/summarization.cpp b/gtsam_unstable/nonlinear/summarization.cpp index d5630488e..d691574b0 100644 --- a/gtsam_unstable/nonlinear/summarization.cpp +++ b/gtsam_unstable/nonlinear/summarization.cpp @@ -15,15 +15,15 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -GaussianFactorGraphOrdered::shared_ptr summarizeGraphSequential( - const GaussianFactorGraphOrdered& full_graph, const std::vector& indices, bool useQR) { +GaussianFactorGraph::shared_ptr summarizeGraphSequential( + const GaussianFactorGraph& full_graph, const std::vector& indices, bool useQR) { GaussianSequentialSolver solver(full_graph, useQR); return solver.jointFactorGraph(indices); } /* ************************************************************************* */ -GaussianFactorGraphOrdered::shared_ptr summarizeGraphSequential( - const GaussianFactorGraphOrdered& full_graph, const OrderingOrdered& ordering, const KeySet& saved_keys, bool useQR) { +GaussianFactorGraph::shared_ptr summarizeGraphSequential( + const GaussianFactorGraph& full_graph, const Ordering& ordering, const KeySet& saved_keys, bool useQR) { std::vector indices; BOOST_FOREACH(const Key& k, saved_keys) indices.push_back(ordering[k]); diff --git a/gtsam_unstable/nonlinear/summarization.h b/gtsam_unstable/nonlinear/summarization.h index cb46e05f2..4d783ebe1 100644 --- a/gtsam_unstable/nonlinear/summarization.h +++ b/gtsam_unstable/nonlinear/summarization.h @@ -21,12 +21,12 @@ namespace gtsam { * Summarization function that eliminates a set of variables (does not convert to Jacobians) * NOTE: uses sequential solver - requires fully constrained system */ -GaussianFactorGraphOrdered::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential( - const GaussianFactorGraphOrdered& full_graph, const std::vector& indices, bool useQR = false); +GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential( + const GaussianFactorGraph& full_graph, const std::vector& indices, bool useQR = false); /** Summarization that also converts keys to indices */ -GaussianFactorGraphOrdered::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential( - const GaussianFactorGraphOrdered& full_graph, const OrderingOrdered& ordering, +GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential( + const GaussianFactorGraph& full_graph, const Ordering& ordering, const KeySet& saved_keys, bool useQR = false); } // \namespace gtsam diff --git a/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp index a1c70c4e5..66ed5f5c1 100644 --- a/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp @@ -33,7 +33,7 @@ TEST( LinearizedFactor, equals_jacobian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - OrderingOrdered ordering; + Ordering ordering; ordering.push_back(x1); ordering.push_back(x2); Values values; @@ -46,7 +46,7 @@ TEST( LinearizedFactor, equals_jacobian ) // Create two identical factors and make sure they're equal - JacobianFactorOrdered::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); LinearizedJacobianFactor jacobian1(jf, ordering, values); LinearizedJacobianFactor jacobian2(jf, ordering, values); @@ -59,7 +59,7 @@ TEST( LinearizedFactor, clone_jacobian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - OrderingOrdered ordering; + Ordering ordering; ordering.push_back(x1); ordering.push_back(x2); Values values; @@ -71,13 +71,13 @@ TEST( LinearizedFactor, clone_jacobian ) BetweenFactor betweenFactor(x1, x2, measured, model); // Create one factor that is a clone of the other and make sure they're equal - JacobianFactorOrdered::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); LinearizedJacobianFactor jacobian1(jf, ordering, values); LinearizedJacobianFactor::shared_ptr jacobian2 = boost::static_pointer_cast(jacobian1.clone()); CHECK(assert_equal(jacobian1, *jacobian2)); - JacobianFactorOrdered::shared_ptr jf1 = boost::static_pointer_cast(jacobian1.linearize(values, ordering)); - JacobianFactorOrdered::shared_ptr jf2 = boost::static_pointer_cast(jacobian2->linearize(values, ordering)); + JacobianFactor::shared_ptr jf1 = boost::static_pointer_cast(jacobian1.linearize(values, ordering)); + JacobianFactor::shared_ptr jf2 = boost::static_pointer_cast(jacobian2->linearize(values, ordering)); CHECK(assert_equal(*jf1, *jf2)); Matrix information1 = jf1->augmentedInformation(); @@ -91,7 +91,7 @@ TEST( LinearizedFactor, add_jacobian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - OrderingOrdered ordering; + Ordering ordering; ordering.push_back(x1); ordering.push_back(x2); Values values; @@ -103,7 +103,7 @@ TEST( LinearizedFactor, add_jacobian ) BetweenFactor betweenFactor(x1, x2, measured, model); // Create two factor graphs, one using 'push_back' and one using 'add' and make sure they're equal - JacobianFactorOrdered::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); LinearizedJacobianFactor::shared_ptr jacobian(new LinearizedJacobianFactor(jf, ordering, values)); NonlinearFactorGraph graph1; graph1.push_back(jacobian); NonlinearFactorGraph graph2; graph2.add(*jacobian); @@ -120,7 +120,7 @@ TEST( LinearizedFactor, add_jacobian ) // // Create a Between Factor from a Point3. This is actually a linear factor. // Key key1(1); // Key key2(2); -// OrderingOrdered ordering; +// Ordering ordering; // ordering.push_back(key1); // ordering.push_back(key2); // Values values; @@ -175,7 +175,7 @@ TEST( LinearizedFactor, equals_hessian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - OrderingOrdered ordering; + Ordering ordering; ordering.push_back(x1); ordering.push_back(x2); Values values; @@ -188,8 +188,8 @@ TEST( LinearizedFactor, equals_hessian ) // Create two identical factors and make sure they're equal - JacobianFactorOrdered::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); - HessianFactorOrdered::shared_ptr hf(new HessianFactorOrdered(*jf)); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + HessianFactor::shared_ptr hf(new HessianFactor(*jf)); LinearizedHessianFactor hessian1(hf, ordering, values); LinearizedHessianFactor hessian2(hf, ordering, values); @@ -202,7 +202,7 @@ TEST( LinearizedFactor, clone_hessian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - OrderingOrdered ordering; + Ordering ordering; ordering.push_back(x1); ordering.push_back(x2); Values values; @@ -215,8 +215,8 @@ TEST( LinearizedFactor, clone_hessian ) // Create two identical factors and make sure they're equal - JacobianFactorOrdered::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); - HessianFactorOrdered::shared_ptr hf(new HessianFactorOrdered(*jf)); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + HessianFactor::shared_ptr hf(new HessianFactor(*jf)); LinearizedHessianFactor hessian1(hf, ordering, values); LinearizedHessianFactor::shared_ptr hessian2 = boost::static_pointer_cast(hessian1.clone()); @@ -229,7 +229,7 @@ TEST( LinearizedFactor, add_hessian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - OrderingOrdered ordering; + Ordering ordering; ordering.push_back(x1); ordering.push_back(x2); Values values; @@ -242,8 +242,8 @@ TEST( LinearizedFactor, add_hessian ) // Create two identical factors and make sure they're equal - JacobianFactorOrdered::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); - HessianFactorOrdered::shared_ptr hf(new HessianFactorOrdered(*jf)); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + HessianFactor::shared_ptr hf(new HessianFactor(*jf)); LinearizedHessianFactor::shared_ptr hessian(new LinearizedHessianFactor(hf, ordering, values)); NonlinearFactorGraph graph1; graph1.push_back(hessian); NonlinearFactorGraph graph2; graph2.add(*hessian); @@ -257,7 +257,7 @@ TEST( LinearizedFactor, add_hessian ) // // Create a Between Factor from a Point3. This is actually a linear factor. // Key key1(1); // Key key2(2); -// OrderingOrdered ordering; +// Ordering ordering; // ordering.push_back(key1); // ordering.push_back(key2); // Values values; @@ -271,7 +271,7 @@ TEST( LinearizedFactor, add_hessian ) // // // Create a linearized hessian factor // JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); -// HessianFactorOrdered::shared_ptr hf(new HessianFactorOrdered(*jf)); +// HessianFactor::shared_ptr hf(new HessianFactor(*jf)); // LinearizedHessianFactor hessian(hf, ordering, values); // // @@ -294,7 +294,7 @@ TEST( LinearizedFactor, add_hessian ) // EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); // // // Check that the linearized factors are identical -// GaussianFactor::shared_ptr expected_factor = HessianFactorOrdered::shared_ptr(new HessianFactorOrdered(*betweenFactor.linearize(linpoint, ordering))); +// GaussianFactor::shared_ptr expected_factor = HessianFactor::shared_ptr(new HessianFactor(*betweenFactor.linearize(linpoint, ordering))); // GaussianFactor::shared_ptr actual_factor = hessian.linearize(linpoint, ordering); // CHECK(assert_equal(*expected_factor, *actual_factor)); // } diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp index 1f76fcb73..6f4b3b109 100644 --- a/gtsam_unstable/slam/DummyFactor.cpp +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -37,11 +37,11 @@ bool DummyFactor::equals(const NonlinearFactor& f, double tol) const { } /* ************************************************************************* */ -boost::shared_ptr -DummyFactor::linearize(const Values& c, const OrderingOrdered& ordering) const { +boost::shared_ptr +DummyFactor::linearize(const Values& c, const Ordering& ordering) const { // Only linearize if the factor is active if (!this->active(c)) - return boost::shared_ptr(); + return boost::shared_ptr(); // Fill in terms with zero matrices std::vector > terms(this->size()); @@ -51,8 +51,8 @@ DummyFactor::linearize(const Values& c, const OrderingOrdered& ordering) const { } noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(rowDim_); - return GaussianFactorOrdered::shared_ptr( - new JacobianFactorOrdered(terms, zero(rowDim_), model)); + return GaussianFactor::shared_ptr( + new JacobianFactor(terms, zero(rowDim_), model)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/DummyFactor.h b/gtsam_unstable/slam/DummyFactor.h index bed3819ba..d2ea2567f 100644 --- a/gtsam_unstable/slam/DummyFactor.h +++ b/gtsam_unstable/slam/DummyFactor.h @@ -54,8 +54,8 @@ public: virtual size_t dim() const { return rowDim_; } /** linearize to a GaussianFactor */ - virtual boost::shared_ptr - linearize(const Values& c, const OrderingOrdered& ordering) const; + virtual boost::shared_ptr + linearize(const Values& c, const Ordering& ordering) const; /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow diff --git a/gtsam_unstable/slam/tests/testDummyFactor.cpp b/gtsam_unstable/slam/tests/testDummyFactor.cpp index 407d5c469..c6a1c8bd3 100644 --- a/gtsam_unstable/slam/tests/testDummyFactor.cpp +++ b/gtsam_unstable/slam/tests/testDummyFactor.cpp @@ -41,14 +41,14 @@ TEST( testDummyFactor, basics ) { // errors - all zeros DOUBLES_EQUAL(0.0, dummyfactor.error(c), tol); - OrderingOrdered ordering; + Ordering ordering; ordering += key1, key2; // linearization: all zeros - GaussianFactorOrdered::shared_ptr actLinearization = dummyfactor.linearize(c, ordering); + GaussianFactor::shared_ptr actLinearization = dummyfactor.linearize(c, ordering); CHECK(actLinearization); noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3); - GaussianFactorOrdered::shared_ptr expLinearization(new JacobianFactorOrdered( + GaussianFactor::shared_ptr expLinearization(new JacobianFactor( ordering[key1], zeros(3,3), ordering[key2], zeros(3,3), zero(3), model3)); EXPECT(assert_equal(*expLinearization, *actLinearization, tol)); } diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 06d343e6e..516b7e070 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -116,10 +116,10 @@ TEST( testBoundingConstraint, unary_linearization_inactive) { Point2 pt1(2.0, 3.0); Values config1; config1.insert(key, pt1); - OrderingOrdered ordering; + Ordering ordering; ordering += key; - GaussianFactorOrdered::shared_ptr actual1 = constraint1.linearize(config1, ordering); - GaussianFactorOrdered::shared_ptr actual2 = constraint2.linearize(config1, ordering); + GaussianFactor::shared_ptr actual1 = constraint1.linearize(config1, ordering); + GaussianFactor::shared_ptr actual2 = constraint2.linearize(config1, ordering); EXPECT(!actual1); EXPECT(!actual2); } @@ -129,14 +129,14 @@ TEST( testBoundingConstraint, unary_linearization_active) { Point2 pt2(-2.0, -3.0); Values config2; config2.insert(key, pt2); - OrderingOrdered ordering; + Ordering ordering; ordering += key; - GaussianFactorOrdered::shared_ptr actual1 = constraint1.linearize(config2, ordering); - GaussianFactorOrdered::shared_ptr actual2 = constraint2.linearize(config2, ordering); - JacobianFactorOrdered expected1(ordering[key], Matrix_(1, 2, 1.0, 0.0), repeat(1, 3.0), hard_model1); - JacobianFactorOrdered expected2(ordering[key], Matrix_(1, 2, 0.0, 1.0), repeat(1, 5.0), hard_model1); - EXPECT(assert_equal((const GaussianFactorOrdered&)expected1, *actual1, tol)); - EXPECT(assert_equal((const GaussianFactorOrdered&)expected2, *actual2, tol)); + GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2, ordering); + GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2, ordering); + JacobianFactor expected1(ordering[key], Matrix_(1, 2, 1.0, 0.0), repeat(1, 3.0), hard_model1); + JacobianFactor expected2(ordering[key], Matrix_(1, 2, 0.0, 1.0), repeat(1, 5.0), hard_model1); + EXPECT(assert_equal((const GaussianFactor&)expected1, *actual1, tol)); + EXPECT(assert_equal((const GaussianFactor&)expected2, *actual2, tol)); } /* ************************************************************************* */ @@ -199,7 +199,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { Values config1; config1.insert(key1, pt1); config1.insert(key2, pt1); - OrderingOrdered ordering; ordering += key1, key2; + Ordering ordering; ordering += key1, key2; EXPECT(!rangeBound.active(config1)); EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); EXPECT(!rangeBound.linearize(config1, ordering)); diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 6244590d7..008ea443a 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -46,28 +46,28 @@ using symbol_shorthand::X; using symbol_shorthand::L; /* ************************************************************************* */ -double computeError(const GaussianBayesNetOrdered& gbn, const LieVector& values) { +double computeError(const GaussianBayesNet& gbn, const LieVector& values) { // Convert Vector to VectorValues - VectorValuesOrdered vv = *allocateVectorValues(gbn); + VectorValues vv = *allocateVectorValues(gbn); internal::writeVectorValuesSlices(values, vv, boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size())); // Convert to factor graph - GaussianFactorGraphOrdered gfg(gbn); + GaussianFactorGraph gfg(gbn); return gfg.error(vv); } /* ************************************************************************* */ -double computeErrorBt(const BayesTreeOrdered& gbt, const LieVector& values) { +double computeErrorBt(const BayesTree& gbt, const LieVector& values) { // Convert Vector to VectorValues - VectorValuesOrdered vv = *allocateVectorValues(gbt); + VectorValues vv = *allocateVectorValues(gbt); internal::writeVectorValuesSlices(values, vv, boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size())); // Convert to factor graph - GaussianFactorGraphOrdered gfg(gbt); + GaussianFactorGraph gfg(gbt); return gfg.error(vv); } @@ -75,58 +75,58 @@ double computeErrorBt(const BayesTreeOrdered& gbt, c TEST(DoglegOptimizer, ComputeSteepestDescentPoint) { // Create an arbitrary Bayes Net - GaussianBayesNetOrdered gbn; - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + GaussianBayesNet gbn; + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0), 3, Matrix_(2,2, 7.0,8.0,9.0,10.0), 4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0), 2, Matrix_(2,2, 21.0,22.0,23.0,24.0), 4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0), 3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0), 4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2))); // Compute the Hessian numerically Matrix hessian = numericalHessian( boost::function(boost::bind(&computeError, gbn, _1)), - LieVector(VectorValuesOrdered::Zero(*allocateVectorValues(gbn)).asVector())); + LieVector(VectorValues::Zero(*allocateVectorValues(gbn)).asVector())); // Compute the gradient numerically - VectorValuesOrdered gradientValues = *allocateVectorValues(gbn); + VectorValues gradientValues = *allocateVectorValues(gbn); Vector gradient = numericalGradient( boost::function(boost::bind(&computeError, gbn, _1)), - LieVector(VectorValuesOrdered::Zero(gradientValues).asVector())); + LieVector(VectorValues::Zero(gradientValues).asVector())); internal::writeVectorValuesSlices(gradient, gradientValues, boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size())); // Compute the gradient using dense matrices - Matrix augmentedHessian = GaussianFactorGraphOrdered(gbn).augmentedHessian(); + Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); LONGS_EQUAL(11, augmentedHessian.cols()); - VectorValuesOrdered denseMatrixGradient = *allocateVectorValues(gbn); + VectorValues denseMatrixGradient = *allocateVectorValues(gbn); internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient, boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size())); EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5)); // Compute the steepest descent point double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0); - VectorValuesOrdered expected = gradientValues; scal(step, expected); + VectorValues expected = gradientValues; scal(step, expected); // Compute the steepest descent point with the dogleg function - VectorValuesOrdered actual = optimizeGradientSearch(gbn); + VectorValues actual = optimizeGradientSearch(gbn); // Check that points agree EXPECT(assert_equal(expected, actual, 1e-5)); // Check that point causes a decrease in error - double origError = GaussianFactorGraphOrdered(gbn).error(VectorValuesOrdered::Zero(*allocateVectorValues(gbn))); - double newError = GaussianFactorGraphOrdered(gbn).error(actual); + double origError = GaussianFactorGraph(gbn).error(VectorValues::Zero(*allocateVectorValues(gbn))); + double newError = GaussianFactorGraph(gbn).error(actual); EXPECT(newError < origError); } @@ -134,9 +134,9 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) { TEST(DoglegOptimizer, BT_BN_equivalency) { // Create an arbitrary Bayes Tree - BayesTreeOrdered bt; - bt.insert(BayesTreeOrdered::sharedClique(new BayesTreeOrdered::Clique( - GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + BayesTree bt; + bt.insert(BayesTree::sharedClique(new BayesTree::Clique( + GaussianConditional::shared_ptr(new GaussianConditional( boost::assign::pair_list_of (2, Matrix_(6,2, 31.0,32.0, @@ -160,8 +160,8 @@ TEST(DoglegOptimizer, BT_BN_equivalency) { 51.0,52.0, 0.0,54.0)), 3, Vector_(6, 29.0,30.0,39.0,40.0,49.0,50.0), ones(6)))))); - bt.insert(BayesTreeOrdered::sharedClique(new BayesTreeOrdered::Clique( - GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + bt.insert(BayesTree::sharedClique(new BayesTree::Clique( + GaussianConditional::shared_ptr(new GaussianConditional( boost::assign::pair_list_of (0, Matrix_(4,2, 3.0,4.0, @@ -191,26 +191,26 @@ TEST(DoglegOptimizer, BT_BN_equivalency) { 2, Vector_(4, 1.0,2.0,15.0,16.0), ones(4)))))); // Create an arbitrary Bayes Net - GaussianBayesNetOrdered gbn; - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + GaussianBayesNet gbn; + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0), 3, Matrix_(2,2, 7.0,8.0,9.0,10.0), 4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0), 2, Matrix_(2,2, 21.0,22.0,23.0,24.0), 4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0), 3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0), 4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2))); - GaussianFactorGraphOrdered expected(gbn); - GaussianFactorGraphOrdered actual(bt); + GaussianFactorGraph expected(gbn); + GaussianFactorGraph actual(bt); EXPECT(assert_equal(expected.augmentedHessian(), actual.augmentedHessian())); } @@ -219,9 +219,9 @@ TEST(DoglegOptimizer, BT_BN_equivalency) { TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) { // Create an arbitrary Bayes Tree - BayesTreeOrdered bt; - bt.insert(BayesTreeOrdered::sharedClique(new BayesTreeOrdered::Clique( - GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + BayesTree bt; + bt.insert(BayesTree::sharedClique(new BayesTree::Clique( + GaussianConditional::shared_ptr(new GaussianConditional( boost::assign::pair_list_of (2, Matrix_(6,2, 31.0,32.0, @@ -245,8 +245,8 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) { 51.0,52.0, 0.0,54.0)), 3, Vector_(6, 29.0,30.0,39.0,40.0,49.0,50.0), ones(6)))))); - bt.insert(BayesTreeOrdered::sharedClique(new BayesTreeOrdered::Clique( - GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + bt.insert(BayesTree::sharedClique(new BayesTree::Clique( + GaussianConditional::shared_ptr(new GaussianConditional( boost::assign::pair_list_of (0, Matrix_(4,2, 3.0,4.0, @@ -278,30 +278,30 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) { // Compute the Hessian numerically Matrix hessian = numericalHessian( boost::function(boost::bind(&computeErrorBt, bt, _1)), - LieVector(VectorValuesOrdered::Zero(*allocateVectorValues(bt)).asVector())); + LieVector(VectorValues::Zero(*allocateVectorValues(bt)).asVector())); // Compute the gradient numerically - VectorValuesOrdered gradientValues = *allocateVectorValues(bt); + VectorValues gradientValues = *allocateVectorValues(bt); Vector gradient = numericalGradient( boost::function(boost::bind(&computeErrorBt, bt, _1)), - LieVector(VectorValuesOrdered::Zero(gradientValues).asVector())); + LieVector(VectorValues::Zero(gradientValues).asVector())); internal::writeVectorValuesSlices(gradient, gradientValues, boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size())); // Compute the gradient using dense matrices - Matrix augmentedHessian = GaussianFactorGraphOrdered(bt).augmentedHessian(); + Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian(); LONGS_EQUAL(11, augmentedHessian.cols()); - VectorValuesOrdered denseMatrixGradient = *allocateVectorValues(bt); + VectorValues denseMatrixGradient = *allocateVectorValues(bt); internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient, boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size())); EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5)); // Compute the steepest descent point double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0); - VectorValuesOrdered expected = gradientValues; scal(step, expected); + VectorValues expected = gradientValues; scal(step, expected); // Known steepest descent point from Bayes' net version - VectorValuesOrdered expectedFromBN(5,2); + VectorValues expectedFromBN(5,2); expectedFromBN[0] = Vector_(2, 0.000129034, 0.000688183); expectedFromBN[1] = Vector_(2, 0.0109679, 0.0253767); expectedFromBN[2] = Vector_(2, 0.0680441, 0.114496); @@ -309,90 +309,90 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) { expectedFromBN[4] = Vector_(2, 0.300134, 0.423233); // Compute the steepest descent point with the dogleg function - VectorValuesOrdered actual = optimizeGradientSearch(bt); + VectorValues actual = optimizeGradientSearch(bt); // Check that points agree EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expectedFromBN, actual, 1e-5)); // Check that point causes a decrease in error - double origError = GaussianFactorGraphOrdered(bt).error(VectorValuesOrdered::Zero(*allocateVectorValues(bt))); - double newError = GaussianFactorGraphOrdered(bt).error(actual); + double origError = GaussianFactorGraph(bt).error(VectorValues::Zero(*allocateVectorValues(bt))); + double newError = GaussianFactorGraph(bt).error(actual); EXPECT(newError < origError); } /* ************************************************************************* */ TEST(DoglegOptimizer, ComputeBlend) { // Create an arbitrary Bayes Net - GaussianBayesNetOrdered gbn; - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + GaussianBayesNet gbn; + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0), 3, Matrix_(2,2, 7.0,8.0,9.0,10.0), 4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0), 2, Matrix_(2,2, 21.0,22.0,23.0,24.0), 4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0), 3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0), 4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2))); // Compute steepest descent point - VectorValuesOrdered xu = optimizeGradientSearch(gbn); + VectorValues xu = optimizeGradientSearch(gbn); // Compute Newton's method point - VectorValuesOrdered xn = optimize(gbn); + VectorValues xn = optimize(gbn); // The Newton's method point should be more "adventurous", i.e. larger, than the steepest descent point EXPECT(xu.asVector().norm() < xn.asVector().norm()); // Compute blend double Delta = 1.5; - VectorValuesOrdered xb = DoglegOptimizerImpl::ComputeBlend(Delta, xu, xn); + VectorValues xb = DoglegOptimizerImpl::ComputeBlend(Delta, xu, xn); DOUBLES_EQUAL(Delta, xb.asVector().norm(), 1e-10); } /* ************************************************************************* */ TEST(DoglegOptimizer, ComputeDoglegPoint) { // Create an arbitrary Bayes Net - GaussianBayesNetOrdered gbn; - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + GaussianBayesNet gbn; + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0), 3, Matrix_(2,2, 7.0,8.0,9.0,10.0), 4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0), 2, Matrix_(2,2, 21.0,22.0,23.0,24.0), 4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0), 3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0), 4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2))); - gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered( + gbn += GaussianConditional::shared_ptr(new GaussianConditional( 4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2))); // Compute dogleg point for different deltas double Delta1 = 0.5; // Less than steepest descent - VectorValuesOrdered actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, optimizeGradientSearch(gbn), optimize(gbn)); + VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, optimizeGradientSearch(gbn), optimize(gbn)); DOUBLES_EQUAL(Delta1, actual1.asVector().norm(), 1e-5); double Delta2 = 1.5; // Between steepest descent and Newton's method - VectorValuesOrdered expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, optimizeGradientSearch(gbn), optimize(gbn)); - VectorValuesOrdered actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, optimizeGradientSearch(gbn), optimize(gbn)); + VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, optimizeGradientSearch(gbn), optimize(gbn)); + VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, optimizeGradientSearch(gbn), optimize(gbn)); DOUBLES_EQUAL(Delta2, actual2.asVector().norm(), 1e-5); EXPECT(assert_equal(expected2, actual2)); double Delta3 = 5.0; // Larger than Newton's method point - VectorValuesOrdered expected3 = optimize(gbn); - VectorValuesOrdered actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, optimizeGradientSearch(gbn), optimize(gbn)); + VectorValues expected3 = optimize(gbn); + VectorValues actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, optimizeGradientSearch(gbn), optimize(gbn)); EXPECT(assert_equal(expected3, actual3)); } @@ -408,16 +408,16 @@ TEST(DoglegOptimizer, Iterate) { config->insert(X(1), x0); // ordering - boost::shared_ptr ord(new OrderingOrdered()); + boost::shared_ptr ord(new Ordering()); ord->push_back(X(1)); double Delta = 1.0; for(size_t it=0; it<10; ++it) { GaussianSequentialSolver solver(*fg->linearize(*config, *ord)); - GaussianBayesNetOrdered gbn = *solver.eliminate(); + GaussianBayesNet gbn = *solver.eliminate(); // Iterate assumes that linear error = nonlinear error at the linearization point, and this should be true double nonlinearError = fg->error(*config); - double linearError = GaussianFactorGraphOrdered(gbn).error(VectorValuesOrdered::Zero(*allocateVectorValues(gbn))); + double linearError = GaussianFactorGraph(gbn).error(VectorValues::Zero(*allocateVectorValues(gbn))); DOUBLES_EQUAL(nonlinearError, linearError, 1e-5); // cout << "it " << it << ", Delta = " << Delta << ", error = " << fg->error(*config) << endl; DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, gbn, *fg, *config, *ord, fg->error(*config)); diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index bb79cbdea..6bdfa7eee 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -192,7 +192,7 @@ public: * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z * Hence b = z - h(x1,x2) = - error_vector(x) */ - boost::shared_ptr linearize(const Values& c, const OrderingOrdered& ordering) const { + boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { const X1& x1 = c.at(key1()); const X2& x2 = c.at(key2()); Matrix A1, A2; @@ -201,7 +201,7 @@ public: SharedDiagonal constrained = boost::dynamic_pointer_cast(this->noiseModel_); if (constrained.get() != NULL) { - return JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(var1, A1, var2, + return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, A2, b, constrained)); } // "Whiten" the system before converting to a Gaussian Factor @@ -209,7 +209,7 @@ public: A1 = Qinvsqrt*A1; A2 = Qinvsqrt*A2; b = Qinvsqrt*b; - return GaussianFactorOrdered::shared_ptr(new JacobianFactorOrdered(var1, A1, var2, + return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, A2, b, noiseModel::Unit::Create(b.size()))); } @@ -329,7 +329,7 @@ public: * Ax-b \approx h(x1+dx1)-z = h(x1) + A1*dx1 - z * Hence b = z - h(x1) = - error_vector(x) */ - boost::shared_ptr linearize(const Values& c, const OrderingOrdered& ordering) const { + boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { const X& x1 = c.at(key()); Matrix A1; Vector b = -evaluateError(x1, A1); @@ -337,13 +337,13 @@ public: SharedDiagonal constrained = boost::dynamic_pointer_cast(this->noiseModel_); if (constrained.get() != NULL) { - return JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(var1, A1, b, constrained)); + return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, b, constrained)); } // "Whiten" the system before converting to a Gaussian Factor Matrix Rinvsqrt = RInvSqrt(x1); A1 = Rinvsqrt*A1; b = Rinvsqrt*b; - return GaussianFactorOrdered::shared_ptr(new JacobianFactorOrdered(var1, A1, b, noiseModel::Unit::Create(b.size()))); + return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, b, noiseModel::Unit::Create(b.size()))); } /** vector of errors */ diff --git a/tests/testGaussianBayesTree.cpp b/tests/testGaussianBayesTree.cpp index 2d0cde7a0..85ea792fe 100644 --- a/tests/testGaussianBayesTree.cpp +++ b/tests/testGaussianBayesTree.cpp @@ -52,45 +52,45 @@ C4 x3 : x4 C5 x2 : x3 C6 x1 : x2 **************************************************************************** */ -TEST( GaussianBayesTreeOrdered, linear_smoother_shortcuts ) +TEST( GaussianBayesTree, linear_smoother_shortcuts ) { // Create smoother with 7 nodes - OrderingOrdered ordering; - GaussianFactorGraphOrdered smoother; + Ordering ordering; + GaussianFactorGraph smoother; boost::tie(smoother, ordering) = createSmoother(7); - GaussianBayesTreeOrdered bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); + GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); // Create the Bayes tree LONGS_EQUAL(6, bayesTree.size()); // Check the conditional P(Root|Root) - GaussianBayesNetOrdered empty; - GaussianBayesTreeOrdered::sharedClique R = bayesTree.root(); - GaussianBayesNetOrdered actual1 = R->shortcut(R, EliminateCholeskyOrdered); + GaussianBayesNet empty; + GaussianBayesTree::sharedClique R = bayesTree.root(); + GaussianBayesNet actual1 = R->shortcut(R, EliminateCholesky); EXPECT(assert_equal(empty,actual1,tol)); // Check the conditional P(C2|Root) - GaussianBayesTreeOrdered::sharedClique C2 = bayesTree[ordering[X(5)]]; - GaussianBayesNetOrdered actual2 = C2->shortcut(R, EliminateCholeskyOrdered); + GaussianBayesTree::sharedClique C2 = bayesTree[ordering[X(5)]]; + GaussianBayesNet actual2 = C2->shortcut(R, EliminateCholesky); EXPECT(assert_equal(empty,actual2,tol)); // Check the conditional P(C3|Root) double sigma3 = 0.61808; Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022); - GaussianBayesNetOrdered expected3; + GaussianBayesNet expected3; push_front(expected3,ordering[X(5)], zero(2), eye(2)/sigma3, ordering[X(6)], A56/sigma3, ones(2)); - GaussianBayesTreeOrdered::sharedClique C3 = bayesTree[ordering[X(4)]]; - GaussianBayesNetOrdered actual3 = C3->shortcut(R, EliminateCholeskyOrdered); + GaussianBayesTree::sharedClique C3 = bayesTree[ordering[X(4)]]; + GaussianBayesNet actual3 = C3->shortcut(R, EliminateCholesky); EXPECT(assert_equal(expected3,actual3,tol)); // Check the conditional P(C4|Root) double sigma4 = 0.661968; Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067); - GaussianBayesNetOrdered expected4; + GaussianBayesNet expected4; push_front(expected4, ordering[X(4)], zero(2), eye(2)/sigma4, ordering[X(6)], A46/sigma4, ones(2)); - GaussianBayesTreeOrdered::sharedClique C4 = bayesTree[ordering[X(3)]]; - GaussianBayesNetOrdered actual4 = C4->shortcut(R, EliminateCholeskyOrdered); + GaussianBayesTree::sharedClique C4 = bayesTree[ordering[X(3)]]; + GaussianBayesNet actual4 = C4->shortcut(R, EliminateCholesky); EXPECT(assert_equal(expected4,actual4,tol)); } @@ -113,18 +113,18 @@ TEST( GaussianBayesTreeOrdered, linear_smoother_shortcuts ) C4 x7 : x6 ************************************************************************* */ -TEST( GaussianBayesTreeOrdered, balanced_smoother_marginals ) +TEST( GaussianBayesTree, balanced_smoother_marginals ) { // Create smoother with 7 nodes - OrderingOrdered ordering; + Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); - GaussianFactorGraphOrdered smoother = createSmoother(7, ordering).first; + GaussianFactorGraph smoother = createSmoother(7, ordering).first; // Create the Bayes tree - GaussianBayesTreeOrdered bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); + GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); - VectorValuesOrdered expectedSolution(VectorValuesOrdered::Zero(7,2)); - VectorValuesOrdered actualSolution = optimize(bayesTree); + VectorValues expectedSolution(VectorValues::Zero(7,2)); + VectorValues actualSolution = optimize(bayesTree); EXPECT(assert_equal(expectedSolution,actualSolution,tol)); LONGS_EQUAL(4,bayesTree.size()); @@ -132,73 +132,73 @@ TEST( GaussianBayesTreeOrdered, balanced_smoother_marginals ) double tol=1e-5; // Check marginal on x1 - GaussianBayesNetOrdered expected1 = simpleGaussian(ordering[X(1)], zero(2), sigmax1); - GaussianBayesNetOrdered actual1 = *bayesTree.marginalBayesNet(ordering[X(1)], EliminateCholeskyOrdered); + GaussianBayesNet expected1 = simpleGaussian(ordering[X(1)], zero(2), sigmax1); + GaussianBayesNet actual1 = *bayesTree.marginalBayesNet(ordering[X(1)], EliminateCholesky); Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1); Matrix actualCovarianceX1; - GaussianFactorOrdered::shared_ptr m = bayesTree.marginalFactor(ordering[X(1)], EliminateCholeskyOrdered); - actualCovarianceX1 = bayesTree.marginalFactor(ordering[X(1)], EliminateCholeskyOrdered)->information().inverse(); + GaussianFactor::shared_ptr m = bayesTree.marginalFactor(ordering[X(1)], EliminateCholesky); + actualCovarianceX1 = bayesTree.marginalFactor(ordering[X(1)], EliminateCholesky)->information().inverse(); EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol)); EXPECT(assert_equal(expected1,actual1,tol)); // Check marginal on x2 double sigx2 = 0.68712938; // FIXME: this should be corrected analytically - GaussianBayesNetOrdered expected2 = simpleGaussian(ordering[X(2)], zero(2), sigx2); - GaussianBayesNetOrdered actual2 = *bayesTree.marginalBayesNet(ordering[X(2)], EliminateCholeskyOrdered); + GaussianBayesNet expected2 = simpleGaussian(ordering[X(2)], zero(2), sigx2); + GaussianBayesNet actual2 = *bayesTree.marginalBayesNet(ordering[X(2)], EliminateCholesky); Matrix expectedCovarianceX2 = eye(2,2) * (sigx2 * sigx2); Matrix actualCovarianceX2; - actualCovarianceX2 = bayesTree.marginalFactor(ordering[X(2)], EliminateCholeskyOrdered)->information().inverse(); + actualCovarianceX2 = bayesTree.marginalFactor(ordering[X(2)], EliminateCholesky)->information().inverse(); EXPECT(assert_equal(expectedCovarianceX2, actualCovarianceX2, tol)); EXPECT(assert_equal(expected2,actual2,tol)); // Check marginal on x3 - GaussianBayesNetOrdered expected3 = simpleGaussian(ordering[X(3)], zero(2), sigmax3); - GaussianBayesNetOrdered actual3 = *bayesTree.marginalBayesNet(ordering[X(3)], EliminateCholeskyOrdered); + GaussianBayesNet expected3 = simpleGaussian(ordering[X(3)], zero(2), sigmax3); + GaussianBayesNet actual3 = *bayesTree.marginalBayesNet(ordering[X(3)], EliminateCholesky); Matrix expectedCovarianceX3 = eye(2,2) * (sigmax3 * sigmax3); Matrix actualCovarianceX3; - actualCovarianceX3 = bayesTree.marginalFactor(ordering[X(3)], EliminateCholeskyOrdered)->information().inverse(); + actualCovarianceX3 = bayesTree.marginalFactor(ordering[X(3)], EliminateCholesky)->information().inverse(); EXPECT(assert_equal(expectedCovarianceX3, actualCovarianceX3, tol)); EXPECT(assert_equal(expected3,actual3,tol)); // Check marginal on x4 - GaussianBayesNetOrdered expected4 = simpleGaussian(ordering[X(4)], zero(2), sigmax4); - GaussianBayesNetOrdered actual4 = *bayesTree.marginalBayesNet(ordering[X(4)], EliminateCholeskyOrdered); + GaussianBayesNet expected4 = simpleGaussian(ordering[X(4)], zero(2), sigmax4); + GaussianBayesNet actual4 = *bayesTree.marginalBayesNet(ordering[X(4)], EliminateCholesky); Matrix expectedCovarianceX4 = eye(2,2) * (sigmax4 * sigmax4); Matrix actualCovarianceX4; - actualCovarianceX4 = bayesTree.marginalFactor(ordering[X(4)], EliminateCholeskyOrdered)->information().inverse(); + actualCovarianceX4 = bayesTree.marginalFactor(ordering[X(4)], EliminateCholesky)->information().inverse(); EXPECT(assert_equal(expectedCovarianceX4, actualCovarianceX4, tol)); EXPECT(assert_equal(expected4,actual4,tol)); // Check marginal on x7 (should be equal to x1) - GaussianBayesNetOrdered expected7 = simpleGaussian(ordering[X(7)], zero(2), sigmax7); - GaussianBayesNetOrdered actual7 = *bayesTree.marginalBayesNet(ordering[X(7)], EliminateCholeskyOrdered); + GaussianBayesNet expected7 = simpleGaussian(ordering[X(7)], zero(2), sigmax7); + GaussianBayesNet actual7 = *bayesTree.marginalBayesNet(ordering[X(7)], EliminateCholesky); Matrix expectedCovarianceX7 = eye(2,2) * (sigmax7 * sigmax7); Matrix actualCovarianceX7; - actualCovarianceX7 = bayesTree.marginalFactor(ordering[X(7)], EliminateCholeskyOrdered)->information().inverse(); + actualCovarianceX7 = bayesTree.marginalFactor(ordering[X(7)], EliminateCholesky)->information().inverse(); EXPECT(assert_equal(expectedCovarianceX7, actualCovarianceX7, tol)); EXPECT(assert_equal(expected7,actual7,tol)); } /* ************************************************************************* */ -TEST( GaussianBayesTreeOrdered, balanced_smoother_shortcuts ) +TEST( GaussianBayesTree, balanced_smoother_shortcuts ) { // Create smoother with 7 nodes - OrderingOrdered ordering; + Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); - GaussianFactorGraphOrdered smoother = createSmoother(7, ordering).first; + GaussianFactorGraph smoother = createSmoother(7, ordering).first; // Create the Bayes tree - GaussianBayesTreeOrdered bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); + GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); // Check the conditional P(Root|Root) - GaussianBayesNetOrdered empty; - GaussianBayesTreeOrdered::sharedClique R = bayesTree.root(); - GaussianBayesNetOrdered actual1 = R->shortcut(R, EliminateCholeskyOrdered); + GaussianBayesNet empty; + GaussianBayesTree::sharedClique R = bayesTree.root(); + GaussianBayesNet actual1 = R->shortcut(R, EliminateCholesky); EXPECT(assert_equal(empty,actual1,tol)); // Check the conditional P(C2|Root) - GaussianBayesTreeOrdered::sharedClique C2 = bayesTree[ordering[X(3)]]; - GaussianBayesNetOrdered actual2 = C2->shortcut(R, EliminateCholeskyOrdered); + GaussianBayesTree::sharedClique C2 = bayesTree[ordering[X(3)]]; + GaussianBayesNet actual2 = C2->shortcut(R, EliminateCholesky); EXPECT(assert_equal(empty,actual2,tol)); // Check the conditional P(C3|Root), which should be equal to P(x2|x4) @@ -206,11 +206,11 @@ TEST( GaussianBayesTreeOrdered, balanced_smoother_shortcuts ) * p_x2_x4 is now an element conditional of the multifrontal conditional bayesTree[ordering[X(2)]]->conditional() * We don't know yet how to take it out. */ -// GaussianConditionalOrdered::shared_ptr p_x2_x4 = bayesTree[ordering[X(2)]]->conditional(); +// GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering[X(2)]]->conditional(); // p_x2_x4->print("Conditional p_x2_x4: "); -// GaussianBayesNetOrdered expected3(p_x2_x4); +// GaussianBayesNet expected3(p_x2_x4); // GaussianISAM::sharedClique C3 = isamTree[ordering[X(1)]]; -// GaussianBayesNetOrdered actual3 = GaussianISAM::shortcut(C3,R); +// GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R); // EXPECT(assert_equal(expected3,actual3,tol)); } @@ -218,96 +218,96 @@ TEST( GaussianBayesTreeOrdered, balanced_smoother_shortcuts ) //TEST( BayesTree, balanced_smoother_clique_marginals ) //{ // // Create smoother with 7 nodes -// OrderingOrdered ordering; +// Ordering ordering; // ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); -// GaussianFactorGraphOrdered smoother = createSmoother(7, ordering).first; +// GaussianFactorGraph smoother = createSmoother(7, ordering).first; // // // Create the Bayes tree -// GaussianBayesNetOrdered chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate(); +// GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate(); // GaussianISAM bayesTree(chordalBayesNet); // // // Check the clique marginal P(C3) // double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED! -// GaussianBayesNetOrdered expected = simpleGaussian(ordering[X(2)],zero(2),sigmax2_alt); +// GaussianBayesNet expected = simpleGaussian(ordering[X(2)],zero(2),sigmax2_alt); // push_front(expected,ordering[X(1)], zero(2), eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2)); // GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[X(1)]]; -// GaussianFactorGraphOrdered marginal = C3->marginal(R); +// GaussianFactorGraph marginal = C3->marginal(R); // GaussianVariableIndex varIndex(marginal); // Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size())); // Permutation toFrontInverse(*toFront.inverse()); // varIndex.permute(toFront); -// BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, marginal) { +// BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) { // factor->permuteWithInverse(toFrontInverse); } -// GaussianBayesNetOrdered actual = *inference::EliminateUntil(marginal, C3->keys().size(), varIndex); +// GaussianBayesNet actual = *inference::EliminateUntil(marginal, C3->keys().size(), varIndex); // actual.permuteWithInverse(toFront); // EXPECT(assert_equal(expected,actual,tol)); //} /* ************************************************************************* */ -TEST( GaussianBayesTreeOrdered, balanced_smoother_joint ) +TEST( GaussianBayesTree, balanced_smoother_joint ) { // Create smoother with 7 nodes - OrderingOrdered ordering; + Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); - GaussianFactorGraphOrdered smoother = createSmoother(7, ordering).first; + GaussianFactorGraph smoother = createSmoother(7, ordering).first; // Create the Bayes tree, expected to look like: // x5 x6 x4 // x3 x2 : x4 // x1 : x2 // x7 : x6 - GaussianBayesTreeOrdered bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); + GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); // Conditional density elements reused by both tests const Vector sigma = ones(2); const Matrix I = eye(2), A = -0.00429185*I; // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7) - GaussianBayesNetOrdered expected1; + GaussianBayesNet expected1; // Why does the sign get flipped on the prior? - GaussianConditionalOrdered::shared_ptr - parent1(new GaussianConditionalOrdered(ordering[X(7)], zero(2), -1*I/sigmax7, ones(2))); + GaussianConditional::shared_ptr + parent1(new GaussianConditional(ordering[X(7)], zero(2), -1*I/sigmax7, ones(2))); expected1.push_front(parent1); push_front(expected1,ordering[X(1)], zero(2), I/sigmax7, ordering[X(7)], A/sigmax7, sigma); - GaussianBayesNetOrdered actual1 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(7)], EliminateCholeskyOrdered); + GaussianBayesNet actual1 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(7)], EliminateCholesky); EXPECT(assert_equal(expected1,actual1,tol)); // // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1) - // GaussianBayesNetOrdered expected2; - // GaussianConditionalOrdered::shared_ptr - // parent2(new GaussianConditionalOrdered(ordering[X(1)], zero(2), -1*I/sigmax1, ones(2))); + // GaussianBayesNet expected2; + // GaussianConditional::shared_ptr + // parent2(new GaussianConditional(ordering[X(1)], zero(2), -1*I/sigmax1, ones(2))); // expected2.push_front(parent2); // push_front(expected2,ordering[X(7)], zero(2), I/sigmax1, ordering[X(1)], A/sigmax1, sigma); - // GaussianBayesNetOrdered actual2 = *bayesTree.jointBayesNet(ordering[X(7)],ordering[X(1)]); + // GaussianBayesNet actual2 = *bayesTree.jointBayesNet(ordering[X(7)],ordering[X(1)]); // EXPECT(assert_equal(expected2,actual2,tol)); // Check the joint density P(x1,x4), i.e. with a root variable - GaussianBayesNetOrdered expected3; - GaussianConditionalOrdered::shared_ptr - parent3(new GaussianConditionalOrdered(ordering[X(4)], zero(2), I/sigmax4, ones(2))); + GaussianBayesNet expected3; + GaussianConditional::shared_ptr + parent3(new GaussianConditional(ordering[X(4)], zero(2), I/sigmax4, ones(2))); expected3.push_front(parent3); double sig14 = 0.784465; Matrix A14 = -0.0769231*I; push_front(expected3,ordering[X(1)], zero(2), I/sig14, ordering[X(4)], A14/sig14, sigma); - GaussianBayesNetOrdered actual3 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(4)], EliminateCholeskyOrdered); + GaussianBayesNet actual3 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(4)], EliminateCholesky); EXPECT(assert_equal(expected3,actual3,tol)); // // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way - // GaussianBayesNetOrdered expected4; - // GaussianConditionalOrdered::shared_ptr - // parent4(new GaussianConditionalOrdered(ordering[X(1)], zero(2), -1.0*I/sigmax1, ones(2))); + // GaussianBayesNet expected4; + // GaussianConditional::shared_ptr + // parent4(new GaussianConditional(ordering[X(1)], zero(2), -1.0*I/sigmax1, ones(2))); // expected4.push_front(parent4); // double sig41 = 0.668096; // Matrix A41 = -0.055794*I; // push_front(expected4,ordering[X(4)], zero(2), I/sig41, ordering[X(1)], A41/sig41, sigma); - // GaussianBayesNetOrdered actual4 = *bayesTree.jointBayesNet(ordering[X(4)],ordering[X(1)]); + // GaussianBayesNet actual4 = *bayesTree.jointBayesNet(ordering[X(4)],ordering[X(1)]); // EXPECT(assert_equal(expected4,actual4,tol)); } /* ************************************************************************* */ -TEST(GaussianBayesTreeOrdered, simpleMarginal) +TEST(GaussianBayesTree, simpleMarginal) { - GaussianFactorGraphOrdered gfg; + GaussianFactorGraph gfg; Matrix A12 = Rot2::fromDegrees(45.0).matrix(); @@ -322,7 +322,7 @@ TEST(GaussianBayesTreeOrdered, simpleMarginal) } /* ************************************************************************* */ -TEST(GaussianBayesTreeOrdered, shortcut_overlapping_separator) +TEST(GaussianBayesTree, shortcut_overlapping_separator) { // Test computing shortcuts when the separator overlaps. This previously // would have highlighted a problem where information was duplicated. @@ -332,7 +332,7 @@ TEST(GaussianBayesTreeOrdered, shortcut_overlapping_separator) // f(3,4,5) // f(5,6) // f(6,7) - GaussianFactorGraphOrdered fg; + GaussianFactorGraph fg; noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); fg.add(1, Matrix_(1,1, 1.0), 3, Matrix_(1,1, 2.0), 5, Matrix_(1,1, 3.0), Vector_(1, 4.0), model); fg.add(1, Matrix_(1,1, 5.0), Vector_(1, 6.0), model); @@ -347,9 +347,9 @@ TEST(GaussianBayesTreeOrdered, shortcut_overlapping_separator) // c(5|6) // c(1,2|5) // c(3,4|5) - GaussianBayesTreeOrdered bt = *GaussianMultifrontalSolver(fg).eliminate(); + GaussianBayesTree bt = *GaussianMultifrontalSolver(fg).eliminate(); - GaussianFactorGraphOrdered joint = *bt.joint(1,2, EliminateQROrdered); + GaussianFactorGraph joint = *bt.joint(1,2, EliminateQR); Matrix expectedJointJ = (Matrix(2,3) << 0, 11, 12, diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 9275d9dea..b8bea8c6e 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -47,19 +47,19 @@ using symbol_shorthand::X; using symbol_shorthand::L; /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, equals ) { +TEST( GaussianFactorGraph, equals ) { - OrderingOrdered ordering; ordering += X(1),X(2),L(1); - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering); - GaussianFactorGraphOrdered fg2 = createGaussianFactorGraph(ordering); + Ordering ordering; ordering += X(1),X(2),L(1); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianFactorGraph fg2 = createGaussianFactorGraph(ordering); EXPECT(fg.equals(fg2)); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, error ) { - OrderingOrdered ordering; ordering += X(1),X(2),L(1); - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering); - VectorValuesOrdered cfg = createZeroDelta(ordering); +TEST( GaussianFactorGraph, error ) { + Ordering ordering; ordering += X(1),X(2),L(1); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + VectorValues cfg = createZeroDelta(ordering); // note the error is the same as in testNonlinearFactorGraph as a // zero delta config in the linear graph is equivalent to noisy in @@ -69,71 +69,71 @@ TEST( GaussianFactorGraphOrdered, error ) { } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, eliminateOne_x1 ) +TEST( GaussianFactorGraph, eliminateOne_x1 ) { - OrderingOrdered ordering; ordering += X(1),L(1),X(2); - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering); + Ordering ordering; ordering += X(1),L(1),X(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - GaussianConditionalOrdered::shared_ptr conditional; - GaussianFactorGraphOrdered remaining; - boost::tie(conditional,remaining) = fg.eliminateOne(0, EliminateQROrdered); + GaussianConditional::shared_ptr conditional; + GaussianFactorGraph remaining; + boost::tie(conditional,remaining) = fg.eliminateOne(0, EliminateQR); // create expected Conditional Gaussian Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); - GaussianConditionalOrdered expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma); + GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma); EXPECT(assert_equal(expected,*conditional,tol)); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, eliminateOne_x2 ) +TEST( GaussianFactorGraph, eliminateOne_x2 ) { - OrderingOrdered ordering; ordering += X(2),L(1),X(1); - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering); - GaussianConditionalOrdered::shared_ptr actual = fg.eliminateOne(0, EliminateQROrdered).first; + Ordering ordering; ordering += X(2),L(1),X(1); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianConditional::shared_ptr actual = fg.eliminateOne(0, EliminateQR).first; // create expected Conditional Gaussian double sig = 0.0894427; Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); - GaussianConditionalOrdered expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma); + GaussianConditional expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, eliminateOne_l1 ) +TEST( GaussianFactorGraph, eliminateOne_l1 ) { - OrderingOrdered ordering; ordering += L(1),X(1),X(2); - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering); - GaussianConditionalOrdered::shared_ptr actual = fg.eliminateOne(0, EliminateQROrdered).first; + Ordering ordering; ordering += L(1),X(1),X(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianConditional::shared_ptr actual = fg.eliminateOne(0, EliminateQR).first; // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); - GaussianConditionalOrdered expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); + GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, eliminateOne_x1_fast ) +TEST( GaussianFactorGraph, eliminateOne_x1_fast ) { - OrderingOrdered ordering; ordering += X(1),L(1),X(2); - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering); - GaussianConditionalOrdered::shared_ptr conditional; - GaussianFactorGraphOrdered remaining; - boost::tie(conditional,remaining) = fg.eliminateOne(ordering[X(1)], EliminateQROrdered); + Ordering ordering; ordering += X(1),L(1),X(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianConditional::shared_ptr conditional; + GaussianFactorGraph remaining; + boost::tie(conditional,remaining) = fg.eliminateOne(ordering[X(1)], EliminateQR); // create expected Conditional Gaussian Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); - GaussianConditionalOrdered expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma); + GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma); // Create expected remaining new factor - JacobianFactorOrdered expectedFactor(1, Matrix_(4,2, + JacobianFactor expectedFactor(1, Matrix_(4,2, 4.714045207910318, 0., 0., 4.714045207910318, 0., 0., @@ -146,52 +146,52 @@ TEST( GaussianFactorGraphOrdered, eliminateOne_x1_fast ) Vector_(4, -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094), noiseModel::Unit::Create(4)); EXPECT(assert_equal(expected,*conditional,tol)); - EXPECT(assert_equal((const GaussianFactorOrdered&)expectedFactor,*remaining.back(),tol)); + EXPECT(assert_equal((const GaussianFactor&)expectedFactor,*remaining.back(),tol)); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, eliminateOne_x2_fast ) +TEST( GaussianFactorGraph, eliminateOne_x2_fast ) { - OrderingOrdered ordering; ordering += X(1),L(1),X(2); - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering); - GaussianConditionalOrdered::shared_ptr actual = fg.eliminateOne(ordering[X(2)], EliminateQROrdered).first; + Ordering ordering; ordering += X(1),L(1),X(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianConditional::shared_ptr actual = fg.eliminateOne(ordering[X(2)], EliminateQR).first; // create expected Conditional Gaussian double sig = 0.0894427; Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); - GaussianConditionalOrdered expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma); + GaussianConditional expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma); EXPECT(assert_equal(expected,*actual,tol)); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, eliminateOne_l1_fast ) +TEST( GaussianFactorGraph, eliminateOne_l1_fast ) { - OrderingOrdered ordering; ordering += X(1),L(1),X(2); - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering); - GaussianConditionalOrdered::shared_ptr actual = fg.eliminateOne(ordering[L(1)], EliminateQROrdered).first; + Ordering ordering; ordering += X(1),L(1),X(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + GaussianConditional::shared_ptr actual = fg.eliminateOne(ordering[L(1)], EliminateQR).first; // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); - GaussianConditionalOrdered expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); + GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, eliminateAll ) +TEST( GaussianFactorGraph, eliminateAll ) { // create expected Chordal bayes Net Matrix I = eye(2); - OrderingOrdered ordering; + Ordering ordering; ordering += X(2),L(1),X(1); Vector d1 = Vector_(2, -0.1,-0.1); - GaussianBayesNetOrdered expected = simpleGaussian(ordering[X(1)],d1,0.1); + GaussianBayesNet expected = simpleGaussian(ordering[X(1)],d1,0.1); double sig1 = 0.149071; Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2); @@ -202,108 +202,108 @@ TEST( GaussianFactorGraphOrdered, eliminateAll ) push_front(expected,ordering[X(2)],d3, I/sig2,ordering[L(1)], (-0.2)*I/sig2, ordering[X(1)], (-0.8)*I/sig2, sigma3); // Check one ordering - GaussianFactorGraphOrdered fg1 = createGaussianFactorGraph(ordering); - GaussianBayesNetOrdered actual = *GaussianSequentialSolver(fg1).eliminate(); + GaussianFactorGraph fg1 = createGaussianFactorGraph(ordering); + GaussianBayesNet actual = *GaussianSequentialSolver(fg1).eliminate(); EXPECT(assert_equal(expected,actual,tol)); - GaussianBayesNetOrdered actualQR = *GaussianSequentialSolver(fg1, true).eliminate(); + GaussianBayesNet actualQR = *GaussianSequentialSolver(fg1, true).eliminate(); EXPECT(assert_equal(expected,actualQR,tol)); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, copying ) +TEST( GaussianFactorGraph, copying ) { // Create a graph - OrderingOrdered ordering; ordering += X(2),L(1),X(1); - GaussianFactorGraphOrdered actual = createGaussianFactorGraph(ordering); + Ordering ordering; ordering += X(2),L(1),X(1); + GaussianFactorGraph actual = createGaussianFactorGraph(ordering); // Copy the graph ! - GaussianFactorGraphOrdered copy = actual; + GaussianFactorGraph copy = actual; // now eliminate the copy - GaussianBayesNetOrdered actual1 = *GaussianSequentialSolver(copy).eliminate(); + GaussianBayesNet actual1 = *GaussianSequentialSolver(copy).eliminate(); // Create the same graph, but not by copying - GaussianFactorGraphOrdered expected = createGaussianFactorGraph(ordering); + GaussianFactorGraph expected = createGaussianFactorGraph(ordering); // and check that original is still the same graph EXPECT(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, CONSTRUCTOR_GaussianBayesNet ) +TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) { - OrderingOrdered ord; + Ordering ord; ord += X(2),L(1),X(1); - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ord); + GaussianFactorGraph fg = createGaussianFactorGraph(ord); // render with a given ordering - GaussianBayesNetOrdered CBN = *GaussianSequentialSolver(fg).eliminate(); + GaussianBayesNet CBN = *GaussianSequentialSolver(fg).eliminate(); // True GaussianFactorGraph - GaussianFactorGraphOrdered fg2(CBN); - GaussianBayesNetOrdered CBN2 = *GaussianSequentialSolver(fg2).eliminate(); + GaussianFactorGraph fg2(CBN); + GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate(); EXPECT(assert_equal(CBN,CBN2)); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, getOrdering) +TEST( GaussianFactorGraph, getOrdering) { - OrderingOrdered original; original += L(1),X(1),X(2); - FactorGraphOrdered symbolic(createGaussianFactorGraph(original)); - Permutation perm(*inference::PermutationCOLAMD(VariableIndexOrdered(symbolic))); - OrderingOrdered actual = original; actual.permuteInPlace(perm); - OrderingOrdered expected; expected += L(1),X(2),X(1); + Ordering original; original += L(1),X(1),X(2); + FactorGraph symbolic(createGaussianFactorGraph(original)); + Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic))); + Ordering actual = original; actual.permuteInPlace(perm); + Ordering expected; expected += L(1),X(2),X(1); EXPECT(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, optimize_Cholesky ) +TEST( GaussianFactorGraph, optimize_Cholesky ) { // create an ordering - OrderingOrdered ord; ord += X(2),L(1),X(1); + Ordering ord; ord += X(2),L(1),X(1); // create a graph - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ord); + GaussianFactorGraph fg = createGaussianFactorGraph(ord); // optimize the graph - VectorValuesOrdered actual = *GaussianSequentialSolver(fg, false).optimize(); + VectorValues actual = *GaussianSequentialSolver(fg, false).optimize(); // verify - VectorValuesOrdered expected = createCorrectDelta(ord); + VectorValues expected = createCorrectDelta(ord); EXPECT(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, optimize_QR ) +TEST( GaussianFactorGraph, optimize_QR ) { // create an ordering - OrderingOrdered ord; ord += X(2),L(1),X(1); + Ordering ord; ord += X(2),L(1),X(1); // create a graph - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ord); + GaussianFactorGraph fg = createGaussianFactorGraph(ord); // optimize the graph - VectorValuesOrdered actual = *GaussianSequentialSolver(fg, true).optimize(); + VectorValues actual = *GaussianSequentialSolver(fg, true).optimize(); // verify - VectorValuesOrdered expected = createCorrectDelta(ord); + VectorValues expected = createCorrectDelta(ord); EXPECT(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, combine) +TEST( GaussianFactorGraph, combine) { // create an ordering - OrderingOrdered ord; ord += X(2),L(1),X(1); + Ordering ord; ord += X(2),L(1),X(1); // create a test graph - GaussianFactorGraphOrdered fg1 = createGaussianFactorGraph(ord); + GaussianFactorGraph fg1 = createGaussianFactorGraph(ord); // create another factor graph - GaussianFactorGraphOrdered fg2 = createGaussianFactorGraph(ord); + GaussianFactorGraph fg2 = createGaussianFactorGraph(ord); // get sizes size_t size1 = fg1.size(); @@ -316,23 +316,23 @@ TEST( GaussianFactorGraphOrdered, combine) } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, combine2) +TEST( GaussianFactorGraph, combine2) { // create an ordering - OrderingOrdered ord; ord += X(2),L(1),X(1); + Ordering ord; ord += X(2),L(1),X(1); // create a test graph - GaussianFactorGraphOrdered fg1 = createGaussianFactorGraph(ord); + GaussianFactorGraph fg1 = createGaussianFactorGraph(ord); // create another factor graph - GaussianFactorGraphOrdered fg2 = createGaussianFactorGraph(ord); + GaussianFactorGraph fg2 = createGaussianFactorGraph(ord); // get sizes size_t size1 = fg1.size(); size_t size2 = fg2.size(); // combine them - GaussianFactorGraphOrdered fg3 = GaussianFactorGraphOrdered::combine2(fg1, fg2); + GaussianFactorGraph fg3 = GaussianFactorGraph::combine2(fg1, fg2); EXPECT(size1+size2 == fg3.size()); } @@ -346,31 +346,31 @@ void print(vector v) { } /* ************************************************************************* */ -TEST(GaussianFactorGraphOrdered, createSmoother) +TEST(GaussianFactorGraph, createSmoother) { - GaussianFactorGraphOrdered fg1 = createSmoother(2).first; + GaussianFactorGraph fg1 = createSmoother(2).first; LONGS_EQUAL(3,fg1.size()); - GaussianFactorGraphOrdered fg2 = createSmoother(3).first; + GaussianFactorGraph fg2 = createSmoother(3).first; LONGS_EQUAL(5,fg2.size()); } /* ************************************************************************* */ -double error(const VectorValuesOrdered& x) { +double error(const VectorValues& x) { // create an ordering - OrderingOrdered ord; ord += X(2),L(1),X(1); + Ordering ord; ord += X(2),L(1),X(1); - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ord); + GaussianFactorGraph fg = createGaussianFactorGraph(ord); return fg.error(x); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, multiplication ) +TEST( GaussianFactorGraph, multiplication ) { // create an ordering - OrderingOrdered ord; ord += X(2),L(1),X(1); + Ordering ord; ord += X(2),L(1),X(1); - GaussianFactorGraphOrdered A = createGaussianFactorGraph(ord); - VectorValuesOrdered x = createCorrectDelta(ord); + GaussianFactorGraph A = createGaussianFactorGraph(ord); + VectorValues x = createCorrectDelta(ord); Errors actual = A * x; Errors expected; expected += Vector_(2,-1.0,-1.0); @@ -382,12 +382,12 @@ TEST( GaussianFactorGraphOrdered, multiplication ) /* ************************************************************************* */ // Extra test on elimination prompted by Michael's email to Frank 1/4/2010 -TEST( GaussianFactorGraphOrdered, elimination ) +TEST( GaussianFactorGraph, elimination ) { - OrderingOrdered ord; + Ordering ord; ord += X(1), X(2); // Create Gaussian Factor Graph - GaussianFactorGraphOrdered fg; + GaussianFactorGraph fg; Matrix Ap = eye(1), An = eye(1) * -1; Vector b = Vector_(1, 0.0); SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,2.0); @@ -396,7 +396,7 @@ TEST( GaussianFactorGraphOrdered, elimination ) fg.add(ord[X(2)], Ap, b, sigma); // Eliminate - GaussianBayesNetOrdered bayesNet = *GaussianSequentialSolver(fg).eliminate(); + GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate(); // Check sigma EXPECT_DOUBLES_EQUAL(1.0,bayesNet[ord[X(2)]]->get_sigmas()(0),1e-5); @@ -416,48 +416,48 @@ TEST( GaussianFactorGraphOrdered, elimination ) /* ************************************************************************* */ // Tests ported from ConstrainedGaussianFactorGraph /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, constrained_simple ) +TEST( GaussianFactorGraph, constrained_simple ) { // get a graph with a constraint in it - GaussianFactorGraphOrdered fg = createSimpleConstraintGraph(); + GaussianFactorGraph fg = createSimpleConstraintGraph(); EXPECT(hasConstraints(fg)); // eliminate and solve - VectorValuesOrdered actual = *GaussianSequentialSolver(fg).optimize(); + VectorValues actual = *GaussianSequentialSolver(fg).optimize(); // verify - VectorValuesOrdered expected = createSimpleConstraintValues(); + VectorValues expected = createSimpleConstraintValues(); EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, constrained_single ) +TEST( GaussianFactorGraph, constrained_single ) { // get a graph with a constraint in it - GaussianFactorGraphOrdered fg = createSingleConstraintGraph(); + GaussianFactorGraph fg = createSingleConstraintGraph(); EXPECT(hasConstraints(fg)); // eliminate and solve - VectorValuesOrdered actual = *GaussianSequentialSolver(fg).optimize(); + VectorValues actual = *GaussianSequentialSolver(fg).optimize(); // verify - VectorValuesOrdered expected = createSingleConstraintValues(); + VectorValues expected = createSingleConstraintValues(); EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, constrained_multi1 ) +TEST( GaussianFactorGraph, constrained_multi1 ) { // get a graph with a constraint in it - GaussianFactorGraphOrdered fg = createMultiConstraintGraph(); + GaussianFactorGraph fg = createMultiConstraintGraph(); EXPECT(hasConstraints(fg)); // eliminate and solve - VectorValuesOrdered actual = *GaussianSequentialSolver(fg).optimize(); + VectorValues actual = *GaussianSequentialSolver(fg).optimize(); // verify - VectorValuesOrdered expected = createMultiConstraintValues(); + VectorValues expected = createMultiConstraintValues(); EXPECT(assert_equal(expected, actual)); } @@ -466,27 +466,27 @@ TEST( GaussianFactorGraphOrdered, constrained_multi1 ) static SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1); /* ************************************************************************* */ -TEST(GaussianFactorGraphOrdered, replace) +TEST(GaussianFactorGraph, replace) { - OrderingOrdered ord; ord += X(1),X(2),X(3),X(4),X(5),X(6); + Ordering ord; ord += X(1),X(2),X(3),X(4),X(5),X(6); SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0)); - GaussianFactorGraphOrdered::sharedFactor f1(new JacobianFactorOrdered( + GaussianFactorGraph::sharedFactor f1(new JacobianFactor( ord[X(1)], eye(3,3), ord[X(2)], eye(3,3), zero(3), noise)); - GaussianFactorGraphOrdered::sharedFactor f2(new JacobianFactorOrdered( + GaussianFactorGraph::sharedFactor f2(new JacobianFactor( ord[X(2)], eye(3,3), ord[X(3)], eye(3,3), zero(3), noise)); - GaussianFactorGraphOrdered::sharedFactor f3(new JacobianFactorOrdered( + GaussianFactorGraph::sharedFactor f3(new JacobianFactor( ord[X(3)], eye(3,3), ord[X(4)], eye(3,3), zero(3), noise)); - GaussianFactorGraphOrdered::sharedFactor f4(new JacobianFactorOrdered( + GaussianFactorGraph::sharedFactor f4(new JacobianFactor( ord[X(5)], eye(3,3), ord[X(6)], eye(3,3), zero(3), noise)); - GaussianFactorGraphOrdered actual; + GaussianFactorGraph actual; actual.push_back(f1); actual.push_back(f2); actual.push_back(f3); actual.replace(0, f4); - GaussianFactorGraphOrdered expected; + GaussianFactorGraph expected; expected.push_back(f4); expected.push_back(f2); expected.push_back(f3); @@ -495,35 +495,35 @@ TEST(GaussianFactorGraphOrdered, replace) } /* ************************************************************************* */ -TEST(GaussianFactorGraphOrdered, createSmoother2) +TEST(GaussianFactorGraph, createSmoother2) { using namespace example; - GaussianFactorGraphOrdered fg2; - OrderingOrdered ordering; + GaussianFactorGraph fg2; + Ordering ordering; boost::tie(fg2,ordering) = createSmoother(3); LONGS_EQUAL(5,fg2.size()); // eliminate vector x3var; x3var.push_back(ordering[X(3)]); vector x1var; x1var.push_back(ordering[X(1)]); - GaussianBayesNetOrdered p_x3 = *GaussianSequentialSolver( + GaussianBayesNet p_x3 = *GaussianSequentialSolver( *GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate(); - GaussianBayesNetOrdered p_x1 = *GaussianSequentialSolver( + GaussianBayesNet p_x1 = *GaussianSequentialSolver( *GaussianSequentialSolver(fg2).jointFactorGraph(x1var)).eliminate(); CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry } /* ************************************************************************* */ -TEST(GaussianFactorGraphOrdered, hasConstraints) +TEST(GaussianFactorGraph, hasConstraints) { - FactorGraphOrdered fgc1 = createMultiConstraintGraph(); + FactorGraph fgc1 = createMultiConstraintGraph(); EXPECT(hasConstraints(fgc1)); - FactorGraphOrdered fgc2 = createSimpleConstraintGraph() ; + FactorGraph fgc2 = createSimpleConstraintGraph() ; EXPECT(hasConstraints(fgc2)); - OrderingOrdered ordering; ordering += X(1), X(2), L(1); - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering); + Ordering ordering; ordering += X(1), X(2), L(1); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); EXPECT(!hasConstraints(fg)); } @@ -534,7 +534,7 @@ TEST(GaussianFactorGraphOrdered, hasConstraints) #include /* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, conditional_sigma_failure) { +TEST( GaussianFactorGraph, conditional_sigma_failure) { // This system derives from a failure case in DDF in which a Bayes Tree // has non-unit sigmas for conditionals in the Bayes Tree, which // should never happen by construction @@ -578,17 +578,17 @@ TEST( GaussianFactorGraphOrdered, conditional_sigma_failure) { factors.add(RangeFactor(xC1, l32, relElevation, elevationModel)); factors.add(RangeFactor(xC1, l41, relElevation, elevationModel)); - OrderingOrdered orderingC; orderingC += xC1, l32, l41; + Ordering orderingC; orderingC += xC1, l32, l41; // Check that sigmas are correct (i.e., unit) - GaussianFactorGraphOrdered lfg = *factors.linearize(initValues, orderingC); + GaussianFactorGraph lfg = *factors.linearize(initValues, orderingC); GaussianMultifrontalSolver solver(lfg, false); - GaussianBayesTreeOrdered actBT = *solver.eliminate(); + GaussianBayesTree actBT = *solver.eliminate(); // Check that all sigmas in an unconstrained bayes tree are set to one - BOOST_FOREACH(const GaussianBayesTreeOrdered::sharedClique& clique, actBT.nodes()) { - GaussianConditionalOrdered::shared_ptr conditional = clique->conditional(); + BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, actBT.nodes()) { + GaussianConditional::shared_ptr conditional = clique->conditional(); size_t dim = conditional->dim(); EXPECT(assert_equal(gtsam::ones(dim), conditional->get_sigmas(), tol)); } diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index bd6ffc5cf..aa169c3a0 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -152,21 +152,21 @@ TEST_UNSAFE(ISAM2, ImplAddVariables) { Values newTheta; newTheta.insert(1, Pose2(.6, .7, .8)); - VectorValuesOrdered delta; + VectorValues delta; delta.insert(0, Vector_(3, .1, .2, .3)); delta.insert(1, Vector_(2, .4, .5)); - VectorValuesOrdered deltaNewton; + VectorValues deltaNewton; deltaNewton.insert(0, Vector_(3, .1, .2, .3)); deltaNewton.insert(1, Vector_(2, .4, .5)); - VectorValuesOrdered deltaRg; + VectorValues deltaRg; deltaRg.insert(0, Vector_(3, .1, .2, .3)); deltaRg.insert(1, Vector_(2, .4, .5)); vector replacedKeys(2, false); - OrderingOrdered ordering; ordering += 100, 0; + Ordering ordering; ordering += 100, 0; // Verify initial state LONGS_EQUAL(0, ordering[100]); @@ -180,24 +180,24 @@ TEST_UNSAFE(ISAM2, ImplAddVariables) { thetaExpected.insert(100, Point2(.4, .5)); thetaExpected.insert(1, Pose2(.6, .7, .8)); - VectorValuesOrdered deltaExpected; + VectorValues deltaExpected; deltaExpected.insert(0, Vector_(3, .1, .2, .3)); deltaExpected.insert(1, Vector_(2, .4, .5)); deltaExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - VectorValuesOrdered deltaNewtonExpected; + VectorValues deltaNewtonExpected; deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3)); deltaNewtonExpected.insert(1, Vector_(2, .4, .5)); deltaNewtonExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - VectorValuesOrdered deltaRgExpected; + VectorValues deltaRgExpected; deltaRgExpected.insert(0, Vector_(3, .1, .2, .3)); deltaRgExpected.insert(1, Vector_(2, .4, .5)); deltaRgExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); vector replacedKeysExpected(3, false); - OrderingOrdered orderingExpected; orderingExpected += 100, 0, 1; + Ordering orderingExpected; orderingExpected += 100, 0, 1; // Expand initial state ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering); @@ -219,22 +219,22 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) { theta.insert(1, Pose2(.6, .7, .8)); theta.insert(100, Point2(.4, .5)); - SymbolicFactorGraphOrdered sfg; - sfg.push_back(boost::make_shared(Index(0), Index(2))); - sfg.push_back(boost::make_shared(Index(0), Index(1))); - VariableIndexOrdered variableIndex(sfg); + SymbolicFactorGraph sfg; + sfg.push_back(boost::make_shared(Index(0), Index(2))); + sfg.push_back(boost::make_shared(Index(0), Index(1))); + VariableIndex variableIndex(sfg); - VectorValuesOrdered delta; + VectorValues delta; delta.insert(0, Vector_(3, .1, .2, .3)); delta.insert(1, Vector_(3, .4, .5, .6)); delta.insert(2, Vector_(2, .7, .8)); - VectorValuesOrdered deltaNewton; + VectorValues deltaNewton; deltaNewton.insert(0, Vector_(3, .1, .2, .3)); deltaNewton.insert(1, Vector_(3, .4, .5, .6)); deltaNewton.insert(2, Vector_(2, .7, .8)); - VectorValuesOrdered deltaRg; + VectorValues deltaRg; deltaRg.insert(0, Vector_(3, .1, .2, .3)); deltaRg.insert(1, Vector_(3, .4, .5, .6)); deltaRg.insert(2, Vector_(2, .7, .8)); @@ -244,7 +244,7 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) { replacedKeys[1] = false; replacedKeys[2] = true; - OrderingOrdered ordering; ordering += 100, 1, 0; + Ordering ordering; ordering += 100, 1, 0; ISAM2::Nodes nodes(3); @@ -258,26 +258,26 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) { thetaExpected.insert(0, Pose2(.1, .2, .3)); thetaExpected.insert(100, Point2(.4, .5)); - SymbolicFactorGraphOrdered sfgRemoved; - sfgRemoved.push_back(boost::make_shared(Index(0), Index(1))); - sfgRemoved.push_back(SymbolicFactorGraphOrdered::sharedFactor()); // Add empty factor to keep factor indices consistent - VariableIndexOrdered variableIndexExpected(sfgRemoved); + SymbolicFactorGraph sfgRemoved; + sfgRemoved.push_back(boost::make_shared(Index(0), Index(1))); + sfgRemoved.push_back(SymbolicFactorGraph::sharedFactor()); // Add empty factor to keep factor indices consistent + VariableIndex variableIndexExpected(sfgRemoved); - VectorValuesOrdered deltaExpected; + VectorValues deltaExpected; deltaExpected.insert(0, Vector_(3, .1, .2, .3)); deltaExpected.insert(1, Vector_(2, .7, .8)); - VectorValuesOrdered deltaNewtonExpected; + VectorValues deltaNewtonExpected; deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3)); deltaNewtonExpected.insert(1, Vector_(2, .7, .8)); - VectorValuesOrdered deltaRgExpected; + VectorValues deltaRgExpected; deltaRgExpected.insert(0, Vector_(3, .1, .2, .3)); deltaRgExpected.insert(1, Vector_(2, .7, .8)); vector replacedKeysExpected(2, true); - OrderingOrdered orderingExpected; orderingExpected += 100, 0; + Ordering orderingExpected; orderingExpected += 100, 0; ISAM2::Nodes nodesExpected(2); @@ -285,9 +285,9 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) { FastSet unusedKeys; unusedKeys.insert(1); vector removedFactorsI; removedFactorsI.push_back(1); - SymbolicFactorGraphOrdered removedFactors; removedFactors.push_back(sfg[1]); + SymbolicFactorGraph removedFactors; removedFactors.push_back(sfg[1]); variableIndex.remove(removedFactorsI, removedFactors); - GaussianFactorGraphOrdered linearFactors; + GaussianFactorGraph linearFactors; FastSet fixedVariables; ISAM2::Impl::RemoveVariables(unusedKeys, ISAM2::sharedClique(), theta, variableIndex, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes, linearFactors, fixedVariables); @@ -307,7 +307,7 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) { // using namespace gtsam::planarSLAM; // typedef GaussianISAM2::Impl Impl; // -// OrderingOrdered ordering; ordering += (0), (0), (1); +// Ordering ordering; ordering += (0), (0), (1); // NonlinearFactorGraph graph; // graph.add(PriorFactor((0), Pose2(), noiseModel::Unit::Create(Pose2::dimension)); // graph.addRange((0), (0), 1.0, noiseModel::Unit::Create(1)); @@ -327,7 +327,7 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) { // typedef GaussianISAM2::Impl Impl; // // // Create values where indices 1 and 3 are above the threshold of 0.1 -// VectorValuesOrdered values; +// VectorValues values; // values.reserve(4, 10); // values.push_back_preallocated(Vector_(2, 0.09, 0.09)); // values.push_back_preallocated(Vector_(3, 0.11, 0.11, 0.09)); @@ -341,7 +341,7 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) { // permutation[2] = 1; // permutation[3] = 3; // -// Permuted permuted(permutation, values); +// Permuted permuted(permutation, values); // // // After permutation, the indices above the threshold are 2 and 2 // FastSet expected; @@ -367,19 +367,19 @@ TEST(ISAM2, optimize2) { 10, 0.0, 0.0, 0.0, 10, 0.0, 0.0, 0.0, 31.8309886; - GaussianConditionalOrdered::shared_ptr conditional(new GaussianConditionalOrdered(0, d, R, Vector::Ones(3))); + GaussianConditional::shared_ptr conditional(new GaussianConditional(0, d, R, Vector::Ones(3))); // Create ordering - OrderingOrdered ordering; ordering += (0); + Ordering ordering; ordering += (0); // Expected vector - VectorValuesOrdered expected(1, 3); + VectorValues expected(1, 3); conditional->solveInPlace(expected); // Clique ISAM2::sharedClique clique( - ISAM2::Clique::Create(make_pair(conditional,GaussianFactorOrdered::shared_ptr()))); - VectorValuesOrdered actual(theta.dims(ordering)); + ISAM2::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr()))); + VectorValues actual(theta.dims(ordering)); internal::optimizeInPlace(clique, actual); // expected.print("expected: "); @@ -394,12 +394,12 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c const std::string name_ = test.getName(); Values actual = isam.calculateEstimate(); - OrderingOrdered ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first; - GaussianFactorGraphOrdered linearized = *fullgraph.linearize(fullinit, ordering); + Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first; + GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); // linearized.print("Expected linearized: "); - GaussianBayesNetOrdered gbn = *GaussianSequentialSolver(linearized).eliminate(); + GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); // gbn.print("Expected bayesnet: "); - VectorValuesOrdered delta = optimize(gbn); + VectorValues delta = optimize(gbn); Values expected = fullinit.retract(delta, ordering); bool isamEqual = assert_equal(expected, actual); @@ -411,13 +411,13 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c typedef ISAM2::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient - FactorGraphOrdered jfg; - jfg.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(*clique->conditional()))); - VectorValuesOrdered expectedGradient(*allocateVectorValues(isam)); + FactorGraph jfg; + jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); + VectorValues expectedGradient(*allocateVectorValues(isam)); gradientAtZero(jfg, expectedGradient); // Compare with actual gradients int variablePosition = 0; - for(GaussianConditionalOrdered::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { + for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { const int dim = clique->conditional()->dim(jit); Vector actual = clique->gradientContribution().segment(variablePosition, dim); bool gradOk = assert_equal(expectedGradient[*jit], actual); @@ -431,10 +431,10 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c } // Check gradient - VectorValuesOrdered expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraphOrdered(isam), expectedGradient); - VectorValuesOrdered expectedGradient2(gradient(FactorGraphOrdered(isam), VectorValuesOrdered::Zero(expectedGradient))); - VectorValuesOrdered actualGradient(*allocateVectorValues(isam)); + VectorValues expectedGradient(*allocateVectorValues(isam)); + gradientAtZero(FactorGraph(isam), expectedGradient); + VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); + VectorValues actualGradient(*allocateVectorValues(isam)); gradientAtZero(isam, actualGradient); bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient); EXPECT(expectedGradOk); @@ -531,49 +531,49 @@ TEST(ISAM2, permute_cached) { typedef boost::shared_ptr sharedISAM2Clique; // Construct expected permuted BayesTree (variable 2 has been changed to 1) - BayesTreeOrdered expected; + BayesTree expected; expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of + boost::make_shared(pair_list_of (3, Matrix_(1,1,1.0)) (4, Matrix_(1,1,2.0)), 2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4) - HessianFactorOrdered::shared_ptr())))); // Cached: empty + HessianFactor::shared_ptr())))); // Cached: empty expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of + boost::make_shared(pair_list_of (2, Matrix_(1,1,1.0)) (3, Matrix_(1,1,2.0)), 1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3) - boost::make_shared(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3) + boost::make_shared(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3) expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of + boost::make_shared(pair_list_of (0, Matrix_(1,1,1.0)) (2, Matrix_(1,1,2.0)), 1, Vector_(1,1.0), Vector_(1,1.0)), // p(0|2) - boost::make_shared(1, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(1) + boost::make_shared(1, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(1) // Change variable 2 to 1 expected.root()->children().front()->conditional()->keys()[0] = 1; expected.root()->children().front()->children().front()->conditional()->keys()[1] = 1; // Construct unpermuted BayesTree - BayesTreeOrdered actual; + BayesTree actual; actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of + boost::make_shared(pair_list_of (3, Matrix_(1,1,1.0)) (4, Matrix_(1,1,2.0)), 2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4) - HessianFactorOrdered::shared_ptr())))); // Cached: empty + HessianFactor::shared_ptr())))); // Cached: empty actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of + boost::make_shared(pair_list_of (2, Matrix_(1,1,1.0)) (3, Matrix_(1,1,2.0)), 1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3) - boost::make_shared(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3) + boost::make_shared(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3) actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of + boost::make_shared(pair_list_of (0, Matrix_(1,1,1.0)) (2, Matrix_(1,1,2.0)), 1, Vector_(1,1.0), Vector_(1,1.0)), // p(0|2) - boost::make_shared(2, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(2) + boost::make_shared(2, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(2) // Create permutation that changes variable 2 -> 0 Permutation permutation = Permutation::Identity(5); @@ -664,13 +664,13 @@ TEST_UNSAFE(ISAM2, swapFactors) typedef ISAM2::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient - FactorGraphOrdered jfg; - jfg.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(*clique->conditional()))); - VectorValuesOrdered expectedGradient(*allocateVectorValues(isam)); + FactorGraph jfg; + jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); + VectorValues expectedGradient(*allocateVectorValues(isam)); gradientAtZero(jfg, expectedGradient); // Compare with actual gradients int variablePosition = 0; - for(GaussianConditionalOrdered::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { + for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { const int dim = clique->conditional()->dim(jit); Vector actual = clique->gradientContribution().segment(variablePosition, dim); EXPECT(assert_equal(expectedGradient[*jit], actual)); @@ -680,10 +680,10 @@ TEST_UNSAFE(ISAM2, swapFactors) } // Check gradient - VectorValuesOrdered expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraphOrdered(isam), expectedGradient); - VectorValuesOrdered expectedGradient2(gradient(FactorGraphOrdered(isam), VectorValuesOrdered::Zero(expectedGradient))); - VectorValuesOrdered actualGradient(*allocateVectorValues(isam)); + VectorValues expectedGradient(*allocateVectorValues(isam)); + gradientAtZero(FactorGraph(isam), expectedGradient); + VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); + VectorValues actualGradient(*allocateVectorValues(isam)); gradientAtZero(isam, actualGradient); EXPECT(assert_equal(expectedGradient2, expectedGradient)); EXPECT(assert_equal(expectedGradient, actualGradient)); @@ -795,13 +795,13 @@ TEST(ISAM2, constrained_ordering) typedef ISAM2::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient - FactorGraphOrdered jfg; - jfg.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(*clique->conditional()))); - VectorValuesOrdered expectedGradient(*allocateVectorValues(isam)); + FactorGraph jfg; + jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); + VectorValues expectedGradient(*allocateVectorValues(isam)); gradientAtZero(jfg, expectedGradient); // Compare with actual gradients int variablePosition = 0; - for(GaussianConditionalOrdered::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { + for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { const int dim = clique->conditional()->dim(jit); Vector actual = clique->gradientContribution().segment(variablePosition, dim); EXPECT(assert_equal(expectedGradient[*jit], actual)); @@ -811,10 +811,10 @@ TEST(ISAM2, constrained_ordering) } // Check gradient - VectorValuesOrdered expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraphOrdered(isam), expectedGradient); - VectorValuesOrdered expectedGradient2(gradient(FactorGraphOrdered(isam), VectorValuesOrdered::Zero(expectedGradient))); - VectorValuesOrdered actualGradient(*allocateVectorValues(isam)); + VectorValues expectedGradient(*allocateVectorValues(isam)); + gradientAtZero(FactorGraph(isam), expectedGradient); + VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); + VectorValues actualGradient(*allocateVectorValues(isam)); gradientAtZero(isam, actualGradient); EXPECT(assert_equal(expectedGradient2, expectedGradient)); EXPECT(assert_equal(expectedGradient, actualGradient)); @@ -844,9 +844,9 @@ namespace { toKeep.push_back(i); // Calculate expected marginal from iSAM2 tree - GaussianFactorGraphOrdered isamAsGraph(isam); + GaussianFactorGraph isamAsGraph(isam); GaussianSequentialSolver solver(isamAsGraph); - GaussianFactorGraphOrdered marginalgfg = *solver.jointFactorGraph(toKeep); + GaussianFactorGraph marginalgfg = *solver.jointFactorGraph(toKeep); expectedAugmentedHessian = marginalgfg.augmentedHessian(); //// Calculate expected marginal from cached linear factors @@ -864,7 +864,7 @@ namespace { isam.marginalizeLeaves(leafKeys); // Check - GaussianFactorGraphOrdered actualMarginalGraph(isam); + GaussianFactorGraph actualMarginalGraph(isam); Matrix actualAugmentedHessian = actualMarginalGraph.augmentedHessian(); //Matrix actual2AugmentedHessian = linearFactors_.augmentedHessian(); Matrix actual3AugmentedHessian = isam.getFactorsUnsafe().linearize( diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 4b6769935..b24b2b901 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -59,11 +59,11 @@ using symbol_shorthand::L; TEST( GaussianJunctionTreeB, constructor2 ) { // create a graph - OrderingOrdered ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); - GaussianFactorGraphOrdered fg = createSmoother(7, ordering).first; + Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); + GaussianFactorGraph fg = createSmoother(7, ordering).first; // create an ordering - GaussianJunctionTreeOrdered actual(fg); + GaussianJunctionTree actual(fg); vector frontal1; frontal1 += ordering[X(5)], ordering[X(6)], ordering[X(4)]; vector frontal2; frontal2 += ordering[X(3)], ordering[X(2)]; @@ -76,10 +76,10 @@ TEST( GaussianJunctionTreeB, constructor2 ) EXPECT(assert_equal(frontal1, actual.root()->frontal)); EXPECT(assert_equal(sep1, actual.root()->separator)); LONGS_EQUAL(5, actual.root()->size()); - list::const_iterator child0it = actual.root()->children().begin(); - list::const_iterator child1it = child0it; ++child1it; - GaussianJunctionTreeOrdered::sharedClique child0 = *child0it; - GaussianJunctionTreeOrdered::sharedClique child1 = *child1it; + list::const_iterator child0it = actual.root()->children().begin(); + list::const_iterator child1it = child0it; ++child1it; + GaussianJunctionTree::sharedClique child0 = *child0it; + GaussianJunctionTree::sharedClique child1 = *child1it; EXPECT(assert_equal(frontal2, child0->frontal)); EXPECT(assert_equal(sep2, child0->separator)); LONGS_EQUAL(4, child0->size()); @@ -95,16 +95,16 @@ TEST( GaussianJunctionTreeB, constructor2 ) TEST( GaussianJunctionTreeB, optimizeMultiFrontal ) { // create a graph - GaussianFactorGraphOrdered fg; - OrderingOrdered ordering; + GaussianFactorGraph fg; + Ordering ordering; boost::tie(fg,ordering) = createSmoother(7); // optimize the graph - GaussianJunctionTreeOrdered tree(fg); - VectorValuesOrdered actual = tree.optimize(&EliminateQROrdered); + GaussianJunctionTree tree(fg); + VectorValues actual = tree.optimize(&EliminateQR); // verify - VectorValuesOrdered expected(vector(7,2)); // expected solution + VectorValues expected(vector(7,2)); // expected solution Vector v = Vector_(2, 0., 0.); for (int i=1; i<=7; i++) expected[ordering[X(i)]] = v; @@ -117,15 +117,15 @@ TEST( GaussianJunctionTreeB, optimizeMultiFrontal2) // create a graph example::Graph nlfg = createNonlinearFactorGraph(); Values noisy = createNoisyValues(); - OrderingOrdered ordering; ordering += X(1),X(2),L(1); - GaussianFactorGraphOrdered fg = *nlfg.linearize(noisy, ordering); + Ordering ordering; ordering += X(1),X(2),L(1); + GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering); // optimize the graph - GaussianJunctionTreeOrdered tree(fg); - VectorValuesOrdered actual = tree.optimize(&EliminateQROrdered); + GaussianJunctionTree tree(fg); + VectorValues actual = tree.optimize(&EliminateQR); // verify - VectorValuesOrdered expected = createCorrectDelta(ordering); // expected solution + VectorValues expected = createCorrectDelta(ordering); // expected solution EXPECT(assert_equal(expected,actual)); } @@ -177,15 +177,15 @@ TEST(GaussianJunctionTreeB, slamlike) { ++ i; // Compare solutions - OrderingOrdered ordering = *fullgraph.orderingCOLAMD(init); - GaussianFactorGraphOrdered linearized = *fullgraph.linearize(init, ordering); + Ordering ordering = *fullgraph.orderingCOLAMD(init); + GaussianFactorGraph linearized = *fullgraph.linearize(init, ordering); - GaussianJunctionTreeOrdered gjt(linearized); - VectorValuesOrdered deltaactual = gjt.optimize(&EliminateQROrdered); + GaussianJunctionTree gjt(linearized); + VectorValues deltaactual = gjt.optimize(&EliminateQR); Values actual = init.retract(deltaactual, ordering); - GaussianBayesNetOrdered gbn = *GaussianSequentialSolver(linearized).eliminate(); - VectorValuesOrdered delta = optimize(gbn); + GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); + VectorValues delta = optimize(gbn); Values expected = init.retract(delta, ordering); EXPECT(assert_equal(expected, actual)); @@ -194,7 +194,7 @@ TEST(GaussianJunctionTreeB, slamlike) { /* ************************************************************************* */ TEST(GaussianJunctionTreeB, simpleMarginal) { - typedef BayesTreeOrdered GaussianBayesTree; + typedef BayesTree GaussianBayesTree; // Create a simple graph NonlinearFactorGraph fg; @@ -205,10 +205,10 @@ TEST(GaussianJunctionTreeB, simpleMarginal) { init.insert(X(0), Pose2()); init.insert(X(1), Pose2(1.0, 0.0, 0.0)); - OrderingOrdered ordering; + Ordering ordering; ordering += X(1), X(0); - GaussianFactorGraphOrdered gfg = *fg.linearize(init, ordering); + GaussianFactorGraph gfg = *fg.linearize(init, ordering); // Compute marginals with both sequential and multifrontal Matrix expected = GaussianSequentialSolver(gfg).marginalCovariance(1); @@ -216,15 +216,15 @@ TEST(GaussianJunctionTreeB, simpleMarginal) { Matrix actual1 = GaussianMultifrontalSolver(gfg).marginalCovariance(1); // Compute marginal directly from marginal factor - GaussianFactorOrdered::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1); - JacobianFactorOrdered::shared_ptr marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); + GaussianFactor::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1); + JacobianFactor::shared_ptr marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); Matrix actual2 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); // Compute marginal directly from BayesTree GaussianBayesTree gbt; - gbt.insert(GaussianJunctionTreeOrdered(gfg).eliminate(EliminateCholeskyOrdered)); - marginalFactor = gbt.marginalFactor(1, EliminateCholeskyOrdered); - marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); + gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateCholesky)); + marginalFactor = gbt.marginalFactor(1, EliminateCholesky); + marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); EXPECT(assert_equal(expected, actual1)); diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 0f68a03d9..dc00b3f4a 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -36,7 +36,7 @@ using namespace gtsam; // x1 -> x2 // -> x3 -> x4 // -> x5 -TEST ( OrderingOrdered, predecessorMap2Keys ) { +TEST ( Ordering, predecessorMap2Keys ) { PredecessorMap p_map; p_map.insert(1,1); p_map.insert(2,1); diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index 0097170b5..1484052e5 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -41,16 +41,16 @@ static ConjugateGradientParameters parameters; TEST( Iterative, steepestDescent ) { // Create factor graph - OrderingOrdered ordering; + Ordering ordering; ordering += L(1), X(1), X(2); - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // eliminate and solve - VectorValuesOrdered expected = *GaussianSequentialSolver(fg).optimize(); + VectorValues expected = *GaussianSequentialSolver(fg).optimize(); // Do gradient descent - VectorValuesOrdered zero = VectorValuesOrdered::Zero(expected); // TODO, how do we do this normally? - VectorValuesOrdered actual = steepestDescent(fg, zero, parameters); + VectorValues zero = VectorValues::Zero(expected); // TODO, how do we do this normally? + VectorValues actual = steepestDescent(fg, zero, parameters); CHECK(assert_equal(expected,actual,1e-2)); } @@ -58,12 +58,12 @@ TEST( Iterative, steepestDescent ) TEST( Iterative, conjugateGradientDescent ) { // Create factor graph - OrderingOrdered ordering; + Ordering ordering; ordering += L(1), X(1), X(2); - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // eliminate and solve - VectorValuesOrdered expected = *GaussianSequentialSolver(fg).optimize(); + VectorValues expected = *GaussianSequentialSolver(fg).optimize(); // get matrices Matrix A; @@ -82,8 +82,8 @@ TEST( Iterative, conjugateGradientDescent ) CHECK(assert_equal(expectedX,actualX2,1e-9)); // Do conjugate gradient descent on factor graph - VectorValuesOrdered zero = VectorValuesOrdered::Zero(expected); - VectorValuesOrdered actual = conjugateGradientDescent(fg, zero, parameters); + VectorValues zero = VectorValues::Zero(expected); + VectorValues actual = conjugateGradientDescent(fg, zero, parameters); CHECK(assert_equal(expected,actual,1e-2)); } @@ -99,19 +99,19 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint ) graph.add(NonlinearEquality(X(1), pose1)); graph.add(BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1))); - OrderingOrdered ordering; + Ordering ordering; ordering += X(1), X(2); - boost::shared_ptr fg = graph.linearize(config,ordering); + boost::shared_ptr fg = graph.linearize(config,ordering); - VectorValuesOrdered zeros = VectorValuesOrdered::Zero(2, 3); + VectorValues zeros = VectorValues::Zero(2, 3); ConjugateGradientParameters parameters; parameters.setEpsilon_abs(1e-3); parameters.setEpsilon_rel(1e-5); parameters.setMaxIterations(100); - VectorValuesOrdered actual = conjugateGradientDescent(*fg, zeros, parameters); + VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); - VectorValuesOrdered expected; + VectorValues expected; expected.insert(0, zero(3)); expected.insert(1, Vector_(3,-0.5,0.,0.)); CHECK(assert_equal(expected, actual)); @@ -128,19 +128,19 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) graph.add(PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10))); graph.add(BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1))); - OrderingOrdered ordering; + Ordering ordering; ordering += X(1), X(2); - boost::shared_ptr fg = graph.linearize(config,ordering); + boost::shared_ptr fg = graph.linearize(config,ordering); - VectorValuesOrdered zeros = VectorValuesOrdered::Zero(2, 3); + VectorValues zeros = VectorValues::Zero(2, 3); ConjugateGradientParameters parameters; parameters.setEpsilon_abs(1e-3); parameters.setEpsilon_rel(1e-5); parameters.setMaxIterations(100); - VectorValuesOrdered actual = conjugateGradientDescent(*fg, zeros, parameters); + VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); - VectorValuesOrdered expected; + VectorValues expected; expected.insert(0, zero(3)); expected.insert(1, Vector_(3,-0.5,0.,0.)); CHECK(assert_equal(expected, actual)); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 5322721b2..f10ce1b35 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -55,9 +55,9 @@ TEST ( NonlinearEquality, linearization ) { // check linearize SharedDiagonal constraintModel = noiseModel::Constrained::All(3); - JacobianFactorOrdered expLF(0, eye(3), zero(3), constraintModel); - GaussianFactorOrdered::shared_ptr actualLF = nle->linearize(linearize, *linearize.orderingArbitrary()); - EXPECT(assert_equal(*actualLF, (const GaussianFactorOrdered&)expLF)); + JacobianFactor expLF(0, eye(3), zero(3), constraintModel); + GaussianFactor::shared_ptr actualLF = nle->linearize(linearize, *linearize.orderingArbitrary()); + EXPECT(assert_equal(*actualLF, (const GaussianFactor&)expLF)); } /* ********************************************************************** */ @@ -71,7 +71,7 @@ TEST ( NonlinearEquality, linearization_pose ) { // create a nonlinear equality constraint shared_poseNLE nle(new PoseNLE(key, value)); - GaussianFactorOrdered::shared_ptr actualLF = nle->linearize(config, *config.orderingArbitrary()); + GaussianFactor::shared_ptr actualLF = nle->linearize(config, *config.orderingArbitrary()); EXPECT(true); } @@ -176,11 +176,11 @@ TEST ( NonlinearEquality, allow_error_pose ) { DOUBLES_EQUAL(500.0, actError, 1e-9); // check linearization - GaussianFactorOrdered::shared_ptr actLinFactor = nle.linearize(config, *config.orderingArbitrary()); + GaussianFactor::shared_ptr actLinFactor = nle.linearize(config, *config.orderingArbitrary()); Matrix A1 = eye(3,3); Vector b = expVec; SharedDiagonal model = noiseModel::Constrained::All(3); - GaussianFactorOrdered::shared_ptr expLinFactor(new JacobianFactorOrdered(0, A1, b, model)); + GaussianFactor::shared_ptr expLinFactor(new JacobianFactor(0, A1, b, model)); EXPECT(assert_equal(*expLinFactor, *actLinFactor, 1e-5)); } @@ -201,7 +201,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) { init.insert(key1, initPose); // optimize - OrderingOrdered ordering; + Ordering ordering; ordering.push_back(key1); Values result = LevenbergMarquardtOptimizer(graph, init, ordering).optimize(); @@ -235,7 +235,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { graph.add(prior); // optimize - OrderingOrdered ordering; + Ordering ordering; ordering.push_back(key1); Values actual = LevenbergMarquardtOptimizer(graph, init, ordering).optimize(); @@ -277,21 +277,21 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { Point2 pt(1.0, 2.0); Symbol key1('x',1); double mu = 1000.0; - OrderingOrdered ordering; + Ordering ordering; ordering += key; eq2D::UnaryEqualityConstraint constraint(pt, key, mu); Values config1; config1.insert(key, pt); - GaussianFactorOrdered::shared_ptr actual1 = constraint.linearize(config1, ordering); - GaussianFactorOrdered::shared_ptr expected1(new JacobianFactorOrdered(ordering[key], eye(2,2), zero(2), hard_model)); + GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering); + GaussianFactor::shared_ptr expected1(new JacobianFactor(ordering[key], eye(2,2), zero(2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); Values config2; Point2 ptBad(2.0, 2.0); config2.insert(key, ptBad); - GaussianFactorOrdered::shared_ptr actual2 = constraint.linearize(config2, ordering); - GaussianFactorOrdered::shared_ptr expected2(new JacobianFactorOrdered(ordering[key], eye(2,2), Vector_(2,-1.0,0.0), hard_model)); + GaussianFactor::shared_ptr actual2 = constraint.linearize(config2, ordering); + GaussianFactor::shared_ptr expected2(new JacobianFactor(ordering[key], eye(2,2), Vector_(2,-1.0,0.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } @@ -359,16 +359,16 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); Symbol key1('x',1), key2('x',2); double mu = 1000.0; - OrderingOrdered ordering; + Ordering ordering; ordering += key1, key2; eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); Values config1; config1.insert(key1, x1); config1.insert(key2, x2); - GaussianFactorOrdered::shared_ptr actual1 = constraint.linearize(config1, ordering); - GaussianFactorOrdered::shared_ptr expected1( - new JacobianFactorOrdered(ordering[key1], -eye(2,2), ordering[key2], + GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering); + GaussianFactor::shared_ptr expected1( + new JacobianFactor(ordering[key1], -eye(2,2), ordering[key2], eye(2,2), zero(2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); @@ -377,9 +377,9 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { Point2 x2bad(2.0, 2.0); config2.insert(key1, x1bad); config2.insert(key2, x2bad); - GaussianFactorOrdered::shared_ptr actual2 = constraint.linearize(config2, ordering); - GaussianFactorOrdered::shared_ptr expected2( - new JacobianFactorOrdered(ordering[key1], -eye(2,2), ordering[key2], + GaussianFactor::shared_ptr actual2 = constraint.linearize(config2, ordering); + GaussianFactor::shared_ptr expected2( + new JacobianFactor(ordering[key1], -eye(2,2), ordering[key2], eye(2,2), Vector_(2, 1.0, 1.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index bb9a52942..c490cbc6d 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -112,10 +112,10 @@ TEST( NonlinearFactor, linearize_f1 ) Graph::sharedFactor nlf = nfg[0]; // We linearize at noisy config from SmallExample - GaussianFactorOrdered::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); + GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); - GaussianFactorGraphOrdered lfg = createGaussianFactorGraph(*c.orderingArbitrary()); - GaussianFactorOrdered::shared_ptr expected = lfg[0]; + GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); + GaussianFactor::shared_ptr expected = lfg[0]; CHECK(assert_equal(*expected,*actual)); @@ -134,10 +134,10 @@ TEST( NonlinearFactor, linearize_f2 ) Graph::sharedFactor nlf = nfg[1]; // We linearize at noisy config from SmallExample - GaussianFactorOrdered::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); + GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); - GaussianFactorGraphOrdered lfg = createGaussianFactorGraph(*c.orderingArbitrary()); - GaussianFactorOrdered::shared_ptr expected = lfg[1]; + GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); + GaussianFactor::shared_ptr expected = lfg[1]; CHECK(assert_equal(*expected,*actual)); } @@ -151,10 +151,10 @@ TEST( NonlinearFactor, linearize_f3 ) // We linearize at noisy config from SmallExample Values c = createNoisyValues(); - GaussianFactorOrdered::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); + GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); - GaussianFactorGraphOrdered lfg = createGaussianFactorGraph(*c.orderingArbitrary()); - GaussianFactorOrdered::shared_ptr expected = lfg[2]; + GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); + GaussianFactor::shared_ptr expected = lfg[2]; CHECK(assert_equal(*expected,*actual)); } @@ -168,10 +168,10 @@ TEST( NonlinearFactor, linearize_f4 ) // We linearize at noisy config from SmallExample Values c = createNoisyValues(); - GaussianFactorOrdered::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); + GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); - GaussianFactorGraphOrdered lfg = createGaussianFactorGraph(*c.orderingArbitrary()); - GaussianFactorOrdered::shared_ptr expected = lfg[3]; + GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); + GaussianFactor::shared_ptr expected = lfg[3]; CHECK(assert_equal(*expected,*actual)); } @@ -205,13 +205,13 @@ TEST( NonlinearFactor, linearize_constraint1 ) Values config; config.insert(X(1), Point2(1.0, 2.0)); - GaussianFactorOrdered::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary()); + GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary()); // create expected - OrderingOrdered ord(*config.orderingArbitrary()); + Ordering ord(*config.orderingArbitrary()); Vector b = Vector_(2, 0., -3.); - JacobianFactorOrdered expected(ord[X(1)], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint); - CHECK(assert_equal((const GaussianFactorOrdered&)expected, *actual)); + JacobianFactor expected(ord[X(1)], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint); + CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } /* ************************************************************************* */ @@ -226,14 +226,14 @@ TEST( NonlinearFactor, linearize_constraint2 ) Values config; config.insert(X(1), Point2(1.0, 2.0)); config.insert(L(1), Point2(5.0, 4.0)); - GaussianFactorOrdered::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary()); + GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary()); // create expected - OrderingOrdered ord(*config.orderingArbitrary()); + Ordering ord(*config.orderingArbitrary()); Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0); Vector b = Vector_(2, -15., -3.); - JacobianFactorOrdered expected(ord[X(1)], -1*A, ord[L(1)], A, b, constraint); - CHECK(assert_equal((const GaussianFactorOrdered&)expected, *actual)); + JacobianFactor expected(ord[X(1)], -1*A, ord[L(1)], A, b, constraint); + CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } /* ************************************************************************* */ @@ -272,8 +272,8 @@ TEST(NonlinearFactor, NoiseModelFactor4) { tv.insert(X(4), LieVector(1, 4.0)); EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); - OrderingOrdered ordering; ordering += X(1), X(2), X(3), X(4); - JacobianFactorOrdered jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); + Ordering ordering; ordering += X(1), X(2), X(3), X(4); + JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[1], 1); LONGS_EQUAL(jf.keys()[2], 2); @@ -320,8 +320,8 @@ TEST(NonlinearFactor, NoiseModelFactor5) { tv.insert(X(5), LieVector(1, 5.0)); EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); - OrderingOrdered ordering; ordering += X(1), X(2), X(3), X(4), X(5); - JacobianFactorOrdered jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); + Ordering ordering; ordering += X(1), X(2), X(3), X(4), X(5); + JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[1], 1); LONGS_EQUAL(jf.keys()[2], 2); @@ -374,8 +374,8 @@ TEST(NonlinearFactor, NoiseModelFactor6) { tv.insert(X(6), LieVector(1, 6.0)); EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); - OrderingOrdered ordering; ordering += X(1), X(2), X(3), X(4), X(5), X(6); - JacobianFactorOrdered jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); + Ordering ordering; ordering += X(1), X(2), X(3), X(4), X(5), X(6); + JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[1], 1); LONGS_EQUAL(jf.keys()[2], 2); diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index a3e792055..5a35cbd94 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -76,20 +76,20 @@ TEST( Graph, keys ) /* ************************************************************************* */ TEST( Graph, GET_ORDERING) { -// OrderingOrdered expected; expected += "x1","l1","x2"; // For starting with x1,x2,l1 - OrderingOrdered expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2 +// Ordering expected; expected += "x1","l1","x2"; // For starting with x1,x2,l1 + Ordering expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2 Graph nlfg = createNonlinearFactorGraph(); - SymbolicFactorGraphOrdered::shared_ptr symbolic; - OrderingOrdered::shared_ptr ordering; + SymbolicFactorGraph::shared_ptr symbolic; + Ordering::shared_ptr ordering; boost::tie(symbolic, ordering) = nlfg.symbolic(createNoisyValues()); - OrderingOrdered actual = *nlfg.orderingCOLAMD(createNoisyValues()); + Ordering actual = *nlfg.orderingCOLAMD(createNoisyValues()); EXPECT(assert_equal(expected,actual)); // Constrained ordering - put x2 at the end std::map constraints; constraints[X(2)] = 1; - OrderingOrdered actualConstrained = *nlfg.orderingCOLAMDConstrained(createNoisyValues(), constraints); - OrderingOrdered expectedConstrained; expectedConstrained += L(1), X(1), X(2); + Ordering actualConstrained = *nlfg.orderingCOLAMDConstrained(createNoisyValues(), constraints); + Ordering expectedConstrained; expectedConstrained += L(1), X(1), X(2); EXPECT(assert_equal(expectedConstrained, actualConstrained)); } @@ -110,8 +110,8 @@ TEST( Graph, linearize ) { Graph fg = createNonlinearFactorGraph(); Values initial = createNoisyValues(); - boost::shared_ptr > linearized = fg.linearize(initial, *initial.orderingArbitrary()); - FactorGraphOrdered expected = createGaussianFactorGraph(*initial.orderingArbitrary()); + boost::shared_ptr > linearized = fg.linearize(initial, *initial.orderingArbitrary()); + FactorGraph expected = createGaussianFactorGraph(*initial.orderingArbitrary()); CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations } diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 7ee6fd438..b41fa95ae 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -50,7 +50,7 @@ TEST(testNonlinearISAM, markov_chain ) { // perform a check on changing orderings if (i == 5) { - OrderingOrdered ordering = isam.getOrdering(); + Ordering ordering = isam.getOrdering(); // swap last two elements Key last = ordering.pop_back().first; @@ -59,7 +59,7 @@ TEST(testNonlinearISAM, markov_chain ) { ordering.push_back(secondLast); isam.setOrdering(ordering); - OrderingOrdered expected; expected += (0), (1), (2), (4), (3); + Ordering expected; expected += (0), (1), (2), (4), (3); EXPECT(assert_equal(expected, isam.getOrdering())); } diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 418ec0bb0..aff07c15b 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -89,7 +89,7 @@ TEST( NonlinearOptimizer, optimize ) DOUBLES_EQUAL(199.0,fg.error(c0),1e-3); // optimize parameters - OrderingOrdered ord; + Ordering ord; ord.push_back(X(1)); // Gauss-Newton @@ -182,7 +182,7 @@ TEST( NonlinearOptimizer, Factorization ) graph.add(PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10))); graph.add(BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1))); - OrderingOrdered ordering; + Ordering ordering; ordering.push_back(X(1)); ordering.push_back(X(2)); @@ -216,7 +216,7 @@ TEST(NonlinearOptimizer, NullFactor) { DOUBLES_EQUAL(199.0,fg.error(c0),1e-3); // optimize parameters - OrderingOrdered ord; + Ordering ord; ord.push_back(X(1)); // Gauss-Newton diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 2214ee23d..57a1e5cb3 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -164,8 +164,8 @@ BOOST_CLASS_EXPORT(gtsam::StereoCamera); /* Create GUIDs for factors */ /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactorOrdered, "gtsam::JacobianFactorOrdered"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactorOrdered , "gtsam::HessianFactorOrdered"); +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector"); BOOST_CLASS_EXPORT_GUID(PriorFactorLieMatrix, "gtsam::PriorFactorLieMatrix"); @@ -234,8 +234,8 @@ BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); TEST (testSerializationSLAM, smallExample_linear) { using namespace example; - OrderingOrdered ordering; ordering += X(1),X(2),L(1); - GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering); + Ordering ordering; ordering += X(1),X(2),L(1); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); EXPECT(equalsObj(ordering)); EXPECT(equalsXML(ordering)); EXPECT(equalsBinary(ordering)); @@ -244,7 +244,7 @@ TEST (testSerializationSLAM, smallExample_linear) { EXPECT(equalsXML(fg)); EXPECT(equalsBinary(fg)); - GaussianBayesNetOrdered cbn = createSmallGaussianBayesNet(); + GaussianBayesNet cbn = createSmallGaussianBayesNet(); EXPECT(equalsObj(cbn)); EXPECT(equalsXML(cbn)); EXPECT(equalsBinary(cbn)); @@ -253,11 +253,11 @@ TEST (testSerializationSLAM, smallExample_linear) { /* ************************************************************************* */ TEST (testSerializationSLAM, gaussianISAM) { using namespace example; - OrderingOrdered ordering; - GaussianFactorGraphOrdered smoother; + Ordering ordering; + GaussianFactorGraph smoother; boost::tie(smoother, ordering) = createSmoother(7); - BayesTreeOrdered bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); - GaussianISAMOrdered isam(bayesTree); + BayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); + GaussianISAM isam(bayesTree); EXPECT(equalsObj(isam)); EXPECT(equalsXML(isam)); diff --git a/tests/testSimulated2DOriented.cpp b/tests/testSimulated2DOriented.cpp index 29b7697fa..05d64653d 100644 --- a/tests/testSimulated2DOriented.cpp +++ b/tests/testSimulated2DOriented.cpp @@ -64,7 +64,7 @@ TEST( simulated2DOriented, constructor ) simulated2DOriented::Values config; config.insert(X(1), Pose2(1., 0., 0.2)); config.insert(X(2), Pose2(2., 0., 0.1)); - boost::shared_ptr lf = factor.linearize(config, *config.orderingArbitrary()); + boost::shared_ptr lf = factor.linearize(config, *config.orderingArbitrary()); } /* ************************************************************************* */ diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index 0252ea909..53ba92331 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -44,7 +44,7 @@ Symbol key(int x, int y) { /* ************************************************************************* */ TEST( SubgraphPreconditioner, planarOrdering ) { // Check canonical ordering - OrderingOrdered expected, ordering = planarOrdering(3); + Ordering expected, ordering = planarOrdering(3); expected += key(3, 3), key(2, 3), key(1, 3), key(3, 2), key(2, 2), key(1, 2), @@ -54,9 +54,9 @@ TEST( SubgraphPreconditioner, planarOrdering ) { /* ************************************************************************* */ /** unnormalized error */ -static double error(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x) { +static double error(const GaussianFactorGraph& fg, const VectorValues& x) { double total_error = 0.; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, fg) + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg) total_error += factor->error(x); return total_error; } @@ -65,16 +65,16 @@ static double error(const GaussianFactorGraphOrdered& fg, const VectorValuesOrde TEST( SubgraphPreconditioner, planarGraph ) { // Check planar graph construction - GaussianFactorGraphOrdered A; - VectorValuesOrdered xtrue; + GaussianFactorGraph A; + VectorValues xtrue; boost::tie(A, xtrue) = planarGraph(3); LONGS_EQUAL(13,A.size()); LONGS_EQUAL(9,xtrue.size()); DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue // Check that xtrue is optimal - GaussianBayesNetOrdered::shared_ptr R1 = GaussianSequentialSolver(A).eliminate(); - VectorValuesOrdered actual = optimize(*R1); + GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate(); + VectorValues actual = optimize(*R1); CHECK(assert_equal(xtrue,actual)); } @@ -82,19 +82,19 @@ TEST( SubgraphPreconditioner, planarGraph ) TEST( SubgraphPreconditioner, splitOffPlanarTree ) { // Build a planar graph - GaussianFactorGraphOrdered A; - VectorValuesOrdered xtrue; + GaussianFactorGraph A; + VectorValues xtrue; boost::tie(A, xtrue) = planarGraph(3); // Get the spanning tree and constraints, and check their sizes - GaussianFactorGraphOrdered T, C; + GaussianFactorGraph T, C; boost::tie(T, C) = splitOffPlanarTree(3, A); LONGS_EQUAL(9,T.size()); LONGS_EQUAL(4,C.size()); // Check that the tree can be solved to give the ground xtrue - GaussianBayesNetOrdered::shared_ptr R1 = GaussianSequentialSolver(T).eliminate(); - VectorValuesOrdered xbar = optimize(*R1); + GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate(); + VectorValues xbar = optimize(*R1); CHECK(assert_equal(xtrue,xbar)); } @@ -103,37 +103,37 @@ TEST( SubgraphPreconditioner, splitOffPlanarTree ) TEST( SubgraphPreconditioner, system ) { // Build a planar graph - GaussianFactorGraphOrdered Ab; - VectorValuesOrdered xtrue; + GaussianFactorGraph Ab; + VectorValues xtrue; size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b // Get the spanning tree and corresponding ordering - GaussianFactorGraphOrdered Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraphOrdered(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraphOrdered(Ab2_)); + SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); + SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); // Eliminate the spanning tree to build a prior SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 - VectorValuesOrdered xbar = optimize(*Rc1); // xbar = inv(R1)*c1 + VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 // Create Subgraph-preconditioned system - VectorValuesOrdered::shared_ptr xbarShared(new VectorValuesOrdered(xbar)); // TODO: horrible + VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible SubgraphPreconditioner system(Ab2, Rc1, xbarShared); // Create zero config - VectorValuesOrdered zeros = VectorValuesOrdered::Zero(xbar); + VectorValues zeros = VectorValues::Zero(xbar); // Set up y0 as all zeros - VectorValuesOrdered y0 = zeros; + VectorValues y0 = zeros; // y1 = perturbed y0 - VectorValuesOrdered y1 = zeros; + VectorValues y1 = zeros; y1[1] = Vector_(2, 1.0, -1.0); // Check corresponding x values - VectorValuesOrdered expected_x1 = xtrue, x1 = system.x(y1); + VectorValues expected_x1 = xtrue, x1 = system.x(y1); expected_x1[1] = Vector_(2, 2.01, 2.99); expected_x1[0] = Vector_(2, 3.01, 2.99); CHECK(assert_equal(xtrue, system.x(y0))); @@ -146,8 +146,8 @@ TEST( SubgraphPreconditioner, system ) DOUBLES_EQUAL(3,error(system,y1),1e-9); // Test gradient in x - VectorValuesOrdered expected_gx0 = zeros; - VectorValuesOrdered expected_gx1 = zeros; + VectorValues expected_gx0 = zeros; + VectorValues expected_gx1 = zeros; CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); expected_gx1[2] = Vector_(2, -100., 100.); expected_gx1[4] = Vector_(2, -100., 100.); @@ -157,8 +157,8 @@ TEST( SubgraphPreconditioner, system ) CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); // Test gradient in y - VectorValuesOrdered expected_gy0 = zeros; - VectorValuesOrdered expected_gy1 = zeros; + VectorValues expected_gy0 = zeros; + VectorValues expected_gy1 = zeros; expected_gy1[2] = Vector_(2, 2., -2.); expected_gy1[4] = Vector_(2, -2., 2.); expected_gy1[1] = Vector_(2, 3., -3.); @@ -169,7 +169,7 @@ TEST( SubgraphPreconditioner, system ) // Check it numerically for good measure // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) - // Vector numerical_g1 = numericalGradient (error, y1, 0.001); + // Vector numerical_g1 = numericalGradient (error, y1, 0.001); // Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., // 3., -3., 0., 0., -1., 1., 1., -1.); // CHECK(assert_equal(expected_g1,numerical_g1)); @@ -179,41 +179,41 @@ TEST( SubgraphPreconditioner, system ) TEST( SubgraphPreconditioner, conjugateGradients ) { // Build a planar graph - GaussianFactorGraphOrdered Ab; - VectorValuesOrdered xtrue; + GaussianFactorGraph Ab; + VectorValues xtrue; size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b // Get the spanning tree and corresponding ordering - GaussianFactorGraphOrdered Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraphOrdered(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraphOrdered(Ab2_)); + SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); + SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); // Eliminate the spanning tree to build a prior - OrderingOrdered ordering = planarOrdering(N); + Ordering ordering = planarOrdering(N); SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 - VectorValuesOrdered xbar = optimize(*Rc1); // xbar = inv(R1)*c1 + VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 // Create Subgraph-preconditioned system - VectorValuesOrdered::shared_ptr xbarShared(new VectorValuesOrdered(xbar)); // TODO: horrible + VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible SubgraphPreconditioner system(Ab2, Rc1, xbarShared); // Create zero config y0 and perturbed config y1 - VectorValuesOrdered y0 = VectorValuesOrdered::Zero(xbar); + VectorValues y0 = VectorValues::Zero(xbar); - VectorValuesOrdered y1 = y0; + VectorValues y1 = y0; y1[1] = Vector_(2, 1.0, -1.0); - VectorValuesOrdered x1 = system.x(y1); + VectorValues x1 = system.x(y1); // Solve for the remaining constraints using PCG ConjugateGradientParameters parameters; - VectorValuesOrdered actual = conjugateGradients(system, y1, parameters); + VectorValues actual = conjugateGradients(system, y1, parameters); CHECK(assert_equal(y0,actual)); // Compare with non preconditioned version: - VectorValuesOrdered actual2 = conjugateGradientDescent(Ab, x1, parameters); + VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters); CHECK(assert_equal(xtrue,actual2,1e-4)); } diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index 0b8f74922..700f35261 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include @@ -38,9 +38,9 @@ using namespace example; /* ************************************************************************* */ /** unnormalized error */ -static double error(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x) { +static double error(const GaussianFactorGraph& fg, const VectorValues& x) { double total_error = 0.; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, fg) + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg) total_error += factor->error(x); return total_error; } @@ -50,8 +50,8 @@ static double error(const GaussianFactorGraphOrdered& fg, const VectorValuesOrde TEST( SubgraphSolver, constructor1 ) { // Build a planar graph - GaussianFactorGraphOrdered Ab; - VectorValuesOrdered xtrue; + GaussianFactorGraph Ab; + VectorValues xtrue; size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b @@ -59,7 +59,7 @@ TEST( SubgraphSolver, constructor1 ) // and it will split the graph into A1 and A2, where A1 is a spanning tree SubgraphSolverParameters parameters; SubgraphSolver solver(Ab, parameters); - VectorValuesOrdered optimized = solver.optimize(); // does PCG optimization + VectorValues optimized = solver.optimize(); // does PCG optimization DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } @@ -67,20 +67,20 @@ TEST( SubgraphSolver, constructor1 ) TEST( SubgraphSolver, constructor2 ) { // Build a planar graph - GaussianFactorGraphOrdered Ab; - VectorValuesOrdered xtrue; + GaussianFactorGraph Ab; + VectorValues xtrue; size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b // Get the spanning tree and corresponding ordering - GaussianFactorGraphOrdered Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); // The second constructor takes two factor graphs, // so the caller can specify the preconditioner (Ab1) and the constraints that are left out (Ab2) SubgraphSolverParameters parameters; SubgraphSolver solver(Ab1_, Ab2_, parameters); - VectorValuesOrdered optimized = solver.optimize(); + VectorValues optimized = solver.optimize(); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } @@ -88,24 +88,24 @@ TEST( SubgraphSolver, constructor2 ) TEST( SubgraphSolver, constructor3 ) { // Build a planar graph - GaussianFactorGraphOrdered Ab; - VectorValuesOrdered xtrue; + GaussianFactorGraph Ab; + VectorValues xtrue; size_t N = 3; boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b // Get the spanning tree and corresponding ordering - GaussianFactorGraphOrdered Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2 via QR factorization, where R1 is square UT - GaussianBayesNetOrdered::shared_ptr Rc1 = // - EliminationTreeOrdered::Create(Ab1_)->eliminate(&EliminateQROrdered); + GaussianBayesNet::shared_ptr Rc1 = // + EliminationTree::Create(Ab1_)->eliminate(&EliminateQR); // The third constructor allows the caller to pass an already solved preconditioner Rc1_ // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before SubgraphSolverParameters parameters; SubgraphSolver solver(Rc1, Ab2_, parameters); - VectorValuesOrdered optimized = solver.optimize(); + VectorValues optimized = solver.optimize(); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } diff --git a/tests/testSummarization.cpp b/tests/testSummarization.cpp index b342507fc..bf7342202 100644 --- a/tests/testSummarization.cpp +++ b/tests/testSummarization.cpp @@ -74,15 +74,15 @@ TEST( testSummarization, example_from_ddf1 ) { { // Summarize to a linear system - GaussianFactorGraphOrdered actLinGraph; OrderingOrdered actOrdering; + GaussianFactorGraph actLinGraph; Ordering actOrdering; SummarizationMode mode = PARTIAL_QR; boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); - OrderingOrdered expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; + Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; EXPECT(assert_equal(expSumOrdering, actOrdering)); // Does not split out subfactors where possible - GaussianFactorGraphOrdered expLinGraph; + GaussianFactorGraph expLinGraph; expLinGraph.add( expSumOrdering[lA3], Matrix_(4,2, @@ -108,16 +108,16 @@ TEST( testSummarization, example_from_ddf1 ) { { // Summarize to a linear system using cholesky - compare to previous version - GaussianFactorGraphOrdered actLinGraph; OrderingOrdered actOrdering; + GaussianFactorGraph actLinGraph; Ordering actOrdering; SummarizationMode mode = PARTIAL_CHOLESKY; boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); - OrderingOrdered expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; + Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; EXPECT(assert_equal(expSumOrdering, actOrdering)); // Does not split out subfactors where possible - GaussianFactorGraphOrdered expLinGraph; - expLinGraph.add(HessianFactorOrdered(JacobianFactorOrdered( + GaussianFactorGraph expLinGraph; + expLinGraph.add(HessianFactor(JacobianFactor( expSumOrdering[lA3], Matrix_(4,2, 0.595867, 0.605092, @@ -142,15 +142,15 @@ TEST( testSummarization, example_from_ddf1 ) { { // Summarize to a linear system with joint factor graph version - GaussianFactorGraphOrdered actLinGraph; OrderingOrdered actOrdering; + GaussianFactorGraph actLinGraph; Ordering actOrdering; SummarizationMode mode = SEQUENTIAL_QR; boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); - OrderingOrdered expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; + Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; EXPECT(assert_equal(expSumOrdering, actOrdering)); // Does not split out subfactors where possible - GaussianFactorGraphOrdered expLinGraph; + GaussianFactorGraph expLinGraph; expLinGraph.add( expSumOrdering[lA3], Matrix_(2,2, @@ -180,15 +180,15 @@ TEST( testSummarization, example_from_ddf1 ) { { // Summarize to a linear system with joint factor graph version - GaussianFactorGraphOrdered actLinGraph; OrderingOrdered actOrdering; + GaussianFactorGraph actLinGraph; Ordering actOrdering; SummarizationMode mode = SEQUENTIAL_CHOLESKY; boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); - OrderingOrdered expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; + Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; EXPECT(assert_equal(expSumOrdering, actOrdering)); // Does not split out subfactors where possible - GaussianFactorGraphOrdered expLinGraph; + GaussianFactorGraph expLinGraph; expLinGraph.add( expSumOrdering[lA3], Matrix_(2,2, @@ -229,10 +229,10 @@ TEST( testSummarization, no_summarize_case ) { values.insert(key, Pose2(0.0, 0.0, 0.1)); SummarizationMode mode = SEQUENTIAL_CHOLESKY; - GaussianFactorGraphOrdered actLinGraph; OrderingOrdered actOrdering; + GaussianFactorGraph actLinGraph; Ordering actOrdering; boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); - OrderingOrdered expOrdering; expOrdering += key; - GaussianFactorGraphOrdered expLinGraph = *graph.linearize(values, expOrdering); + Ordering expOrdering; expOrdering += key; + GaussianFactorGraph expLinGraph = *graph.linearize(values, expOrdering); EXPECT(assert_equal(expOrdering, actOrdering)); EXPECT(assert_equal(expLinGraph, actLinGraph)); } diff --git a/tests/timeGaussianFactorGraph.cpp b/tests/timeGaussianFactorGraph.cpp index 6491e221d..f7dd8601d 100644 --- a/tests/timeGaussianFactorGraph.cpp +++ b/tests/timeGaussianFactorGraph.cpp @@ -30,8 +30,8 @@ using namespace boost::assign; /* ************************************************************************* */ // Create a Kalman smoother for t=1:T and optimize double timeKalmanSmoother(int T) { - pair smoother_ordering = createSmoother(T); - GaussianFactorGraphOrdered& smoother(smoother_ordering.first); + pair smoother_ordering = createSmoother(T); + GaussianFactorGraph& smoother(smoother_ordering.first); clock_t start = clock(); GaussianSequentialSolver(smoother).optimize(); clock_t end = clock (); @@ -43,8 +43,8 @@ double timeKalmanSmoother(int T) { // Create a planar factor graph and optimize // todo: use COLAMD ordering again (removed when linear baked-in ordering added) double timePlanarSmoother(int N, bool old = true) { - boost::tuple pg = planarGraph(N); - GaussianFactorGraphOrdered& fg(pg.get<0>()); + boost::tuple pg = planarGraph(N); + GaussianFactorGraph& fg(pg.get<0>()); clock_t start = clock(); GaussianSequentialSolver(fg).optimize(); clock_t end = clock (); @@ -56,8 +56,8 @@ double timePlanarSmoother(int N, bool old = true) { // Create a planar factor graph and eliminate // todo: use COLAMD ordering again (removed when linear baked-in ordering added) double timePlanarSmootherEliminate(int N, bool old = true) { - boost::tuple pg = planarGraph(N); - GaussianFactorGraphOrdered& fg(pg.get<0>()); + boost::tuple pg = planarGraph(N); + GaussianFactorGraph& fg(pg.get<0>()); clock_t start = clock(); GaussianSequentialSolver(fg).eliminate(); clock_t end = clock (); @@ -69,23 +69,23 @@ double timePlanarSmootherEliminate(int N, bool old = true) { //// Create a planar factor graph and join factors until matrix formation //// This variation uses the original join factors approach //double timePlanarSmootherJoinAug(int N, size_t reps) { -// GaussianFactorGraphOrdered fgBase; -// VectorValuesOrdered config; +// GaussianFactorGraph fgBase; +// VectorValues config; // boost::tie(fgBase,config) = planarGraph(N); -// OrderingOrdered ordering = fgBase.getOrdering(); +// Ordering ordering = fgBase.getOrdering(); // Symbol key = ordering.front(); // // clock_t start = clock(); // // for (size_t i = 0; ikeys()) // if (k != key) render += k; @@ -102,16 +102,16 @@ double timePlanarSmootherEliminate(int N, bool old = true) { //// Create a planar factor graph and join factors until matrix formation //// This variation uses the single-allocate version to create the matrix //double timePlanarSmootherCombined(int N, size_t reps) { -// GaussianFactorGraphOrdered fgBase; -// VectorValuesOrdered config; +// GaussianFactorGraph fgBase; +// VectorValues config; // boost::tie(fgBase,config) = planarGraph(N); -// OrderingOrdered ordering = fgBase.getOrdering(); +// Ordering ordering = fgBase.getOrdering(); // Symbol key = ordering.front(); // // clock_t start = clock(); // // for (size_t i = 0; i