Re-enabled summarization test/implementation. Sequential versions re-implemented, but tests don't pass
Alex Cunningham
parent
5b7d7b3793
commit
073ea4fa0f
|
|
@ -5,8 +5,6 @@
|
|||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#if 0
|
||||
|
||||
#include <gtsam/nonlinear/summarization.h>
|
||||
#include <gtsam/nonlinear/LinearContainerFactor.h>
|
||||
|
||||
|
|
@ -15,35 +13,29 @@ using namespace std;
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<GaussianFactorGraph,Ordering>
|
||||
summarize(const NonlinearFactorGraph& graph, const Values& values,
|
||||
GaussianFactorGraph summarize(const NonlinearFactorGraph& graph, const Values& values,
|
||||
const KeySet& saved_keys, SummarizationMode mode) {
|
||||
const size_t nrEliminatedKeys = values.size() - saved_keys.size();
|
||||
GaussianFactorGraph full_graph = *graph.linearize(values);
|
||||
|
||||
// If we aren't eliminating anything, linearize and return
|
||||
if (!nrEliminatedKeys || saved_keys.empty()) {
|
||||
Ordering ordering = *values.orderingArbitrary();
|
||||
GaussianFactorGraph linear_graph = *graph.linearize(values, ordering);
|
||||
return make_pair(linear_graph, ordering);
|
||||
}
|
||||
if (!nrEliminatedKeys || saved_keys.empty())
|
||||
return full_graph;
|
||||
|
||||
// Compute a constrained ordering with variables grouped to end
|
||||
std::map<gtsam::Key, int> ordering_constraints;
|
||||
std::vector<Key> saved_keys_vec(saved_keys.begin(), saved_keys.end());
|
||||
|
||||
// group all saved variables together
|
||||
BOOST_FOREACH(const gtsam::Key& key, saved_keys)
|
||||
ordering_constraints.insert(make_pair(key, 1));
|
||||
|
||||
Ordering ordering = *graph.orderingCOLAMDConstrained(values, ordering_constraints);
|
||||
// // Compute a constrained ordering with variables grouped to end
|
||||
// std::map<gtsam::Key, int> ordering_constraints;
|
||||
//
|
||||
// // group all saved variables together
|
||||
// BOOST_FOREACH(const gtsam::Key& key, saved_keys)
|
||||
// ordering_constraints.insert(make_pair(key, 1));
|
||||
//
|
||||
// Ordering ordering = *graph.orderingCOLAMDConstrained(values, ordering_constraints);
|
||||
|
||||
// Linearize the system
|
||||
GaussianFactorGraph full_graph = *graph.linearize(values, ordering);
|
||||
GaussianFactorGraph summarized_system;
|
||||
|
||||
std::vector<Index> indices;
|
||||
BOOST_FOREACH(const Key& k, saved_keys)
|
||||
indices.push_back(ordering[k]);
|
||||
|
||||
// PARTIAL_QR = 0, /// Uses QR solver to eliminate, does not require fully constrained system
|
||||
// PARTIAL_CHOLESKY = 1, /// Uses Cholesky solver, does not require fully constrained system
|
||||
// SEQUENTIAL_QR = 2, /// Uses QR to compute full joint graph (needs fully constrained system)
|
||||
|
|
@ -51,38 +43,37 @@ summarize(const NonlinearFactorGraph& graph, const Values& values,
|
|||
|
||||
switch (mode) {
|
||||
case PARTIAL_QR: {
|
||||
summarized_system.push_back(EliminateQR(full_graph, nrEliminatedKeys).second);
|
||||
// summarized_system.push_back(EliminateQR(full_graph, nrEliminatedKeys).second);
|
||||
break;
|
||||
}
|
||||
case PARTIAL_CHOLESKY: {
|
||||
summarized_system.push_back(EliminateCholesky(full_graph, nrEliminatedKeys).second);
|
||||
// summarized_system.push_back(EliminateCholesky(full_graph, nrEliminatedKeys).second);
|
||||
break;
|
||||
}
|
||||
case SEQUENTIAL_QR: {
|
||||
GaussianSequentialSolver solver(full_graph, true);
|
||||
summarized_system.push_back(*solver.jointFactorGraph(indices));
|
||||
summarized_system.push_back(*full_graph.marginal(saved_keys_vec, EliminateQR));
|
||||
// GaussianSequentialSolver solver(full_graph, true);
|
||||
// summarized_system.push_back(*solver.jointFactorGraph(indices));
|
||||
break;
|
||||
}
|
||||
case SEQUENTIAL_CHOLESKY: {
|
||||
GaussianSequentialSolver solver(full_graph, false);
|
||||
summarized_system.push_back(*solver.jointFactorGraph(indices));
|
||||
summarized_system.push_back(*full_graph.marginal(saved_keys_vec, EliminateCholesky));
|
||||
// GaussianSequentialSolver solver(full_graph, false);
|
||||
// summarized_system.push_back(*solver.jointFactorGraph(indices));
|
||||
break;
|
||||
}
|
||||
}
|
||||
return make_pair(summarized_system, ordering);
|
||||
return summarized_system;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
NonlinearFactorGraph summarizeAsNonlinearContainer(
|
||||
const NonlinearFactorGraph& graph, const Values& values,
|
||||
const KeySet& saved_keys, SummarizationMode mode) {
|
||||
GaussianFactorGraph summarized_graph;
|
||||
Ordering ordering;
|
||||
boost::tie(summarized_graph, ordering) = summarize(graph, values, saved_keys, mode);
|
||||
return LinearContainerFactor::convertLinearGraph(summarized_graph, ordering);
|
||||
GaussianFactorGraph summarized_graph = summarize(graph, values, saved_keys, mode);
|
||||
return LinearContainerFactor::convertLinearGraph(summarized_graph);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // \namespace gtsam
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -9,13 +9,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#if 0
|
||||
|
||||
#include <gtsam/dllexport.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -40,9 +36,8 @@ typedef enum {
|
|||
* @param mode controls what elimination technique and requirements to use
|
||||
* @return a pair of the remaining graph and the ordering used for linearization
|
||||
*/
|
||||
std::pair<GaussianFactorGraph,Ordering> GTSAM_EXPORT
|
||||
summarize(const NonlinearFactorGraph& graph, const Values& values,
|
||||
const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR);
|
||||
GaussianFactorGraph GTSAM_EXPORT summarize(const NonlinearFactorGraph& graph,
|
||||
const Values& values, const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR);
|
||||
|
||||
/**
|
||||
* Performs the same summarization technique used in summarize(), but returns the
|
||||
|
|
@ -59,5 +54,3 @@ NonlinearFactorGraph GTSAM_EXPORT summarizeAsNonlinearContainer(
|
|||
const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR);
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -9,8 +9,6 @@
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#if 0
|
||||
|
||||
#include <boost/assign/std/set.hpp>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
|
||||
|
|
@ -78,23 +76,23 @@ TEST( testSummarization, example_from_ddf1 ) {
|
|||
|
||||
{
|
||||
// Summarize to a linear system
|
||||
GaussianFactorGraph actLinGraph; Ordering actOrdering;
|
||||
GaussianFactorGraph actLinGraph;
|
||||
SummarizationMode mode = PARTIAL_QR;
|
||||
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
|
||||
actLinGraph = summarize(graph, values, saved_keys, mode);
|
||||
|
||||
Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
|
||||
EXPECT(assert_equal(expSumOrdering, actOrdering));
|
||||
// Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
|
||||
// EXPECT(assert_equal(expSumOrdering, actOrdering));
|
||||
|
||||
// Does not split out subfactors where possible
|
||||
GaussianFactorGraph expLinGraph;
|
||||
expLinGraph += JacobianFactor(
|
||||
expSumOrdering[lA3],
|
||||
lA3,
|
||||
Matrix_(4,2,
|
||||
0.595867, 0.605092,
|
||||
0.0, -0.406109,
|
||||
0.0, 0.0,
|
||||
0.0, 0.0),
|
||||
expSumOrdering[lA5],
|
||||
lA5,
|
||||
Matrix_(4,2,
|
||||
-0.125971, -0.160052,
|
||||
0.13586, 0.301096,
|
||||
|
|
@ -105,30 +103,27 @@ TEST( testSummarization, example_from_ddf1 ) {
|
|||
|
||||
// Summarize directly from a nonlinear graph to another nonlinear graph
|
||||
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
|
||||
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering);
|
||||
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph);
|
||||
|
||||
EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
|
||||
}
|
||||
|
||||
{
|
||||
// Summarize to a linear system using cholesky - compare to previous version
|
||||
GaussianFactorGraph actLinGraph; Ordering actOrdering;
|
||||
GaussianFactorGraph actLinGraph;
|
||||
SummarizationMode mode = PARTIAL_CHOLESKY;
|
||||
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
|
||||
|
||||
Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
|
||||
EXPECT(assert_equal(expSumOrdering, actOrdering));
|
||||
actLinGraph = summarize(graph, values, saved_keys, mode);
|
||||
|
||||
// Does not split out subfactors where possible
|
||||
GaussianFactorGraph expLinGraph;
|
||||
expLinGraph += HessianFactor(JacobianFactor(
|
||||
expSumOrdering[lA3],
|
||||
lA3,
|
||||
Matrix_(4,2,
|
||||
0.595867, 0.605092,
|
||||
0.0, -0.406109,
|
||||
0.0, 0.0,
|
||||
0.0, 0.0),
|
||||
expSumOrdering[lA5],
|
||||
lA5,
|
||||
Matrix_(4,2,
|
||||
-0.125971, -0.160052,
|
||||
0.13586, 0.301096,
|
||||
|
|
@ -139,35 +134,31 @@ TEST( testSummarization, example_from_ddf1 ) {
|
|||
|
||||
// Summarize directly from a nonlinear graph to another nonlinear graph
|
||||
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
|
||||
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering);
|
||||
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph);
|
||||
|
||||
EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
|
||||
}
|
||||
|
||||
{
|
||||
// Summarize to a linear system with joint factor graph version
|
||||
GaussianFactorGraph actLinGraph; Ordering actOrdering;
|
||||
SummarizationMode mode = SEQUENTIAL_QR;
|
||||
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
|
||||
|
||||
Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
|
||||
EXPECT(assert_equal(expSumOrdering, actOrdering));
|
||||
GaussianFactorGraph actLinGraph = summarize(graph, values, saved_keys, mode);
|
||||
|
||||
// Does not split out subfactors where possible
|
||||
GaussianFactorGraph expLinGraph;
|
||||
expLinGraph += JacobianFactor(
|
||||
expSumOrdering[lA3],
|
||||
lA3,
|
||||
Matrix_(2,2,
|
||||
0.595867, 0.605092,
|
||||
0.0, 0.406109),
|
||||
expSumOrdering[lA5],
|
||||
lA5,
|
||||
Matrix_(2,2,
|
||||
-0.125971, -0.160052,
|
||||
-0.13586, -0.301096),
|
||||
zero(2), diagmodel2);
|
||||
|
||||
expLinGraph += JacobianFactor(
|
||||
expSumOrdering[lA5],
|
||||
lA5,
|
||||
Matrix_(2,2,
|
||||
0.268667, 0.31703,
|
||||
0.0, 0.131698),
|
||||
|
|
@ -177,35 +168,31 @@ TEST( testSummarization, example_from_ddf1 ) {
|
|||
|
||||
// Summarize directly from a nonlinear graph to another nonlinear graph
|
||||
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
|
||||
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering);
|
||||
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph);
|
||||
|
||||
EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
|
||||
}
|
||||
|
||||
{
|
||||
// Summarize to a linear system with joint factor graph version
|
||||
GaussianFactorGraph actLinGraph; Ordering actOrdering;
|
||||
SummarizationMode mode = SEQUENTIAL_CHOLESKY;
|
||||
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
|
||||
|
||||
Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
|
||||
EXPECT(assert_equal(expSumOrdering, actOrdering));
|
||||
GaussianFactorGraph actLinGraph = summarize(graph, values, saved_keys, mode);
|
||||
|
||||
// Does not split out subfactors where possible
|
||||
GaussianFactorGraph expLinGraph;
|
||||
expLinGraph += JacobianFactor(
|
||||
expSumOrdering[lA3],
|
||||
lA3,
|
||||
Matrix_(2,2,
|
||||
0.595867, 0.605092,
|
||||
0.0, 0.406109),
|
||||
expSumOrdering[lA5],
|
||||
lA5,
|
||||
Matrix_(2,2,
|
||||
-0.125971, -0.160052,
|
||||
-0.13586, -0.301096),
|
||||
zero(2), diagmodel2);
|
||||
|
||||
expLinGraph += JacobianFactor(
|
||||
expSumOrdering[lA5],
|
||||
lA5,
|
||||
Matrix_(2,2,
|
||||
0.268667, 0.31703,
|
||||
0.0, 0.131698),
|
||||
|
|
@ -215,7 +202,7 @@ TEST( testSummarization, example_from_ddf1 ) {
|
|||
|
||||
// Summarize directly from a nonlinear graph to another nonlinear graph
|
||||
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
|
||||
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering);
|
||||
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph);
|
||||
|
||||
EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
|
||||
}
|
||||
|
|
@ -233,16 +220,11 @@ TEST( testSummarization, no_summarize_case ) {
|
|||
values.insert(key, Pose2(0.0, 0.0, 0.1));
|
||||
|
||||
SummarizationMode mode = SEQUENTIAL_CHOLESKY;
|
||||
GaussianFactorGraph actLinGraph; Ordering actOrdering;
|
||||
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
|
||||
Ordering expOrdering; expOrdering += key;
|
||||
GaussianFactorGraph expLinGraph = *graph.linearize(values, expOrdering);
|
||||
EXPECT(assert_equal(expOrdering, actOrdering));
|
||||
GaussianFactorGraph actLinGraph = summarize(graph, values, saved_keys, mode);
|
||||
GaussianFactorGraph expLinGraph = *graph.linearize(values);
|
||||
EXPECT(assert_equal(expLinGraph, actLinGraph));
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue