diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 7eda5f175..99455b08a 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -69,7 +69,7 @@ namespace gtsam { /** Add a factor */ template - void push_back(const boost::shared_ptr& factor); + void push_back(const boost::shared_ptr& factor) { factors_.push_back(sharedFactor(factor)); } /** push back many factors */ void push_back(const FactorGraph& factors); @@ -198,10 +198,7 @@ namespace gtsam { FactorGraph::FactorGraph(const FactorGraph& factorGraph) { factors_.reserve(factorGraph.size()); BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& factor, factorGraph) { - if(factor) - this->push_back(factor); - else - this->push_back(sharedFactor()); + this->push_back(factor); } } @@ -215,17 +212,6 @@ namespace gtsam { } } - /* ************************************************************************* */ - template - template - inline void FactorGraph::push_back(const boost::shared_ptr& factor) { -#ifndef NDEBUG - factors_.push_back(boost::dynamic_pointer_cast(factor)); // add the actual factor -#else - factors_.push_back(boost::static_pointer_cast(factor)); -#endif - } - /* ************************************************************************* */ template template diff --git a/gtsam/linear/iterative.cpp b/gtsam/linear/iterative.cpp index 29d5cab12..cbc1a1402 100644 --- a/gtsam/linear/iterative.cpp +++ b/gtsam/linear/iterative.cpp @@ -56,12 +56,12 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeOptimizationParameters & parameters) { - return conjugateGradients (fg, x, parameters, true); + VectorValues steepestDescent(const FactorGraph& fg, const VectorValues& x, const IterativeOptimizationParameters & parameters) { + return conjugateGradients, VectorValues, Errors> (fg, x, parameters, true); } - VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeOptimizationParameters & parameters) { - return conjugateGradients (fg, x, parameters); + VectorValues conjugateGradientDescent(const FactorGraph& fg, const VectorValues& x, const IterativeOptimizationParameters & parameters) { + return conjugateGradients, VectorValues, Errors> (fg, x, parameters); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianFactor.cpp b/gtsam/linear/tests/testGaussianFactor.cpp index f6b458448..185e0fa93 100644 --- a/gtsam/linear/tests/testGaussianFactor.cpp +++ b/gtsam/linear/tests/testGaussianFactor.cpp @@ -174,7 +174,7 @@ TEST(GaussianFactor, Combine2) gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - JacobianFactor actual = *JacobianFactor::Combine(gfg, VariableSlots(gfg)); + JacobianFactor actual = *JacobianFactor::Combine(*gfg.dynamicCastFactors >(), VariableSlots(gfg)); Matrix zero3x3 = zeros(3,3); Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); @@ -516,7 +516,7 @@ TEST(GaussianFactor, eliminateFrontals) factors.push_back(factor4); // Create combined factor - JacobianFactor combined(*JacobianFactor::Combine(factors, VariableSlots(factors))); + JacobianFactor combined(*JacobianFactor::Combine(*factors.dynamicCastFactors >(), VariableSlots(factors))); // Copies factors as they will be eliminated in place JacobianFactor actualFactor_QR = combined; diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index 26bac00fc..6a8339c8e 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -163,7 +163,7 @@ namespace example { else fg.add(ordering["l1"], 5*eye(2), ordering["x2"], -5*eye(2), Vector_(2, -1.0, 1.5), unit2); - return fg; + return *fg.dynamicCastFactors >(); } /* ************************************************************************* */ @@ -290,7 +290,7 @@ namespace example { Vector b1(2); b1(0) = 1.0; b1(1) = -1.0; - GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); + JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); // create binary constraint factor // between _x_ and _y_, that is going to be the only factor on _y_ @@ -299,7 +299,7 @@ namespace example { Matrix Ax1 = eye(2); Matrix Ay1 = eye(2) * -1; Vector b2 = Vector_(2, 0.0, 0.0); - GaussianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, + JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); // construct the graph @@ -328,7 +328,7 @@ namespace example { b1(0) = 1.0; b1(1) = -1.0; //GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1)); - GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); + JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); // create binary constraint factor // between _x_ and _y_, that is going to be the only factor on _y_ @@ -341,7 +341,7 @@ namespace example { Ax1(1, 1) = 1.0; Matrix Ay1 = eye(2) * 10; Vector b2 = Vector_(2, 1.0, 2.0); - GaussianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, + JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); // construct the graph @@ -365,7 +365,7 @@ namespace example { // unary factor 1 Matrix A = eye(2); Vector b = Vector_(2, -2.0, 2.0); - GaussianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1)); + JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1)); // constraint 1 Matrix A11(2, 2); @@ -383,7 +383,7 @@ namespace example { Vector b1(2); b1(0) = 1.0; b1(1) = 2.0; - GaussianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1, + JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1, constraintModel)); // constraint 2 @@ -402,7 +402,7 @@ namespace example { Vector b2(2); b2(0) = 3.0; b2(1) = 4.0; - GaussianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2, + JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2, constraintModel)); // construct the graph diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp index f9815c80c..a0bbf6699 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraph.cpp @@ -691,7 +691,7 @@ TEST( GaussianFactorGraph, multiplication ) // create an ordering Ordering ord; ord += "x2","l1","x1"; - GaussianFactorGraph A = createGaussianFactorGraph(ord); + FactorGraph A = createGaussianFactorGraph(ord); VectorValues x = createCorrectDelta(ord); Errors actual = A * x; Errors expected;