diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index a1a14b01c..89662ffc4 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -200,39 +200,6 @@ template class BayesTree; /** return the number valid factors */ size_t nrFactors() const; - /** dynamic_cast the factor pointers down or up the class hierarchy */ - template - typename RELATED::shared_ptr dynamicCastFactors() const { - typename RELATED::shared_ptr ret(new RELATED); - ret->reserve(this->size()); - BOOST_FOREACH(const sharedFactor& factor, *this) { - typename RELATED::FactorType::shared_ptr castedFactor(boost::dynamic_pointer_cast(factor)); - if(castedFactor) - ret->push_back(castedFactor); - else - throw std::invalid_argument("In FactorGraph::dynamic_factor_cast(), dynamic_cast failed, meaning an invalid cast was requested."); - } - return ret; - } - - /** - * dynamic_cast factor pointers if possible, otherwise convert with a - * constructor of the target type. - */ - template - typename TARGET::shared_ptr convertCastFactors() const { - typename TARGET::shared_ptr ret(new TARGET); - ret->reserve(this->size()); - BOOST_FOREACH(const sharedFactor& factor, *this) { - typename TARGET::FactorType::shared_ptr castedFactor(boost::dynamic_pointer_cast(factor)); - if(castedFactor) - ret->push_back(castedFactor); - else - ret->push_back(typename TARGET::FactorType::shared_ptr(new typename TARGET::FactorType(*factor))); - } - return ret; - } - private: /** Serialization function */ diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp index 8669329bb..4743d1102 100644 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ b/gtsam/inference/tests/testFactorGraph.cpp @@ -91,12 +91,6 @@ typedef boost::shared_ptr shared; // CHECK(singletonGraph_excepted.equals(singletonGraph)); //} -/* ************************************************************************* */ -TEST(FactorGraph, dynamic_factor_cast) { - FactorGraph fg; - fg.dynamicCastFactors >(); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 2f91a6115..0dc8d771a 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -80,7 +80,7 @@ public: GaussianConditional(); /** constructor */ - GaussianConditional(Index key); + explicit GaussianConditional(Index key); /** constructor with no parents * |Rx-d| diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 853cf26ba..cd436a0b2 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -160,9 +160,18 @@ namespace gtsam { /* ************************************************************************* */ Matrix GaussianFactorGraph::denseJacobian() const { + // Convert to Jacobians + FactorGraph jfg; + jfg.reserve(this->size()); + BOOST_FOREACH(const sharedFactor& factor, *this) { + if(boost::shared_ptr jf = + boost::dynamic_pointer_cast(factor)) + jfg.push_back(jf); + else + jfg.push_back(boost::make_shared(*factor)); + } // combine all factors - JacobianFactor combined(*CombineJacobians(*convertCastFactors > (), VariableSlots(*this))); + JacobianFactor combined(*CombineJacobians(jfg, VariableSlots(*this))); return combined.matrix_augmented(); } diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 5bbee3708..4be0b950c 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -197,10 +197,10 @@ namespace gtsam { const std::vector& gs, double f); /** Construct from Conditional Gaussian */ - HessianFactor(const GaussianConditional& cg); + explicit HessianFactor(const GaussianConditional& cg); /** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */ - HessianFactor(const GaussianFactor& factor); + explicit HessianFactor(const GaussianFactor& factor); /** Special constructor used in EliminateCholesky which combines the given factors */ HessianFactor(const FactorGraph& factors, diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 677900b01..bdb51701e 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -66,6 +66,18 @@ namespace gtsam { assertInvariants(); } + /* ************************************************************************* */ + JacobianFactor::JacobianFactor(const GaussianFactor& gf) : Ab_(matrix_) { + // Copy the matrix data depending on what type of factor we're copying from + if(const JacobianFactor* rhs = dynamic_cast(&gf)) + *this = JacobianFactor(*rhs); + else if(const HessianFactor* rhs = dynamic_cast(&gf)) + *this = JacobianFactor(*rhs); + else + throw std::invalid_argument("In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor"); + assertInvariants(); + } + /* ************************************************************************* */ JacobianFactor::JacobianFactor() : Ab_(matrix_) { assertInvariants(); } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index d45873c3b..276d11f27 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -102,6 +102,9 @@ namespace gtsam { /** Copy constructor */ JacobianFactor(const JacobianFactor& gf); + /** Convert from other GaussianFactor */ + JacobianFactor(const GaussianFactor& gf); + /** default constructor for I/O */ JacobianFactor(); diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index d2af81a6c..9b37d9a4b 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -52,10 +52,29 @@ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(*Ab1)->eliminate(&EliminateQR); VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); - pc_ = boost::make_shared( - Ab1->dynamicCastFactors >(), - Ab2->dynamicCastFactors >(), - Rc1, xbar); + // Convert or cast Ab1 to JacobianFactors + boost::shared_ptr > Ab1Jacobians = boost::make_shared >(); + Ab1Jacobians->reserve(Ab1->size()); + BOOST_FOREACH(const boost::shared_ptr& factor, *Ab1) { + if(boost::shared_ptr jf = + boost::dynamic_pointer_cast(factor)) + Ab1Jacobians->push_back(jf); + else + Ab1Jacobians->push_back(boost::make_shared(*factor)); + } + + // Convert or cast Ab2 to JacobianFactors + boost::shared_ptr > Ab2Jacobians = boost::make_shared >(); + Ab1Jacobians->reserve(Ab2->size()); + BOOST_FOREACH(const boost::shared_ptr& factor, *Ab2) { + if(boost::shared_ptr jf = + boost::dynamic_pointer_cast(factor)) + Ab2Jacobians->push_back(jf); + else + Ab2Jacobians->push_back(boost::make_shared(*factor)); + } + + pc_ = boost::make_shared(Ab1Jacobians, Ab2Jacobians, Rc1, xbar); } VectorValues SubgraphSolver::optimize() { diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 34009f856..6bb4a6348 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -50,10 +50,8 @@ TEST(GaussianFactorGraph, initialization) { fg.add(0, -5*eye(2), 2, 5*eye(2), Vector_(2, 0.0, 1.0), unit2); fg.add(1, -5*eye(2), 2, 5*eye(2), Vector_(2, -1.0, 1.5), unit2); - FactorGraph graph = *fg.dynamicCastFactors >(); - - EXPECT_LONGS_EQUAL(4, graph.size()); - JacobianFactor factor = *graph[0]; + EXPECT_LONGS_EQUAL(4, fg.size()); + JacobianFactor factor = *boost::dynamic_pointer_cast(fg[0]); // Test sparse, which takes a vector and returns a matrix, used in MATLAB // Note that this the augmented vector and the RHS is in column 7 @@ -155,8 +153,14 @@ TEST(GaussianFactorGraph, 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 = *CombineJacobians(*gfg.dynamicCastFactors > (), VariableSlots(gfg)); + // Convert to Jacobians, inefficient copy of all factors instead of selectively converting only Hessians + FactorGraph jacobians; + BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) { + jacobians.push_back(boost::make_shared(*factor)); + } + + // Combine Jacobians into a single dense factor + JacobianFactor actual = *CombineJacobians(jacobians, VariableSlots(gfg)); Matrix zero3x3 = zeros(3,3); Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); @@ -213,8 +217,7 @@ TEST(GaussianFactor, CombineAndEliminate) GaussianConditional::shared_ptr actualBN; GaussianFactor::shared_ptr actualFactor; - boost::tie(actualBN, actualFactor) = // - EliminateQR(*gfg.dynamicCastFactors > (), 1); + boost::tie(actualBN, actualFactor) = EliminateQR(gfg, 1); JacobianFactor::shared_ptr actualJacobian = boost::dynamic_pointer_cast< JacobianFactor>(actualFactor); @@ -410,9 +413,14 @@ TEST(GaussianFactor, eliminateFrontals) Matrix actualDense = factors.denseJacobian(); EXPECT(assert_equal(2.0 * Ab, actualDense)); + // Convert to Jacobians, inefficient copy of all factors instead of selectively converting only Hessians + FactorGraph jacobians; + BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, factors) { + jacobians.push_back(boost::make_shared(*factor)); + } + // Create combined factor - JacobianFactor combined(*CombineJacobians(*factors.dynamicCastFactors > (), VariableSlots(factors))); + JacobianFactor combined(*CombineJacobians(jacobians, VariableSlots(factors))); // Copies factors as they will be eliminated in place JacobianFactor actualFactor_QR = combined; diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 59873862a..bd76fd1d0 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -414,10 +414,20 @@ TEST_UNSAFE(HessianFactor, CombineAndEliminate) // create expected Hessian after elimination HessianFactor expectedCholeskyFactor(expectedFactor); - GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky( - *gfg.convertCastFactors > (), 1); - HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast< - HessianFactor>(actualCholesky.second); + // Convert all factors to hessians + FactorGraph hessians; + BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) { + if(boost::shared_ptr hf = boost::dynamic_pointer_cast(factor)) + hessians.push_back(hf); + else if(boost::shared_ptr jf = boost::dynamic_pointer_cast(factor)) + hessians.push_back(boost::make_shared(*jf)); + else + CHECK(false); + } + + // Eliminate + GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(gfg, 1); + HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast(actualCholesky.second); EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6)); EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6)); diff --git a/gtsam/nonlinear/GradientDescentOptimizer.cpp b/gtsam/nonlinear/GradientDescentOptimizer.cpp index 2243577c7..4a471de4d 100644 --- a/gtsam/nonlinear/GradientDescentOptimizer.cpp +++ b/gtsam/nonlinear/GradientDescentOptimizer.cpp @@ -25,10 +25,16 @@ void gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values, cons // Linearize graph GaussianFactorGraph::shared_ptr linear = nfg.linearize(values, ordering); - const FactorGraph::shared_ptr jfg = linear->dynamicCastFactors >(); + FactorGraph jfg; jfg.reserve(linear->size()); + BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, *linear) { + if(boost::shared_ptr jf = boost::dynamic_pointer_cast(factor)) + jfg.push_back((jf)); + else + jfg.push_back(boost::make_shared(*factor)); + } // compute the gradient direction - gradientAtZero(*jfg, g); + gradientAtZero(jfg, g); } diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp index 5f95a0609..a1700beca 100644 --- a/gtsam/nonlinear/NonlinearISAM.cpp +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -58,8 +58,7 @@ void NonlinearISAM::update(const NonlinearFactorGraph& newFactors, BOOST_FOREACH(Key key, factor->keys()) ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present - boost::shared_ptr linearizedNewFactors( - newFactors.linearize(linPoint_, ordering_)->dynamicCastFactors()); + boost::shared_ptr linearizedNewFactors = newFactors.linearize(linPoint_, ordering_); // Update ISAM isam_.update(*linearizedNewFactors); @@ -81,8 +80,7 @@ void NonlinearISAM::reorder_relinearize() { ordering_ = *factors_.orderingCOLAMD(newLinPoint); // Create a linear factor graph at the new linearization point - boost::shared_ptr gfg( - factors_.linearize(newLinPoint, ordering_)->dynamicCastFactors()); + boost::shared_ptr gfg = factors_.linearize(newLinPoint, ordering_); // Just recreate the whole BayesTree isam_.update(*gfg); diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp index c5d77faa3..81a15290c 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -174,12 +174,10 @@ TEST( planarSLAM, constructor ) Vector expected2 = Vector_(1, -0.1); Vector expected3 = Vector_(1, 0.22); // Get NoiseModelFactors - FactorGraph GNM = - *G.dynamicCastFactors >(); - EXPECT(assert_equal(expected0, GNM[0]->unwhitenedError(c))); - EXPECT(assert_equal(expected1, GNM[1]->unwhitenedError(c))); - EXPECT(assert_equal(expected2, GNM[2]->unwhitenedError(c))); - EXPECT(assert_equal(expected3, GNM[3]->unwhitenedError(c))); + EXPECT(assert_equal(expected0, boost::dynamic_pointer_cast(G[0])->unwhitenedError(c))); + EXPECT(assert_equal(expected1, boost::dynamic_pointer_cast(G[1])->unwhitenedError(c))); + EXPECT(assert_equal(expected2, boost::dynamic_pointer_cast(G[2])->unwhitenedError(c))); + EXPECT(assert_equal(expected3, boost::dynamic_pointer_cast(G[3])->unwhitenedError(c))); } /* ************************************************************************* */ diff --git a/tests/smallExample.cpp b/tests/smallExample.cpp index 3dd0324dc..b0af6c723 100644 --- a/tests/smallExample.cpp +++ b/tests/smallExample.cpp @@ -138,23 +138,23 @@ namespace example { /* ************************************************************************* */ JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { // Create empty graph - GaussianFactorGraph fg; + JacobianFactorGraph fg; SharedDiagonal unit2 = noiseModel::Unit::Create(2); // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg.add(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2); + fg.push_back(boost::make_shared(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2)); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg.add(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2); + fg.push_back(boost::make_shared(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2)); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg.add(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2); + fg.push_back(boost::make_shared(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2)); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg.add(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2); + fg.push_back(boost::make_shared(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2)); - return *fg.dynamicCastFactors(); + return fg; } /* ************************************************************************* */