diff --git a/.cproject b/.cproject index de77b5ece..51b0b039f 100644 --- a/.cproject +++ b/.cproject @@ -769,6 +769,14 @@ true true + + make + -j2 + tests/testSimulated2D.run + true + true + true + make -j2 @@ -905,10 +913,10 @@ true true - + make -j2 - testTupleConfig.run + testTupleValues.run true true true @@ -1664,10 +1672,10 @@ true true - + make -j2 - tests/testHessianFactor.run + tests/testJacobianFactor.run true true true @@ -1704,6 +1712,14 @@ true true + + make + -j2 + tests/testHessianFactor.run + true + true + true + make -j2 @@ -1712,10 +1728,10 @@ true true - + make -j2 - CameraResectioning + SLAMSelfContained.run true true true @@ -1736,54 +1752,6 @@ true true - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j2 - PlanarSLAMSelfContained_advanced.run - true - true - true - - - make - -j2 - Pose2SLAMExample_advanced.run - true - true - true - - - make - -j2 - Pose2SLAMExample_easy.run - true - true - true - - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - make -j2 @@ -1960,6 +1928,14 @@ true true + + make + -j2 + install + true + true + true + make -j2 diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 056ea47ed..bb11cc608 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -82,7 +82,8 @@ namespace gtsam { /* ************************************************************************* */ std::vector > GaussianFactorGraph::sparseJacobian( const std::vector& columnIndices) const { - std::vector > entries; + typedef boost::tuple triplet; + std::vector entries; size_t i = 0; BOOST_FOREACH(const sharedFactor& factor, *this) { // Convert to JacobianFactor if necessary @@ -99,13 +100,13 @@ namespace gtsam { } // Add entries, adjusting the row index i - std::vector > factorEntries( + std::vector factorEntries( jacobianFactor->sparse(columnIndices)); entries.reserve(entries.size() + factorEntries.size()); - for (size_t entry = 0; entry < factorEntries.size(); ++entry) + for (size_t k = 0; k < factorEntries.size(); ++k) entries.push_back(boost::make_tuple( - factorEntries[entry].get<0> () + i, factorEntries[entry].get< - 1> (), factorEntries[entry].get<2> ())); + factorEntries[k].get<0> () + i, factorEntries[k].get< + 1> (), factorEntries[k].get<2> ())); // Increment row index i += jacobianFactor->rows(); @@ -113,6 +114,31 @@ namespace gtsam { return entries; } + /* ************************************************************************* */ + Matrix GaussianFactorGraph::sparse(const Vector& columnIndices) const { + + // translate from base 1 Vector to vector of base 0 indices + std::vector indices; + for (int i = 0; i < columnIndices.size(); i++) + indices.push_back(columnIndices[i] - 1); + + // call sparseJacobian + typedef boost::tuple triplet; + std::vector < boost::tuple > result = + sparseJacobian(indices); + + // translate to base 1 matrix + size_t nzmax = result.size(); + Matrix IJS(3,nzmax); + for (size_t k = 0; k < result.size(); k++) { + const triplet& entry = result[k]; + IJS(0,k) = entry.get<0>() + 1; + IJS(1,k) = entry.get<1>() + 1; + IJS(2,k) = entry.get<2>(); + } + return IJS; + } + /* ************************************************************************* */ Matrix GaussianFactorGraph::denseJacobian() const { // combine all factors diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 0aec3e9f2..e0c3d1c18 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -149,12 +149,21 @@ namespace gtsam { void combine(const GaussianFactorGraph &lfg); /** - * Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix - * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. - * The standard deviations are baked into A and b - * @param first column index for each variable - */ - std::vector > sparseJacobian(const std::vector& columnIndices) const; + * Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix, + * where i(k) and j(k) are the base 0 row and column indices, s(k) a double. + * The standard deviations are baked into A and b + * @param columnIndices First column index for each variable. + */ + std::vector > sparseJacobian( + const std::vector& columnIndices) const; + + /** + * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries + * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. + * The standard deviations are baked into A and b + * @param columnIndices First column index for each variable, base 1, in vector format. + */ + Matrix sparse(const Vector& columnIndices) const; /** * Return a dense \f$ m \times n \f$ Jacobian matrix, augmented with b diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 89a0ccdad..f33160508 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -39,6 +39,33 @@ static SharedDiagonal sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2), constraintModel = noiseModel::Constrained::All(2); +/* ************************************************************************* */ +TEST(GaussianFactorGraph, initialization) { + // Create empty graph + GaussianFactorGraph fg; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + + fg.add(0, 10*eye(2), -1.0*ones(2), unit2); + fg.add(0, -10*eye(2),1, 10*eye(2), Vector_(2, 2.0, -1.0), unit2); + 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]; + + // Test sparse, which takes a vector and returns a matrix, used in MATLAB + Matrix expectedIJS = Matrix_(3,22, + 1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7., 8., 7., 8., + 1., 2., 5., 5., 1., 2., 3., 4., 5., 5., 1., 2., 5., 6., 5., 5., 3., 4., 5., 6., 5., 5., + 10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5., -5., 5., 5., -1., 1.5 + ); + Vector columnIndices = Vector_(3,1.0,3.0,5.0); + Matrix actualIJS = fg.sparse(columnIndices); + EQUALITY(expectedIJS, actualIJS); +} + /* ************************************************************************* */ #ifdef BROKEN TEST(GaussianFactor, Combine) @@ -545,24 +572,6 @@ TEST(GaussianFactor, permuteWithInverse) #endif } -/* ************************************************************************* */ -TEST(GaussianFactorGraph, initialization) { - // Create empty graph - GaussianFactorGraph fg; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - - // ordering x1, x2, l1 - build system in smallExample.cpp: createGaussianFactorGraph() - fg.add(0, 10*eye(2), -1.0*ones(2), unit2); - fg.add(0, -10*eye(2),1, 10*eye(2), Vector_(2, 2.0, -1.0), unit2); - 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]; -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp index ca7f805ac..7724043f6 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraph.cpp @@ -413,25 +413,6 @@ TEST( GaussianFactorGraph, copying ) // EXPECT(6 == mn.second); //} -///* ************************************************************************* */ -//SL-FIX TEST( GaussianFactorGraph, sparse ) -//{ -// // create a small linear factor graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // render with a given ordering -// Ordering ord; -// ord += "x2","l1","x1"; -// -// Matrix ijs = fg.sparse(ord); -// -// EQUALITY(Matrix_(3, 14, -// // f(x1) f(x2,x1) f(l1,x1) f(x2,l1) -// +1., 2., 3., 4., 3., 4., 5.,6., 5., 6., 7., 8., 7., 8., -// +5., 6., 5., 6., 1., 2., 3.,4., 5., 6., 3., 4., 1., 2., -// 10.,10., -10.,-10., 10., 10., 5.,5.,-5.,-5., 5., 5.,-5.,-5.), ijs); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) {