From 4a4e16485c1b59057849d5517fc67a991d716737 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Aug 2013 20:08:54 +0000 Subject: [PATCH] Making more things in gtsam_unstable compile --- .cproject | 8 +++ gtsam_unstable/CMakeLists.txt | 21 ++---- gtsam_unstable/linear/bayesTreeOperations.h | 5 +- .../nonlinear/BatchFixedLagSmoother.cpp | 2 +- .../nonlinear/BatchFixedLagSmoother.h | 5 +- gtsam_unstable/nonlinear/CMakeLists.txt | 9 ++- .../nonlinear/ConcurrentBatchFilter.cpp | 28 ++++---- .../nonlinear/ConcurrentBatchFilter.h | 7 +- .../nonlinear/ConcurrentBatchSmoother.h | 2 +- .../nonlinear/tests/testLinearizedFactor.cpp | 70 +++++++------------ gtsam_unstable/slam/CMakeLists.txt | 10 ++- gtsam_unstable/slam/DummyFactor.cpp | 9 ++- gtsam_unstable/slam/DummyFactor.h | 3 +- gtsam_unstable/slam/SmartProjectionFactor.h | 9 +-- gtsam_unstable/slam/SmartRangeFactor.h | 1 + gtsam_unstable/slam/tests/testDummyFactor.cpp | 12 ++-- .../slam/tests/testSmartProjectionFactor.cpp | 9 ++- .../slam/tests/testSmartRangeFactor.cpp | 6 +- 18 files changed, 100 insertions(+), 116 deletions(-) diff --git a/.cproject b/.cproject index 1ac3c8ca9..6590f19b6 100644 --- a/.cproject +++ b/.cproject @@ -2452,6 +2452,14 @@ true true + + make + -j6 -j8 + check.nonlinear_unstable + true + true + true + make -j5 diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index bc120756b..2a1a350a7 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -7,7 +7,7 @@ set (gtsam_unstable_subdirs linear dynamics nonlinear - #slam + slam ) set(GTSAM_UNSTABLE_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES}) @@ -18,27 +18,18 @@ add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-fail # Add the full name to this list, as in the following example # Sources to remove from builds set (excluded_sources # "") - "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/BatchFixedLagSmoother.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentBatchFilter.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentBatchSmoother.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentFilteringAndSmoothing.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/FixedLagSmoother.cpp" +# "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentFilteringAndSmoothing.cpp" +# "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/FixedLagSmoother.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/IncrementalFixedLagSmoother.cpp" -# "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/LinearizedFactor.cpp" + + "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp" ) set (excluded_headers # "") - "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h" - - "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/BatchFixedLagSmoother.h" - "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentBatchFilter.h" - "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentBatchSmoother.h" - "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentFilteringAndSmoothing.h" - "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/FixedLagSmoother.h" - "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/IncrementalFixedLagSmoother.h" -# "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/LinearizedFactor.h" + "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h" ) # assemble core libaries diff --git a/gtsam_unstable/linear/bayesTreeOperations.h b/gtsam_unstable/linear/bayesTreeOperations.h index b29ead520..ecfb9a711 100644 --- a/gtsam_unstable/linear/bayesTreeOperations.h +++ b/gtsam_unstable/linear/bayesTreeOperations.h @@ -88,7 +88,10 @@ GaussianFactorGraph liquefy(const typename BAYESTREE::sharedClique& root, bool s */ template GaussianFactorGraph liquefy(const BAYESTREE& bayesTree, bool splitConditionals = false) { - return liquefy(bayesTree.root(), splitConditionals); + GaussianFactorGraph result; + BOOST_FOREACH(const typename BAYESTREE::sharedClique& root, bayesTree.roots()) + result.push_back(liquefy(root, splitConditionals)); + return result; } } // \namespace gtsam diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 0a84d307a..5e6c5d172 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -22,7 +22,7 @@ #include #include #include -#include +//#include #include namespace gtsam { diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index dd22939bf..d3706b61c 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -55,7 +55,7 @@ public: * a single variable is needed, it is faster to call calculateEstimate(const KEY&). */ Values calculateEstimate() const { - return theta_.retract(delta_, ordering_); + return theta_.retract(delta_); } /** Compute an estimate for a single variable using its incomplete linear delta computed @@ -66,8 +66,7 @@ public: */ template VALUE calculateEstimate(Key key) const { - const Index index = ordering_.at(key); - const Vector delta = delta_.at(index); + const Vector delta = delta_.at(key); return theta_.at(key).retract(delta); } diff --git a/gtsam_unstable/nonlinear/CMakeLists.txt b/gtsam_unstable/nonlinear/CMakeLists.txt index 2a7aa37dc..225fa09e2 100644 --- a/gtsam_unstable/nonlinear/CMakeLists.txt +++ b/gtsam_unstable/nonlinear/CMakeLists.txt @@ -20,11 +20,10 @@ set (nonlinear_full_libs # Exclude tests that don't work set (nonlinear_excluded_tests #"") -"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/test/testBatchFixedLagSmoother.cpp" -"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/test/testConcurrentBatchFilter.cpp" -"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/test/testConcurrentBatchSmoother.cpp" -"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/test/testIncrementalFixedLagSmoother.cpp" -#"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/test/testLinearizedFactor.cpp" +"${CMAKE_CURRENT_SOURCE_DIR}/tests/testBatchFixedLagSmoother.cpp" +"${CMAKE_CURRENT_SOURCE_DIR}/tests/testConcurrentBatchFilter.cpp" +"${CMAKE_CURRENT_SOURCE_DIR}/tests/testConcurrentBatchSmoother.cpp" +"${CMAKE_CURRENT_SOURCE_DIR}/tests/testIncrementalFixedLagSmoother.cpp" ) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 1e1e39e10..7add1994e 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 - Ordering ordering = *graph.orderingCOLAMD(values); - VectorValues delta = values.zeroVectors(ordering); + Ordering ordering = graph.orderingCOLAMD(); + VectorValues delta = values.zeroVectors(); // 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 - Ordering filterOrdering = *graph.orderingCOLAMDConstrained(values, filterConstraints); - Ordering smootherOrdering = *graph.orderingCOLAMDConstrained(values, smootherConstraints); + Ordering filterOrdering = graph.orderingCOLAMDConstrained(filterConstraints); + Ordering smootherOrdering = graph.orderingCOLAMDConstrained(smootherConstraints); // Extract the set of filter keys and smoother keys std::set filterKeys; @@ -339,7 +339,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac double errorTol = parameters.errorTol; // Create a Values that holds the current evaluation point - Values evalpoint = theta.retract(delta, ordering); + Values evalpoint = theta.retract(delta); result.error = factors.error(evalpoint); // Use a custom optimization loop so the linearization points can be controlled @@ -352,7 +352,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac gttic(optimizer_iteration); { // Linearize graph around the linearization point - GaussianFactorGraph linearFactorGraph = *factors.linearize(theta, ordering); + GaussianFactorGraph linearFactorGraph = *factors.linearize(theta); // Keep increasing lambda until we make make progress while(true) { @@ -377,7 +377,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac // Solve Damped Gaussian Factor Graph newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters.getEliminationFunction()); // update the evalpoint with the new delta - evalpoint = theta.retract(newDelta, ordering); + evalpoint = theta.retract(newDelta); gttoc(solve); // Evaluate the new nonlinear error @@ -442,7 +442,7 @@ void ConcurrentBatchFilter::marginalize(const FastList& keysToMove) { // Note: It is assumed the ordering already has these keys first // Create the linear factor graph - GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); + GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_); // Calculate the variable index VariableIndex variableIndex(linearFactorGraph, ordering_.size()); @@ -474,7 +474,7 @@ void ConcurrentBatchFilter::marginalize(const FastList& keysToMove) { BOOST_FOREACH(Index index, indicesToEliminate) { GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); if(gaussianFactor->size() > 0) { - LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_)); + LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, theta_)); marginalFactors.push_back(marginalFactor); // Add the keys associated with the marginal factor to the separator values BOOST_FOREACH(Key key, *marginalFactor) { @@ -551,7 +551,7 @@ NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGra // Note: It is assumed the ordering already has these keys first // Create the linear factor graph - GaussianFactorGraph linearFactorGraph = *graph.linearize(values, ordering); + GaussianFactorGraph linearFactorGraph = *graph.linearize(values); // Construct a variable index VariableIndex variableIndex(linearFactorGraph, ordering.size()); @@ -576,7 +576,7 @@ NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGra BOOST_FOREACH(Index index, indicesToEliminate) { GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(function); if(gaussianFactor->size() > 0) { - LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values)); + LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, values)); marginalFactors.push_back(marginalFactor); } } @@ -604,13 +604,13 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p } /* ************************************************************************* */ -void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, +void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const std::string& indent, const KeyFormatter& keyFormatter) { std::cout << indent; if(factor) { std::cout << "g( "; - BOOST_FOREACH(Index index, *factor) { - std::cout << keyFormatter(ordering.key(index)) << " "; + BOOST_FOREACH(Index key, *factor) { + std::cout << keyFormatter(key) << " "; } std::cout << ")" << std::endl; } else { diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 9a7690786..def798f1c 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -89,7 +89,7 @@ public: * If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&). */ Values calculateEstimate() const { - return theta_.retract(delta_, ordering_); + return theta_.retract(delta_); } /** Compute the current best estimate of a single variable. This is generally faster than @@ -99,8 +99,7 @@ public: */ template VALUE calculateEstimate(Key key) const { - const Index index = ordering_.at(key); - const Vector delta = delta_.at(index); + const Vector delta = delta_.at(key); return theta_.at(key).retract(delta); } @@ -213,7 +212,7 @@ private: const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** Print just the nonlinear keys in a linear factor */ - static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, + static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); // A custom elimination tree that supports forests and partial elimination diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index f54c1f60d..3142abfd0 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -89,7 +89,7 @@ public: * If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&). */ Values calculateEstimate() const { - return theta_.retract(delta_, ordering_); + return theta_.retract(delta_); } /** Compute the current best estimate of a single variable. This is generally faster than diff --git a/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp index 66ed5f5c1..21e271bd8 100644 --- a/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp @@ -33,9 +33,6 @@ TEST( LinearizedFactor, equals_jacobian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - Ordering ordering; - ordering.push_back(x1); - ordering.push_back(x2); Values values; values.insert(x1, Point3(-22.4, +8.5, +2.4)); values.insert(x2, Point3(-21.0, +5.0, +21.0)); @@ -46,9 +43,9 @@ TEST( LinearizedFactor, equals_jacobian ) // Create two identical factors and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); - LinearizedJacobianFactor jacobian1(jf, ordering, values); - LinearizedJacobianFactor jacobian2(jf, ordering, values); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); + LinearizedJacobianFactor jacobian1(jf, values); + LinearizedJacobianFactor jacobian2(jf, values); CHECK(assert_equal(jacobian1, jacobian2)); } @@ -59,9 +56,6 @@ TEST( LinearizedFactor, clone_jacobian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - Ordering ordering; - ordering.push_back(x1); - ordering.push_back(x2); Values values; values.insert(x1, Point3(-22.4, +8.5, +2.4)); values.insert(x2, Point3(-21.0, +5.0, +21.0)); @@ -71,13 +65,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 - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); - LinearizedJacobianFactor jacobian1(jf, ordering, values); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); + LinearizedJacobianFactor jacobian1(jf, values); LinearizedJacobianFactor::shared_ptr jacobian2 = boost::static_pointer_cast(jacobian1.clone()); CHECK(assert_equal(jacobian1, *jacobian2)); - JacobianFactor::shared_ptr jf1 = boost::static_pointer_cast(jacobian1.linearize(values, ordering)); - JacobianFactor::shared_ptr jf2 = boost::static_pointer_cast(jacobian2->linearize(values, ordering)); + JacobianFactor::shared_ptr jf1 = boost::static_pointer_cast(jacobian1.linearize(values)); + JacobianFactor::shared_ptr jf2 = boost::static_pointer_cast(jacobian2->linearize(values)); CHECK(assert_equal(*jf1, *jf2)); Matrix information1 = jf1->augmentedInformation(); @@ -91,9 +85,6 @@ TEST( LinearizedFactor, add_jacobian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - Ordering ordering; - ordering.push_back(x1); - ordering.push_back(x2); Values values; values.insert(x1, Point3(-22.4, +8.5, +2.4)); values.insert(x2, Point3(-21.0, +5.0, +21.0)); @@ -103,10 +94,10 @@ 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 - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); - LinearizedJacobianFactor::shared_ptr jacobian(new LinearizedJacobianFactor(jf, ordering, values)); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); + LinearizedJacobianFactor::shared_ptr jacobian(new LinearizedJacobianFactor(jf, values)); NonlinearFactorGraph graph1; graph1.push_back(jacobian); - NonlinearFactorGraph graph2; graph2.add(*jacobian); + NonlinearFactorGraph graph2; graph2.push_back(*jacobian); // TODO: When creating a Jacobian from a cached factor, I experienced a problem in the 'add' version // However, I am currently unable to reproduce the error in this unit test. @@ -133,8 +124,8 @@ TEST( LinearizedFactor, add_jacobian ) // // // // Create a linearized jacobian factors -// JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); -// LinearizedJacobianFactor jacobian(jf, ordering, values); +// JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); +// LinearizedJacobianFactor jacobian(jf, values); // // // for(double x1 = -10; x1 < 10; x1 += 2.0) { @@ -156,8 +147,8 @@ TEST( LinearizedFactor, add_jacobian ) // EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); // // // Check that the linearized factors are identical -// GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint, ordering); -// GaussianFactor::shared_ptr actual_factor = jacobian.linearize(linpoint, ordering); +// GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint); +// GaussianFactor::shared_ptr actual_factor = jacobian.linearize(linpoint); // CHECK(assert_equal(*expected_factor, *actual_factor)); // } // } @@ -175,9 +166,6 @@ TEST( LinearizedFactor, equals_hessian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - Ordering ordering; - ordering.push_back(x1); - ordering.push_back(x2); Values values; values.insert(x1, Point3(-22.4, +8.5, +2.4)); values.insert(x2, Point3(-21.0, +5.0, +21.0)); @@ -188,10 +176,10 @@ TEST( LinearizedFactor, equals_hessian ) // Create two identical factors and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); HessianFactor::shared_ptr hf(new HessianFactor(*jf)); - LinearizedHessianFactor hessian1(hf, ordering, values); - LinearizedHessianFactor hessian2(hf, ordering, values); + LinearizedHessianFactor hessian1(hf, values); + LinearizedHessianFactor hessian2(hf, values); CHECK(assert_equal(hessian1, hessian2)); } @@ -202,9 +190,6 @@ TEST( LinearizedFactor, clone_hessian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - Ordering ordering; - ordering.push_back(x1); - ordering.push_back(x2); Values values; values.insert(x1, Point3(-22.4, +8.5, +2.4)); values.insert(x2, Point3(-21.0, +5.0, +21.0)); @@ -215,9 +200,9 @@ TEST( LinearizedFactor, clone_hessian ) // Create two identical factors and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); HessianFactor::shared_ptr hf(new HessianFactor(*jf)); - LinearizedHessianFactor hessian1(hf, ordering, values); + LinearizedHessianFactor hessian1(hf, values); LinearizedHessianFactor::shared_ptr hessian2 = boost::static_pointer_cast(hessian1.clone()); CHECK(assert_equal(hessian1, *hessian2)); @@ -229,9 +214,6 @@ TEST( LinearizedFactor, add_hessian ) // Create a Between Factor from a Point3. This is actually a linear factor. Key x1(1); Key x2(2); - Ordering ordering; - ordering.push_back(x1); - ordering.push_back(x2); Values values; values.insert(x1, Point3(-22.4, +8.5, +2.4)); values.insert(x2, Point3(-21.0, +5.0, +21.0)); @@ -242,11 +224,11 @@ TEST( LinearizedFactor, add_hessian ) // Create two identical factors and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); HessianFactor::shared_ptr hf(new HessianFactor(*jf)); - LinearizedHessianFactor::shared_ptr hessian(new LinearizedHessianFactor(hf, ordering, values)); + LinearizedHessianFactor::shared_ptr hessian(new LinearizedHessianFactor(hf, values)); NonlinearFactorGraph graph1; graph1.push_back(hessian); - NonlinearFactorGraph graph2; graph2.add(*hessian); + NonlinearFactorGraph graph2; graph2.push_back(*hessian); CHECK(assert_equal(graph1, graph2)); } @@ -270,9 +252,9 @@ TEST( LinearizedFactor, add_hessian ) // // // // Create a linearized hessian factor -// JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); +// JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); // HessianFactor::shared_ptr hf(new HessianFactor(*jf)); -// LinearizedHessianFactor hessian(hf, ordering, values); +// LinearizedHessianFactor hessian(hf, values); // // // for(double x1 = -10; x1 < 10; x1 += 2.0) { @@ -294,8 +276,8 @@ 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 = HessianFactor::shared_ptr(new HessianFactor(*betweenFactor.linearize(linpoint, ordering))); -// GaussianFactor::shared_ptr actual_factor = hessian.linearize(linpoint, ordering); +// GaussianFactor::shared_ptr expected_factor = HessianFactor::shared_ptr(new HessianFactor(*betweenFactor.linearize(linpoint))); +// GaussianFactor::shared_ptr actual_factor = hessian.linearize(linpoint); // CHECK(assert_equal(*expected_factor, *actual_factor)); // } // } diff --git a/gtsam_unstable/slam/CMakeLists.txt b/gtsam_unstable/slam/CMakeLists.txt index 27ade7a5e..810b7a950 100644 --- a/gtsam_unstable/slam/CMakeLists.txt +++ b/gtsam_unstable/slam/CMakeLists.txt @@ -25,7 +25,15 @@ set (slam_full_libs # Exclude tests that don't work set (slam_excluded_tests - "${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerialization.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tests/testAHRS.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tests/testCombinedImuFactor.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tests/testImuFactor.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tests/testInvDepthFactor3.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tests/testInvDepthFactorVariant1.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tests/testInvDepthFactorVariant2.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tests/testInvDepthFactorVariant3.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tests/testMultiProjectionFactor.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerialization.cpp" # "" # Add to this list, with full path, to exclude ) # Add all tests diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp index 6f4b3b109..62ada7e41 100644 --- a/gtsam_unstable/slam/DummyFactor.cpp +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -7,13 +7,16 @@ #include +#include #include +using namespace boost::assign; + namespace gtsam { /* ************************************************************************* */ DummyFactor::DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2) -: NonlinearFactor(key1, key2) +: NonlinearFactor(cref_list_of<2>(key1)(key2)) { dims_.push_back(dim1); dims_.push_back(dim2); @@ -38,7 +41,7 @@ bool DummyFactor::equals(const NonlinearFactor& f, double tol) const { /* ************************************************************************* */ boost::shared_ptr -DummyFactor::linearize(const Values& c, const Ordering& ordering) const { +DummyFactor::linearize(const Values& c) const { // Only linearize if the factor is active if (!this->active(c)) return boost::shared_ptr(); @@ -46,7 +49,7 @@ DummyFactor::linearize(const Values& c, const Ordering& ordering) const { // Fill in terms with zero matrices std::vector > terms(this->size()); for(size_t j=0; jsize(); ++j) { - terms[j].first = ordering[this->keys()[j]]; + terms[j].first = keys()[j]; terms[j].second = zeros(rowDim_, dims_[j]); } diff --git a/gtsam_unstable/slam/DummyFactor.h b/gtsam_unstable/slam/DummyFactor.h index d2ea2567f..0b0a1bd6c 100644 --- a/gtsam_unstable/slam/DummyFactor.h +++ b/gtsam_unstable/slam/DummyFactor.h @@ -54,8 +54,7 @@ public: virtual size_t dim() const { return rowDim_; } /** linearize to a GaussianFactor */ - virtual boost::shared_ptr - linearize(const Values& c, const Ordering& ordering) const; + virtual boost::shared_ptr linearize(const Values& c) const; /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 47e855b74..000b62aa5 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -147,7 +147,7 @@ namespace gtsam { } /// linearize returns a Hessianfactor that is an approximation of error(p) - virtual boost::shared_ptr linearize(const Values& values, const Ordering& ordering) const { + virtual boost::shared_ptr linearize(const Values& values) const { // std::cout.precision(20); @@ -171,11 +171,6 @@ namespace gtsam { std::vector Gs(keys_.size()*(keys_.size()+1)/2); std::vector gs(keys_.size()); double f = 0; - // fill in the keys - std::vector js; - BOOST_FOREACH(const Key& k, keys_) { - js += ordering[k]; - } bool blockwise = false; @@ -336,7 +331,7 @@ namespace gtsam { // ========================================================================================================== - return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); + return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); } /** diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 9f5d9413a..e9fe1aa3e 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include diff --git a/gtsam_unstable/slam/tests/testDummyFactor.cpp b/gtsam_unstable/slam/tests/testDummyFactor.cpp index c6a1c8bd3..86b4e5584 100644 --- a/gtsam_unstable/slam/tests/testDummyFactor.cpp +++ b/gtsam_unstable/slam/tests/testDummyFactor.cpp @@ -10,10 +10,11 @@ #include -#include - #include +#include +#include + using namespace gtsam; const double tol = 1e-9; @@ -41,15 +42,12 @@ TEST( testDummyFactor, basics ) { // errors - all zeros DOUBLES_EQUAL(0.0, dummyfactor.error(c), tol); - Ordering ordering; - ordering += key1, key2; - // linearization: all zeros - GaussianFactor::shared_ptr actLinearization = dummyfactor.linearize(c, ordering); + GaussianFactor::shared_ptr actLinearization = dummyfactor.linearize(c); CHECK(actLinearization); noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3); GaussianFactor::shared_ptr expLinearization(new JacobianFactor( - ordering[key1], zeros(3,3), ordering[key2], zeros(3,3), zero(3), model3)); + key1, zeros(3,3), key2, zeros(3,3), zero(3), model3)); EXPECT(assert_equal(*expLinearization, *actLinearization, tol)); } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp index 86cb29e8e..71eb1f43d 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -25,11 +25,8 @@ #include #include #include -#include #include #include -#include -#include #include #include #include @@ -40,8 +37,10 @@ #include #include +#include using namespace std; +using namespace boost::assign; using namespace gtsam; // make a realistic calibration matrix @@ -223,8 +222,8 @@ TEST( MultiProjectionFactor, 3poses ){ graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.add(PriorFactor(x1, pose1, noisePrior)); - graph.add(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); // smartFactor1->print("smartFactor1"); diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 8f485738b..7708caf66 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -113,11 +113,11 @@ TEST( SmartRangeFactor, optimization ) { // Create Factor graph NonlinearFactorGraph graph; - graph.add(f); + graph.push_back(f); const noiseModel::Base::shared_ptr // priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI)); - graph.add(PriorFactor(1, pose1, priorNoise)); - graph.add(PriorFactor(2, pose2, priorNoise)); + graph.push_back(PriorFactor(1, pose1, priorNoise)); + graph.push_back(PriorFactor(2, pose2, priorNoise)); // Try optimizing LevenbergMarquardtParams params;