From 0550932235a2266b1f724541565651f70bd2d93b Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Wed, 4 Mar 2015 11:12:16 +0100 Subject: [PATCH 01/11] fixed sparseJacobian() to use a std::map such that it works with symbol keys --- gtsam/linear/GaussianFactorGraph.cpp | 34 ++++++++++++++++------------ 1 file changed, 20 insertions(+), 14 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 6953d2969..cd60a3eb9 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -123,26 +123,32 @@ namespace gtsam { /* ************************************************************************* */ vector > GaussianFactorGraph::sparseJacobian() const { // First find dimensions of each variable - vector dims; + typedef std::map KeySizeMap; + KeySizeMap dims; BOOST_FOREACH(const sharedFactor& factor, *this) { - for (GaussianFactor::const_iterator pos = factor->begin(); - pos != factor->end(); ++pos) { - if (dims.size() <= *pos) - dims.resize(*pos + 1, 0); - dims[*pos] = factor->getDim(pos); + if (!static_cast(factor)) continue; + + for (GaussianFactor::const_iterator key = factor->begin(); + key != factor->end(); ++key) { + dims[*key] = factor->getDim(key); } } // Compute first scalar column of each variable - vector columnIndices(dims.size() + 1, 0); - for (size_t j = 1; j < dims.size() + 1; ++j) - columnIndices[j] = columnIndices[j - 1] + dims[j - 1]; + size_t currentColIndex = 0; + KeySizeMap columnIndices = dims; + BOOST_FOREACH(const KeySizeMap::value_type& col, dims) { + columnIndices[col.first] = currentColIndex; + currentColIndex += dims[col.first]; + } // Iterate over all factors, adding sparse scalar entries typedef boost::tuple triplet; vector entries; size_t row = 0; BOOST_FOREACH(const sharedFactor& factor, *this) { + if (!static_cast(factor)) continue; + // Convert to JacobianFactor if necessary JacobianFactor::shared_ptr jacobianFactor( boost::dynamic_pointer_cast(factor)); @@ -159,11 +165,11 @@ namespace gtsam { // Whiten the factor and add entries for it // iterate over all variables in the factor const JacobianFactor whitened(jacobianFactor->whiten()); - for (JacobianFactor::const_iterator pos = whitened.begin(); - pos < whitened.end(); ++pos) { - JacobianFactor::constABlock whitenedA = whitened.getA(pos); + for (JacobianFactor::const_iterator key = whitened.begin(); + key < whitened.end(); ++key) { + JacobianFactor::constABlock whitenedA = whitened.getA(key); // find first column index for this key - size_t column_start = columnIndices[*pos]; + size_t column_start = columnIndices[*key]; for (size_t i = 0; i < (size_t) whitenedA.rows(); i++) for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) { double s = whitenedA(i, j); @@ -173,7 +179,7 @@ namespace gtsam { } JacobianFactor::constBVector whitenedb(whitened.getb()); - size_t bcolumn = columnIndices.back(); + size_t bcolumn = currentColIndex; for (size_t i = 0; i < (size_t) whitenedb.size(); i++) entries.push_back(boost::make_tuple(row + i, bcolumn, whitenedb(i))); From 57716646227b619272f7fa00f11a488489c0ba91 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 16:01:54 -0700 Subject: [PATCH 02/11] Starting to diagnose issue with lower-left entry of Hessian --- gtsam/linear/HessianFactor.cpp | 15 ++++----------- gtsam/linear/JacobianFactor.cpp | 8 ++++---- gtsam/linear/tests/testHessianFactor.cpp | 14 ++++++++------ 3 files changed, 16 insertions(+), 21 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index bbc684a10..21f4b117f 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -268,18 +268,11 @@ void HessianFactor::print(const std::string& s, /* ************************************************************************* */ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { - if (!dynamic_cast(&lf)) + const HessianFactor* rhs = dynamic_cast(&lf); + if (!rhs || !Factor::equals(lf, tol)) return false; - else { - if (!Factor::equals(lf, tol)) - return false; - Matrix thisMatrix = info_.full().selfadjointView(); - thisMatrix(thisMatrix.rows() - 1, thisMatrix.cols() - 1) = 0.0; - Matrix rhsMatrix = - static_cast(lf).info_.full().selfadjointView(); - rhsMatrix(rhsMatrix.rows() - 1, rhsMatrix.cols() - 1) = 0.0; - return equal_with_abs_tol(thisMatrix, rhsMatrix, tol); - } + return equal_with_abs_tol(augmentedInformation(), rhs->augmentedInformation(), + tol); } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 8214294b2..b38aa89e7 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -66,10 +66,10 @@ JacobianFactor::JacobianFactor() : /* ************************************************************************* */ JacobianFactor::JacobianFactor(const GaussianFactor& gf) { // 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); + if (const JacobianFactor* asJacobian = dynamic_cast(&gf)) + *this = JacobianFactor(*asJacobian); + else if (const HessianFactor* asHessian = dynamic_cast(&gf)) + *this = JacobianFactor(*asHessian); else throw std::invalid_argument( "In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor"); diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 96e61aa33..afd611112 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -286,8 +286,8 @@ TEST(HessianFactor, CombineAndEliminate) 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(); - Vector3 b0(1.5, 1.5, 1.5); - Vector3 s0(1.6, 1.6, 1.6); + Vector3 b0(1,0,0);//(1.5, 1.5, 1.5); + Vector3 s0=Vector3::Ones();//(1.6, 1.6, 1.6); Matrix A10 = (Matrix(3,3) << 2.0, 0.0, 0.0, @@ -297,15 +297,15 @@ TEST(HessianFactor, CombineAndEliminate) -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0).finished(); - Vector3 b1(2.5, 2.5, 2.5); - Vector3 s1(2.6, 2.6, 2.6); + Vector3 b1 = Vector3::Zero();//(2.5, 2.5, 2.5); + Vector3 s1=Vector3::Ones();//(2.6, 2.6, 2.6); Matrix A21 = (Matrix(3,3) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0).finished(); - Vector3 b2(3.5, 3.5, 3.5); - Vector3 s2(3.6, 3.6, 3.6); + Vector3 b2 = Vector3::Zero();//(3.5, 3.5, 3.5); + Vector3 s2=Vector3::Ones();//(3.6, 3.6, 3.6); GaussianFactorGraph gfg; gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); @@ -334,6 +334,8 @@ TEST(HessianFactor, CombineAndEliminate) boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0))); EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); + VectorValues vv; vv.insert(1, Vector3(1,2,3)); + EXPECT_DOUBLES_EQUAL(expectedRemainingFactor->error(vv), actualCholeskyFactor->error(vv), 1e-9); EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6)); } From 83171b60960d8df2c823f29aaa6986f1c9edbdcf Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 23:07:28 -0700 Subject: [PATCH 03/11] New example --- gtsam/linear/tests/testHessianFactor.cpp | 124 ++++++++++++++++------- 1 file changed, 87 insertions(+), 37 deletions(-) diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index afd611112..13fb9bd0c 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -280,63 +280,113 @@ TEST(HessianFactor, ConstructorNWay) } /* ************************************************************************* */ -TEST(HessianFactor, CombineAndEliminate) -{ - Matrix A01 = (Matrix(3,3) << - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0).finished(); - Vector3 b0(1,0,0);//(1.5, 1.5, 1.5); - Vector3 s0=Vector3::Ones();//(1.6, 1.6, 1.6); +namespace example { +Matrix A01 = (Matrix(3,3) << + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0).finished(); +Vector3 b0(1,0,0);//(1.5, 1.5, 1.5); +Vector3 s0=Vector3::Ones();//(1.6, 1.6, 1.6); - Matrix A10 = (Matrix(3,3) << - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0).finished(); - Matrix A11 = (Matrix(3,3) << - -2.0, 0.0, 0.0, - 0.0, -2.0, 0.0, - 0.0, 0.0, -2.0).finished(); - Vector3 b1 = Vector3::Zero();//(2.5, 2.5, 2.5); - Vector3 s1=Vector3::Ones();//(2.6, 2.6, 2.6); +Matrix A10 = (Matrix(3,3) << + 2.0, 0.0, 0.0, + 0.0, 2.0, 0.0, + 0.0, 0.0, 2.0).finished(); +Matrix A11 = (Matrix(3,3) << + -2.0, 0.0, 0.0, + 0.0, -2.0, 0.0, + 0.0, 0.0, -2.0).finished(); +Vector3 b1 = Vector3::Zero();//(2.5, 2.5, 2.5); +Vector3 s1=Vector3::Ones();//(2.6, 2.6, 2.6); - Matrix A21 = (Matrix(3,3) << - 3.0, 0.0, 0.0, - 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0).finished(); - Vector3 b2 = Vector3::Zero();//(3.5, 3.5, 3.5); - Vector3 s2=Vector3::Ones();//(3.6, 3.6, 3.6); +Matrix A21 = (Matrix(3,3) << + 3.0, 0.0, 0.0, + 0.0, 3.0, 0.0, + 0.0, 0.0, 3.0).finished(); +Vector3 b2 = Vector3::Zero();//(3.5, 3.5, 3.5); +Vector3 s2=Vector3::Ones();//(3.6, 3.6, 3.6); +} +/* ************************************************************************* */ +TEST(HessianFactor, CombineAndEliminate1) { + using namespace example; GaussianFactorGraph gfg; gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); - gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - Matrix93 A0; A0 << A10, Z_3x3, Z_3x3; - Matrix93 A1; A1 << A11, A01, A21; - Vector9 b; b << b1, b0, b2; - Vector9 sigmas; sigmas << s1, s0, s2; + Matrix63 A1; + A1 << A11, A21; + Vector6 b, sigmas; + b << b0, b2; + sigmas << s0, s2; // create a full, uneliminated version of the factor - JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); + JacobianFactor expectedFactor(1, A1, b, + noiseModel::Diagonal::Sigmas(sigmas, true)); // Make sure combining works EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6)); // perform elimination on jacobian + Ordering ordering = list_of(1); GaussianConditional::shared_ptr expectedConditional; - JacobianFactor::shared_ptr expectedRemainingFactor; - boost::tie(expectedConditional, expectedRemainingFactor) = expectedFactor.eliminate(Ordering(list_of(0))); + JacobianFactor::shared_ptr expectedRemaining; + boost::tie(expectedConditional, expectedRemaining) = // + expectedFactor.eliminate(ordering); // Eliminate GaussianConditional::shared_ptr actualConditional; - HessianFactor::shared_ptr actualCholeskyFactor; - boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0))); + HessianFactor::shared_ptr actualHessian; + boost::tie(actualConditional, actualHessian) = // + EliminateCholesky(gfg, ordering); EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); - VectorValues vv; vv.insert(1, Vector3(1,2,3)); - EXPECT_DOUBLES_EQUAL(expectedRemainingFactor->error(vv), actualCholeskyFactor->error(vv), 1e-9); - EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6)); + VectorValues vv; + vv.insert(1, Vector3(1, 2, 3)); + DOUBLES_EQUAL(expectedRemaining->error(vv), actualHessian->error(vv), 1e-9); + EXPECT(assert_equal(HessianFactor(*expectedRemaining), *actualHessian, 1e-6)); +} + +/* ************************************************************************* */ +TEST(HessianFactor, CombineAndEliminate2) { + using namespace example; + GaussianFactorGraph gfg; + gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); + gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); + gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); + + Matrix93 A0, A1; + A0 << A10, Z_3x3, Z_3x3; + A1 << A11, A01, A21; + Vector9 b, sigmas; + b << b1, b0, b2; + sigmas << s1, s0, s2; + + // create a full, uneliminated version of the factor + JacobianFactor expectedFactor(0, A0, 1, A1, b, + noiseModel::Diagonal::Sigmas(sigmas, true)); + + // Make sure combining works + EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6)); + + // perform elimination on jacobian + Ordering ordering = list_of(0); + GaussianConditional::shared_ptr expectedConditional; + JacobianFactor::shared_ptr expectedRemaining; + boost::tie(expectedConditional, expectedRemaining) = // + expectedFactor.eliminate(ordering); + + // Eliminate + GaussianConditional::shared_ptr actualConditional; + HessianFactor::shared_ptr actualHessian; + boost::tie(actualConditional, actualHessian) = // + EliminateCholesky(gfg, ordering); + + EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); + VectorValues vv; + vv.insert(1, Vector3(1, 2, 3)); + DOUBLES_EQUAL(expectedRemaining->error(vv), actualHessian->error(vv), 1e-9); + EXPECT(assert_equal(HessianFactor(*expectedRemaining), *actualHessian, 1e-6)); } /* ************************************************************************* */ From f523db2d9887d3b4a37537173e8cb4171b6b01fc Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 15 Jun 2015 00:52:43 -0700 Subject: [PATCH 04/11] IMPORTANT CHANGE/BUGFIX: QR elimination now (I think properly) leaves a row of zeros if the RHS after QR contains a non-zero. A corresponding change in the error calculation makes that Jacobian and Hessian factors now agree on error. I think this was a bug, because it affected the error, but (I think) it only pertained to "empty" JacobianFactors which have no bearing on optimization/elimination. --- gtsam/linear/JacobianFactor.cpp | 12 +-- gtsam/linear/tests/testHessianFactor.cpp | 111 +++++++++++----------- gtsam/linear/tests/testJacobianFactor.cpp | 6 +- 3 files changed, 65 insertions(+), 64 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index b38aa89e7..61986e71d 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -432,8 +432,6 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const { /* ************************************************************************* */ double JacobianFactor::error(const VectorValues& c) const { - if (empty()) - return 0; Vector weighted = error_vector(c); return 0.5 * weighted.dot(weighted); } @@ -684,8 +682,8 @@ std::pair, jointFactor->Ab_.matrix().triangularView().setZero(); // Split elimination result into conditional and remaining factor - GaussianConditional::shared_ptr conditional = jointFactor->splitConditional( - keys.size()); + GaussianConditional::shared_ptr conditional = // + jointFactor->splitConditional(keys.size()); return make_pair(conditional, jointFactor); } @@ -714,11 +712,11 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional( } GaussianConditional::shared_ptr conditional = boost::make_shared< GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel); - const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd) + const DenseIndex maxRemainingRows = std::min(Ab_.cols(), originalRowEnd) - Ab_.rowStart() - frontalDim; const DenseIndex remainingRows = - model_ ? - std::min(model_->sigmas().size() - frontalDim, maxRemainingRows) : + model_ ? std::min(model_->sigmas().size() - frontalDim, + maxRemainingRows) : maxRemainingRows; Ab_.rowStart() += frontalDim; Ab_.rowEnd() = Ab_.rowStart() + remainingRows; diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 13fb9bd0c..10fb34988 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -279,60 +279,43 @@ TEST(HessianFactor, ConstructorNWay) EXPECT(assert_equal(G33, factor.info(factor.begin()+2, factor.begin()+2))); } -/* ************************************************************************* */ -namespace example { -Matrix A01 = (Matrix(3,3) << - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0).finished(); -Vector3 b0(1,0,0);//(1.5, 1.5, 1.5); -Vector3 s0=Vector3::Ones();//(1.6, 1.6, 1.6); - -Matrix A10 = (Matrix(3,3) << - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0).finished(); -Matrix A11 = (Matrix(3,3) << - -2.0, 0.0, 0.0, - 0.0, -2.0, 0.0, - 0.0, 0.0, -2.0).finished(); -Vector3 b1 = Vector3::Zero();//(2.5, 2.5, 2.5); -Vector3 s1=Vector3::Ones();//(2.6, 2.6, 2.6); - -Matrix A21 = (Matrix(3,3) << - 3.0, 0.0, 0.0, - 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0).finished(); -Vector3 b2 = Vector3::Zero();//(3.5, 3.5, 3.5); -Vector3 s2=Vector3::Ones();//(3.6, 3.6, 3.6); -} - /* ************************************************************************* */ TEST(HessianFactor, CombineAndEliminate1) { - using namespace example; + Matrix3 A01 = 3.0 * I_3x3; + Vector3 b0(1, 0, 0); + + Matrix3 A21 = 4.0 * I_3x3; + Vector3 b2 = Vector3::Zero(); + GaussianFactorGraph gfg; - gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); - gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); + gfg.add(1, A01, b0); + gfg.add(1, A21, b2); Matrix63 A1; - A1 << A11, A21; - Vector6 b, sigmas; + A1 << A01, A21; + Vector6 b; b << b0, b2; - sigmas << s0, s2; // create a full, uneliminated version of the factor - JacobianFactor expectedFactor(1, A1, b, - noiseModel::Diagonal::Sigmas(sigmas, true)); + JacobianFactor jacobian(1, A1, b); // Make sure combining works - EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6)); + HessianFactor hessian(gfg); + VectorValues v; + v.insert(1, Vector3(1, 0, 0)); + EXPECT_DOUBLES_EQUAL(jacobian.error(v), hessian.error(v), 1e-9); + EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6)); + EXPECT(assert_equal(25.0 * I_3x3, hessian.information(), 1e-9)); + EXPECT( + assert_equal(jacobian.augmentedInformation(), + hessian.augmentedInformation(), 1e-9)); // perform elimination on jacobian Ordering ordering = list_of(1); GaussianConditional::shared_ptr expectedConditional; - JacobianFactor::shared_ptr expectedRemaining; - boost::tie(expectedConditional, expectedRemaining) = // - expectedFactor.eliminate(ordering); + JacobianFactor::shared_ptr expectedFactor; + boost::tie(expectedConditional, expectedFactor) = // + jacobian.eliminate(ordering); // Eliminate GaussianConditional::shared_ptr actualConditional; @@ -341,15 +324,28 @@ TEST(HessianFactor, CombineAndEliminate1) { EliminateCholesky(gfg, ordering); EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); - VectorValues vv; - vv.insert(1, Vector3(1, 2, 3)); - DOUBLES_EQUAL(expectedRemaining->error(vv), actualHessian->error(vv), 1e-9); - EXPECT(assert_equal(HessianFactor(*expectedRemaining), *actualHessian, 1e-6)); + EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9); + EXPECT( + assert_equal(expectedFactor->augmentedInformation(), + actualHessian->augmentedInformation(), 1e-9)); + EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6)); } /* ************************************************************************* */ TEST(HessianFactor, CombineAndEliminate2) { - using namespace example; + Matrix A01 = I_3x3; + Vector3 b0(1.5, 1.5, 1.5); + Vector3 s0(1.6, 1.6, 1.6); + + Matrix A10 = 2.0 * I_3x3; + Matrix A11 = -2.0 * I_3x3; + Vector3 b1(2.5, 2.5, 2.5); + Vector3 s1(2.6, 2.6, 2.6); + + Matrix A21 = 3.0 * I_3x3; + Vector3 b2(3.5, 3.5, 3.5); + Vector3 s2(3.6, 3.6, 3.6); + GaussianFactorGraph gfg; gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); @@ -363,18 +359,22 @@ TEST(HessianFactor, CombineAndEliminate2) { sigmas << s1, s0, s2; // create a full, uneliminated version of the factor - JacobianFactor expectedFactor(0, A0, 1, A1, b, + JacobianFactor jacobian(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); // Make sure combining works - EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6)); + HessianFactor hessian(gfg); + EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6)); + EXPECT( + assert_equal(jacobian.augmentedInformation(), + hessian.augmentedInformation(), 1e-9)); // perform elimination on jacobian Ordering ordering = list_of(0); GaussianConditional::shared_ptr expectedConditional; - JacobianFactor::shared_ptr expectedRemaining; - boost::tie(expectedConditional, expectedRemaining) = // - expectedFactor.eliminate(ordering); + JacobianFactor::shared_ptr expectedFactor; + boost::tie(expectedConditional, expectedFactor) = // + jacobian.eliminate(ordering); // Eliminate GaussianConditional::shared_ptr actualConditional; @@ -383,10 +383,13 @@ TEST(HessianFactor, CombineAndEliminate2) { EliminateCholesky(gfg, ordering); EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); - VectorValues vv; - vv.insert(1, Vector3(1, 2, 3)); - DOUBLES_EQUAL(expectedRemaining->error(vv), actualHessian->error(vv), 1e-9); - EXPECT(assert_equal(HessianFactor(*expectedRemaining), *actualHessian, 1e-6)); + VectorValues v; + v.insert(1, Vector3(1, 2, 3)); + EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9); + EXPECT( + assert_equal(expectedFactor->augmentedInformation(), + actualHessian->augmentedInformation(), 1e-9)); + EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 7b2d59171..17ceaf5f0 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -540,9 +540,9 @@ TEST(JacobianFactor, EliminateQR) EXPECT(assert_equal(size_t(2), actualJF.keys().size())); EXPECT(assert_equal(Key(9), actualJF.keys()[0])); EXPECT(assert_equal(Key(11), actualJF.keys()[1])); - EXPECT(assert_equal(Matrix(R.block(6, 6, 4, 2)), actualJF.getA(actualJF.begin()), 0.001)); - EXPECT(assert_equal(Matrix(R.block(6, 8, 4, 2)), actualJF.getA(actualJF.begin()+1), 0.001)); - EXPECT(assert_equal(Vector(R.col(10).segment(6, 4)), actualJF.getb(), 0.001)); + EXPECT(assert_equal(Matrix(R.block(6, 6, 5, 2)), actualJF.getA(actualJF.begin()), 0.001)); + EXPECT(assert_equal(Matrix(R.block(6, 8, 5, 2)), actualJF.getA(actualJF.begin()+1), 0.001)); + EXPECT(assert_equal(Vector(R.col(10).segment(6, 5)), actualJF.getb(), 0.001)); EXPECT(!actualJF.get_model()); } From ca2aa0cfd4bcbf80c2f18ebf040e72dc7aefab96 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Jun 2015 20:33:33 -0700 Subject: [PATCH 05/11] Refactoring to get grouped factors to compile again --- gtsam/slam/RegularHessianFactor.h | 97 +++++++++++++------ gtsam/slam/tests/testRegularHessianFactor.cpp | 81 +++++++++++----- 2 files changed, 122 insertions(+), 56 deletions(-) diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index 5765f67fd..be14067db 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include #include @@ -27,15 +28,11 @@ namespace gtsam { template class RegularHessianFactor: public HessianFactor { -private: - - // Use Eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - public: + typedef Eigen::Matrix VectorD; + typedef Eigen::Matrix MatrixD; + /** Construct an n-way factor. Gs contains the upper-triangle blocks of the * quadratic term (the Hessian matrix) provided in row-order, gs the pieces * of the linear vector term, and f the constant term. @@ -43,27 +40,68 @@ public: RegularHessianFactor(const std::vector& js, const std::vector& Gs, const std::vector& gs, double f) : HessianFactor(js, Gs, gs, f) { + checkInvariants(); } /** Construct a binary factor. Gxx are the upper-triangle blocks of the * quadratic term (the Hessian matrix), gx the pieces of the linear vector * term, and f the constant term. */ - RegularHessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12, - const Vector& g1, const Matrix& G22, const Vector& g2, double f) : + RegularHessianFactor(Key j1, Key j2, const MatrixD& G11, const MatrixD& G12, + const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) : HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) { } + /** Construct a ternary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + */ + RegularHessianFactor(Key j1, Key j2, Key j3, + const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& g1, + const MatrixD& G22, const MatrixD& G23, const VectorD& g2, + const MatrixD& G33, const VectorD& g3, double f) : + HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) { + } + /** Constructor with an arbitrary number of keys and with the augmented information matrix * specified as a block matrix. */ template RegularHessianFactor(const KEYS& keys, const SymmetricBlockMatrix& augmentedInformation) : HessianFactor(keys, augmentedInformation) { + checkInvariants(); } + /// Construct from RegularJacobianFactor + RegularHessianFactor(const RegularJacobianFactor& jf) + : HessianFactor(jf) {} + + /// Construct from a GaussianFactorGraph + RegularHessianFactor(const GaussianFactorGraph& factors, + boost::optional scatter = boost::none) + : HessianFactor(factors, scatter) { + checkInvariants(); + } + +private: + + /// Check invariants after construction + void checkInvariants() { + if (info_.cols() != 1 + (info_.nBlocks()-1) * (DenseIndex)D) + throw std::invalid_argument( + "RegularHessianFactor constructor was given non-regular factors"); + } + + // Use Eigen magic to access raw memory + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + // Scratch space for multiplyHessianAdd - mutable std::vector y; + // According to link below this is thread-safe. + // http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe + mutable std::vector y_; + +public: /** y += alpha * A'*A*x */ virtual void multiplyHessianAdd(double alpha, const VectorValues& x, @@ -74,32 +112,32 @@ public: /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { - // Create a vector of temporary y values, corresponding to rows i - y.resize(size()); - BOOST_FOREACH(DVector & yi, y) + // Create a vector of temporary y_ values, corresponding to rows i + y_.resize(size()); + BOOST_FOREACH(VectorD & yi, y_) yi.setZero(); // Accessing the VectorValues one by one is expensive // So we will loop over columns to access x only once per column - // And fill the above temporary y values, to be added into yvalues after - DVector xj(D); + // And fill the above temporary y_ values, to be added into yvalues after + VectorD xj(D); for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { Key key = keys_[j]; const double* xj = x + key * D; DenseIndex i = 0; for (; i < j; ++i) - y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); // blocks on the diagonal are only half - y[i] += info_(j, j).selfadjointView() * ConstDMap(xj); + y_[i] += info_(j, j).selfadjointView() * ConstDMap(xj); // for below diagonal, we take transpose block from upper triangular part for (i = j + 1; i < (DenseIndex) size(); ++i) - y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); } // copy to yvalues for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { Key key = keys_[i]; - DMap(yvalues + key * D) += alpha * y[i]; + DMap(yvalues + key * D) += alpha * y_[i]; } } @@ -107,28 +145,27 @@ public: void multiplyHessianAdd(double alpha, const double* x, double* yvalues, std::vector offsets) const { - // Create a vector of temporary y values, corresponding to rows i - std::vector y; - y.reserve(size()); - for (const_iterator it = begin(); it != end(); it++) - y.push_back(zero(getDim(it))); + // Create a vector of temporary y_ values, corresponding to rows i + y_.resize(size()); + BOOST_FOREACH(VectorD & yi, y_) + yi.setZero(); // Accessing the VectorValues one by one is expensive // So we will loop over columns to access x only once per column - // And fill the above temporary y values, to be added into yvalues after + // And fill the above temporary y_ values, to be added into yvalues after for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { DenseIndex i = 0; for (; i < j; ++i) - y[i] += info_(i, j).knownOffDiagonal() + y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(x + offsets[keys_[j]], offsets[keys_[j] + 1] - offsets[keys_[j]]); // blocks on the diagonal are only half - y[i] += info_(j, j).selfadjointView() + y_[i] += info_(j, j).selfadjointView() * ConstDMap(x + offsets[keys_[j]], offsets[keys_[j] + 1] - offsets[keys_[j]]); // for below diagonal, we take transpose block from upper triangular part for (i = j + 1; i < (DenseIndex) size(); ++i) - y[i] += info_(i, j).knownOffDiagonal() + y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(x + offsets[keys_[j]], offsets[keys_[j] + 1] - offsets[keys_[j]]); } @@ -136,7 +173,7 @@ public: // copy to yvalues for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) DMap(yvalues + offsets[keys_[i]], - offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y[i]; + offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y_[i]; } /** Return the diagonal of the Hessian for this factor (raw memory version) */ @@ -158,7 +195,7 @@ public: for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal - DVector dj = -info_(pos, size()).knownOffDiagonal(); + VectorD dj = -info_(pos, size()).knownOffDiagonal(); DMap(d + D * j) += dj; } } diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp index e2b8ac3cf..efdef9d44 100644 --- a/gtsam/slam/tests/testRegularHessianFactor.cpp +++ b/gtsam/slam/tests/testRegularHessianFactor.cpp @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -29,30 +30,58 @@ using namespace gtsam; using namespace boost::assign; /* ************************************************************************* */ -TEST(RegularHessianFactor, ConstructorNWay) +TEST(RegularHessianFactor, Constructors) { - Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114).finished(); - Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124).finished(); - Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134).finished(); + // First construct a regular JacobianFactor + Matrix A1 = I_2x2, A2 = I_2x2, A3 = I_2x2; + Vector2 b(1,2); + vector > terms; + terms += make_pair(0, A1), make_pair(1, A2), make_pair(3, A3); + RegularJacobianFactor<2> jf(terms, b); - Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224).finished(); - Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234).finished(); + // Test conversion from JacobianFactor + RegularHessianFactor<2> factor(jf); - Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334).finished(); + Matrix G11 = I_2x2; + Matrix G12 = I_2x2; + Matrix G13 = I_2x2; - Vector g1 = (Vector(2) << -7, -9).finished(); - Vector g2 = (Vector(2) << -9, 1).finished(); - Vector g3 = (Vector(2) << 2, 3).finished(); + Matrix G22 = I_2x2; + Matrix G23 = I_2x2; + + Matrix G33 = I_2x2; + + Vector2 g1 = b, g2 = b, g3 = b; double f = 10; - std::vector js; - js.push_back(0); js.push_back(1); js.push_back(3); - std::vector Gs; - Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33); - std::vector gs; - gs.push_back(g1); gs.push_back(g2); gs.push_back(g3); - RegularHessianFactor<2> factor(js, Gs, gs, f); + // Test ternary constructor + RegularHessianFactor<2> factor2(0, 1, 3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f); + EXPECT(assert_equal(factor,factor2)); + + // Test n-way constructor + vector keys; keys += 0, 1, 3; + vector Gs; Gs += G11, G12, G13, G22, G23, G33; + vector gs; gs += g1, g2, g3; + RegularHessianFactor<2> factor3(keys, Gs, gs, f); + EXPECT(assert_equal(factor, factor3)); + + // Test constructor from Gaussian Factor Graph + GaussianFactorGraph gfg; + gfg += jf; + RegularHessianFactor<2> factor4(gfg); + EXPECT(assert_equal(factor, factor4)); + GaussianFactorGraph gfg2; + gfg2 += factor; + RegularHessianFactor<2> factor5(gfg); + EXPECT(assert_equal(factor, factor5)); + + // Test constructor from Information matrix + Matrix info = factor.augmentedInformation(); + vector dims; dims += 2, 2, 2; + SymmetricBlockMatrix sym(dims, info, true); + RegularHessianFactor<2> factor6(keys, sym); + EXPECT(assert_equal(factor, factor6)); // multiplyHessianAdd: { @@ -61,13 +90,13 @@ TEST(RegularHessianFactor, ConstructorNWay) HessianFactor::const_iterator i1 = factor.begin(); HessianFactor::const_iterator i2 = i1 + 1; Vector X(6); X << 1,2,3,4,5,6; - Vector Y(6); Y << 2633, 2674, 4465, 4501, 5669, 5696; + Vector Y(6); Y << 9, 12, 9, 12, 9, 12; EXPECT(assert_equal(Y,AtA*X)); VectorValues x = map_list_of - (0, (Vector(2) << 1,2).finished()) - (1, (Vector(2) << 3,4).finished()) - (3, (Vector(2) << 5,6).finished()); + (0, Vector2(1,2)) + (1, Vector2(3,4)) + (3, Vector2(5,6)); VectorValues expected; expected.insert(0, Y.segment<2>(0)); @@ -77,15 +106,15 @@ TEST(RegularHessianFactor, ConstructorNWay) // VectorValues version double alpha = 1.0; VectorValues actualVV; - actualVV.insert(0, zero(2)); - actualVV.insert(1, zero(2)); - actualVV.insert(3, zero(2)); + actualVV.insert(0, Vector2::Zero()); + actualVV.insert(1, Vector2::Zero()); + actualVV.insert(3, Vector2::Zero()); factor.multiplyHessianAdd(alpha, x, actualVV); EXPECT(assert_equal(expected, actualVV)); // RAW ACCESS - Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696; - Vector fast_y = gtsam::zero(8); + Vector expected_y(8); expected_y << 9, 12, 9, 12, 0, 0, 9, 12; + Vector fast_y = Vector8::Zero(); double xvalues[8] = {1,2,3,4,0,0,5,6}; factor.multiplyHessianAdd(alpha, xvalues, fast_y.data()); EXPECT(assert_equal(expected_y, fast_y)); From b91ed6dc6ee404f1832deab575706dc1c0b82a3f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Jun 2015 21:17:22 -0700 Subject: [PATCH 06/11] Got rid of warning --- gtsam/slam/SmartFactorBase.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index cdbe71718..c448dbed4 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -643,7 +643,6 @@ public: // gs = F' * (b - E * P * E' * b) MatrixDD matrixBlock; - typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix FastMap KeySlotMap; for (size_t slot = 0; slot < allKeys.size(); slot++) From 0f02b7d47385fd04be4fc8ec96b30fbf8fe14735 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 17 Jun 2015 16:23:27 -0400 Subject: [PATCH 07/11] Prohibit Timing build mode with TBB. See issue #173 --- CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6e9aa0009..a77173ba0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -158,6 +158,12 @@ else() set(GTSAM_USE_TBB 0) # This will go into config.h endif() +############################################################################### +# Prohibit Timing build mode in combination with TBB +if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing")) + message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.") +endif() + ############################################################################### # Find Google perftools From 5719ba27fad1013d45ac921bf2e26f4a0e4332c9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 20 Jun 2015 15:28:25 -0700 Subject: [PATCH 08/11] Reducing header bloat by introducing new ThreadSafeException header. --- gtsam/base/FastSet.h | 9 +- gtsam/base/FastVector.h | 47 +++--- gtsam/base/SymmetricBlockMatrix.cpp | 1 + gtsam/base/SymmetricBlockMatrix.h | 23 ++- gtsam/base/TestableAssertions.h | 11 +- gtsam/base/ThreadsafeException.h | 152 ++++++++++++++++++ .../treeTraversal/parallelTraversalTasks.h | 1 + gtsam/base/types.h | 108 +------------ gtsam/geometry/CalibratedCamera.h | 5 + gtsam/inference/VariableIndex-inl.h | 2 + gtsam/inference/VariableIndex.h | 13 +- gtsam/inference/tests/testKey.cpp | 1 + gtsam/linear/HessianFactor.cpp | 18 ++- gtsam/linear/SubgraphPreconditioner.cpp | 35 +++- gtsam/linear/SubgraphPreconditioner.h | 21 ++- gtsam/linear/linearExceptions.cpp | 1 + gtsam/linear/linearExceptions.h | 5 +- tests/testMarginals.cpp | 3 +- 18 files changed, 282 insertions(+), 174 deletions(-) create mode 100644 gtsam/base/ThreadsafeException.h diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 73df17b0d..c3352dfd5 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -19,15 +19,18 @@ #pragma once #include + #include #include #include #include #include -#include -#include -#include + #include +#include +#include +#include +#include BOOST_MPL_HAS_XXX_TRAIT_DEF(print) diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h index d154ea52a..0a4932e01 100644 --- a/gtsam/base/FastVector.h +++ b/gtsam/base/FastVector.h @@ -18,12 +18,9 @@ #pragma once -#include +#include #include -#include - -#include -#include +#include namespace gtsam { @@ -35,20 +32,27 @@ namespace gtsam { * @addtogroup base */ template -class FastVector: public std::vector::type> { +class FastVector: public std::vector::type> { public: - typedef std::vector::type> Base; + typedef std::vector::type> Base; /** Default constructor */ - FastVector() {} + FastVector() { + } /** Constructor from size */ - explicit FastVector(size_t size) : Base(size) {} + explicit FastVector(size_t size) : + Base(size) { + } /** Constructor from size and initial values */ - explicit FastVector(size_t size, const VALUE& initial) : Base(size, initial) {} + explicit FastVector(size_t size, const VALUE& initial) : + Base(size, initial) { + } /** Constructor from a range, passes through to base class */ template @@ -56,33 +60,22 @@ public: // This if statement works around a bug in boost pool allocator and/or // STL vector where if the size is zero, the pool allocator will allocate // huge amounts of memory. - if(first != last) + if (first != last) Base::assign(first, last); } - /** Copy constructor from a FastList */ - FastVector(const FastList& x) { - if(x.size() > 0) - Base::assign(x.begin(), x.end()); - } - - /** Copy constructor from a FastSet */ - FastVector(const FastSet& x) { - if(x.size() > 0) - Base::assign(x.begin(), x.end()); - } - /** Copy constructor from the base class */ - FastVector(const Base& x) : Base(x) {} + FastVector(const Base& x) : + Base(x) { + } /** Copy constructor from a standard STL container */ template - FastVector(const std::vector& x) - { + FastVector(const std::vector& x) { // This if statement works around a bug in boost pool allocator and/or // STL vector where if the size is zero, the pool allocator will allocate // huge amounts of memory. - if(x.size() > 0) + if (x.size() > 0) Base::assign(x.begin(), x.end()); } diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index 0fbdfeefc..7cca63092 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -20,6 +20,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 41584c7f9..1f81ca1f9 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -17,9 +17,21 @@ */ #pragma once -#include #include +#include #include +#include +#include +#include +#include +#include +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ namespace gtsam { @@ -247,13 +259,8 @@ namespace gtsam { } }; - /* ************************************************************************* */ - class CholeskyFailed : public gtsam::ThreadsafeException - { - public: - CholeskyFailed() throw() {} - virtual ~CholeskyFailed() throw() {} - }; + /// Foward declare exception class + class CholeskyFailed; } diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 04d3fc676..f976be0e7 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -17,13 +17,14 @@ #pragma once -#include -#include -#include +#include +#include + #include #include -#include -#include +#include +#include +#include namespace gtsam { diff --git a/gtsam/base/ThreadsafeException.h b/gtsam/base/ThreadsafeException.h new file mode 100644 index 000000000..3bdd42608 --- /dev/null +++ b/gtsam/base/ThreadsafeException.h @@ -0,0 +1,152 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ThreadSafeException.h + * @brief Base exception type that uses tbb_exception if GTSAM is compiled with TBB + * @author Richard Roberts + * @date Aug 21, 2010 + * @addtogroup base + */ + +#pragma once + +#include +#include +#include +#include + +#ifdef GTSAM_USE_TBB +#include +#include +#endif + +namespace gtsam { + +/// Base exception type that uses tbb_exception if GTSAM is compiled with TBB. +template +class ThreadsafeException: +#ifdef GTSAM_USE_TBB + public tbb::tbb_exception +#else +public std::exception +#endif +{ +#ifdef GTSAM_USE_TBB +private: + typedef tbb::tbb_exception Base; +protected: + typedef std::basic_string, + tbb::tbb_allocator > String; +#else +private: + typedef std::exception Base; +protected: + typedef std::string String; +#endif + +protected: + bool dynamic_; ///< Whether this object was moved + mutable boost::optional description_; ///< Optional description + + /// Default constructor is protected - may only be created from derived classes + ThreadsafeException() : + dynamic_(false) { + } + + /// Copy constructor is protected - may only be created from derived classes + ThreadsafeException(const ThreadsafeException& other) : + Base(other), dynamic_(false) { + } + + /// Construct with description string + ThreadsafeException(const std::string& description) : + dynamic_(false), description_( + String(description.begin(), description.end())) { + } + + /// Default destructor doesn't have the throw() + virtual ~ThreadsafeException() throw () { + } + +public: + // Implement functions for tbb_exception +#ifdef GTSAM_USE_TBB + virtual tbb::tbb_exception* move() throw () { + void* cloneMemory = scalable_malloc(sizeof(DERIVED)); + if (!cloneMemory) { + std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl; + exit(-1); + } + DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast(*this)); + clone->dynamic_ = true; + return clone; + } + + virtual void destroy() throw () { + if (dynamic_) { + DERIVED* derivedPtr = static_cast(this); + derivedPtr->~DERIVED(); + scalable_free(derivedPtr); + } + } + + virtual void throw_self() { + throw *static_cast(this); + } + + virtual const char* name() const throw () { + return typeid(DERIVED).name(); + } +#endif + + virtual const char* what() const throw () { + return description_ ? description_->c_str() : ""; + } +}; + +/// Thread-safe runtime error exception +class RuntimeErrorThreadsafe: public ThreadsafeException { +public: + /// Construct with a string describing the exception + RuntimeErrorThreadsafe(const std::string& description) : + ThreadsafeException(description) { + } +}; + +/// Thread-safe out of range exception +class OutOfRangeThreadsafe: public ThreadsafeException { +public: + /// Construct with a string describing the exception + OutOfRangeThreadsafe(const std::string& description) : + ThreadsafeException(description) { + } +}; + +/// Thread-safe invalid argument exception +class InvalidArgumentThreadsafe: public ThreadsafeException< + InvalidArgumentThreadsafe> { +public: + /// Construct with a string describing the exception + InvalidArgumentThreadsafe(const std::string& description) : + ThreadsafeException(description) { + } +}; + +/// Indicate Cholesky factorization failure +class CholeskyFailed : public gtsam::ThreadsafeException +{ +public: + CholeskyFailed() throw() {} + virtual ~CholeskyFailed() throw() {} +}; + +} // namespace gtsam diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 7986f9534..c1df47a01 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -24,6 +24,7 @@ #ifdef GTSAM_USE_TBB # include +# include # undef max // TBB seems to include windows.h and we don't want these macros # undef min # undef ERROR diff --git a/gtsam/base/types.h b/gtsam/base/types.h index a66db13c8..a325fb1ad 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -20,19 +20,16 @@ #pragma once #include -#include -#include +#include +#include #include -#include +#include #include -#include -#include #ifdef GTSAM_USE_TBB #include #include #include -#include #endif #ifdef GTSAM_USE_EIGEN_MKL_OPENMP @@ -73,7 +70,6 @@ namespace gtsam { /// The index type for Eigen objects typedef ptrdiff_t DenseIndex; - /* ************************************************************************* */ /** * Helper class that uses templates to select between two types based on @@ -148,104 +144,6 @@ namespace gtsam { return ListOfOneContainer(element); } - /* ************************************************************************* */ - /// Base exception type that uses tbb_exception if GTSAM is compiled with TBB. - template - class ThreadsafeException : -#ifdef GTSAM_USE_TBB - public tbb::tbb_exception -#else - public std::exception -#endif - { -#ifdef GTSAM_USE_TBB - private: - typedef tbb::tbb_exception Base; - protected: - typedef std::basic_string, tbb::tbb_allocator > String; -#else - private: - typedef std::exception Base; - protected: - typedef std::string String; -#endif - - protected: - bool dynamic_; ///< Whether this object was moved - mutable boost::optional description_; ///< Optional description - - /// Default constructor is protected - may only be created from derived classes - ThreadsafeException() : dynamic_(false) {} - - /// Copy constructor is protected - may only be created from derived classes - ThreadsafeException(const ThreadsafeException& other) : Base(other), dynamic_(false) {} - - /// Construct with description string - ThreadsafeException(const std::string& description) : dynamic_(false), description_(String(description.begin(), description.end())) {} - - /// Default destructor doesn't have the throw() - virtual ~ThreadsafeException() throw () {} - - public: - // Implement functions for tbb_exception -#ifdef GTSAM_USE_TBB - virtual tbb::tbb_exception* move() throw () { - void* cloneMemory = scalable_malloc(sizeof(DERIVED)); - if (!cloneMemory) { - std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl; - exit(-1); - } - DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast(*this)); - clone->dynamic_ = true; - return clone; - } - - virtual void destroy() throw() { - if (dynamic_) { - DERIVED* derivedPtr = static_cast(this); - derivedPtr->~DERIVED(); - scalable_free(derivedPtr); - } - } - - virtual void throw_self() { - throw *static_cast(this); } - - virtual const char* name() const throw() { - return typeid(DERIVED).name(); } -#endif - - virtual const char* what() const throw() { - return description_ ? description_->c_str() : ""; } - }; - - /* ************************************************************************* */ - /// Threadsafe runtime error exception - class RuntimeErrorThreadsafe : public ThreadsafeException - { - public: - /// Construct with a string describing the exception - RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException(description) {} - }; - - /* ************************************************************************* */ - /// Threadsafe runtime error exception - class OutOfRangeThreadsafe : public ThreadsafeException - { - public: - /// Construct with a string describing the exception - OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException(description) {} - }; - - /* ************************************************************************* */ - /// Threadsafe invalid argument exception - class InvalidArgumentThreadsafe : public ThreadsafeException - { - public: - /// Construct with a string describing the exception - InvalidArgumentThreadsafe(const std::string& description) : ThreadsafeException(description) {} - }; - /* ************************************************************************* */ #ifdef __clang__ # pragma clang diagnostic push diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index f17cc6441..0b278bade 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -19,6 +19,11 @@ #pragma once #include +#include +#include +#include +#include +#include namespace gtsam { diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h index 82eb85c76..c00a3633e 100644 --- a/gtsam/inference/VariableIndex-inl.h +++ b/gtsam/inference/VariableIndex-inl.h @@ -18,6 +18,8 @@ #pragma once #include +#include +#include namespace gtsam { diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index bcaec6ee4..d4b8eeacf 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -17,17 +17,18 @@ #pragma once -#include #include #include -#include +#include #include -#include +#include +#include -#include +#include +#include +#include -#include -#include +#include #include namespace gtsam { diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index 1033c0cc9..b0708e660 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -14,6 +14,7 @@ * @author Alex Cunningham */ +#include #include #include #include diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 21f4b117f..f3e54cffb 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -15,17 +15,19 @@ * @date Dec 8, 2010 */ -#include +#include + #include #include -#include -#include #include -#include -#include -#include -#include +#include +#include #include +#include +#include +#include +#include +#include #include #include @@ -469,7 +471,7 @@ std::pair, // Erase the eliminated keys in the remaining factor jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + numberOfKeysToEliminate); - } catch (CholeskyFailed&) { + } catch (const CholeskyFailed& e) { throw IndeterminantLinearSystemException(keys.front()); } diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index f5ee4ddfc..e0bd8d8cd 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -15,20 +15,41 @@ * @author: Frank Dellaert */ -#include #include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include -#include #include +#include +#include +#include +#include +#include + +#include +#include +#include #include +#include #include -#include +#include +#include +#include +#include // accumulate #include #include +#include +#include #include #include diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 350963bcf..c1600dd45 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -17,13 +17,32 @@ #pragma once -#include #include +#include +#include #include #include #include +#include +#include +#include +#include +#include + +#include #include +#include +#include +#include +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ + namespace gtsam { // Forward declarations diff --git a/gtsam/linear/linearExceptions.cpp b/gtsam/linear/linearExceptions.cpp index 88758ae7a..a4168af2d 100644 --- a/gtsam/linear/linearExceptions.cpp +++ b/gtsam/linear/linearExceptions.cpp @@ -19,6 +19,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/linear/linearExceptions.h b/gtsam/linear/linearExceptions.h index 63bc61e80..32db27fc9 100644 --- a/gtsam/linear/linearExceptions.h +++ b/gtsam/linear/linearExceptions.h @@ -17,9 +17,8 @@ */ #pragma once -#include -#include -#include +#include +#include namespace gtsam { diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index dd22ae738..0af4c4a57 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -226,7 +226,8 @@ TEST(Marginals, order) { vals.at(3).range(vals.at(101)), noiseModel::Unit::Create(2)); Marginals marginals(fg, vals); - FastVector keys(fg.keys()); + FastSet set = fg.keys(); + FastVector keys(set.begin(), set.end()); JointMarginal joint = marginals.jointMarginalCovariance(keys); LONGS_EQUAL(3, (long)joint(0,0).rows()); From 63c28f89bfa0f959bdc3f1616bef8a9ecedd6b51 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 20 Jun 2015 15:37:11 -0700 Subject: [PATCH 09/11] Removed system-specific header --- gtsam/base/types.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/base/types.h b/gtsam/base/types.h index a325fb1ad..4010eb70e 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -23,7 +23,6 @@ #include #include #include -#include #include #ifdef GTSAM_USE_TBB From 8ba8a0ff947e0e0ac85ffa49b54a23cdef74402f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 20 Jun 2015 15:47:30 -0700 Subject: [PATCH 10/11] Removed system specific header (damn eclipse organize includes!) --- gtsam/linear/SubgraphPreconditioner.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index c1600dd45..e74b92df1 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -32,7 +32,6 @@ #include #include -#include #include #include #include From d2bf0120fc5c2740c2bce439348ba75c470bf93b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Jun 2015 15:50:57 -0700 Subject: [PATCH 11/11] And another one ! --- gtsam/linear/SubgraphPreconditioner.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index e0bd8d8cd..ee2447d2f 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -34,7 +34,6 @@ #include #include #include -#include #include #include