12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Moved summarization back to MastSLAM - functionality largely replaced with newer elimination functionality

release/4.3a0
Alex Cunningham 2013-08-16 16:32:20 +00:00
parent 2d413e05c6
commit 0f3732d7f5
3 changed files with 0 additions and 365 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
gtsam/nonlinear/summarization.cpp
View File

@ -1,79 +0,0 @@
/**
* @file summarization.cpp
*
* @date Jun 22, 2012
* @author Alex Cunningham
*/
#include <gtsam/nonlinear/summarization.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
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())
return full_graph;
std::vector<Key> saved_keys_vec(saved_keys.begin(), saved_keys.end());
// // 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 summarized_system;
// 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)
// SEQUENTIAL_CHOLESKY = 3 /// Uses Cholesky to compute full joint graph (needs fully constrained system)
switch (mode) {
case PARTIAL_QR: {
// summarized_system.push_back(EliminateQR(full_graph, nrEliminatedKeys).second);
break;
}
case PARTIAL_CHOLESKY: {
// summarized_system.push_back(EliminateCholesky(full_graph, nrEliminatedKeys).second);
break;
}
case SEQUENTIAL_QR: {
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: {
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 summarized_system;
}
/* ************************************************************************* */
NonlinearFactorGraph summarizeAsNonlinearContainer(
const NonlinearFactorGraph& graph, const Values& values,
const KeySet& saved_keys, SummarizationMode mode) {
GaussianFactorGraph summarized_graph = summarize(graph, values, saved_keys, mode);
return LinearContainerFactor::convertLinearGraph(summarized_graph);
}
/* ************************************************************************* */
} // \namespace gtsam

56
gtsam/nonlinear/summarization.h
View File

@ -1,56 +0,0 @@
/**
* @file summarization.h
*
* @brief Types and utility functions for summarization
*
* @date Jun 22, 2012
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/dllexport.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
namespace gtsam {
// Flag to control how summarization should be performed
typedef enum {
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)
SEQUENTIAL_CHOLESKY = 3 /// Uses Cholesky to compute full joint graph (needs fully constrained system)
} SummarizationMode;
/**
* Summarization function to remove a subset of variables from a system with the
* sequential solver. This does not require that the system be fully constrained.
*
* Requirement: set of keys in the graph should match the set of keys in the
* values structure.
*
* @param graph A full nonlinear graph
* @param values The chosen linearization point
* @param saved_keys is the set of keys for variables that should remain
* @param mode controls what elimination technique and requirements to use
* @return a pair of the remaining graph and the ordering used for linearization
*/
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
* result as a NonlinearFactorGraph comprised of LinearContainerFactors.
*
* @param graph A full nonlinear graph
* @param values The chosen linearization point
* @param saved_keys is the set of keys for variables that should remain
* @param useQR uses QR as the elimination algorithm if true, Cholesky otherwise
* @return a NonlinearFactorGraph with linear factors
*/
NonlinearFactorGraph GTSAM_EXPORT summarizeAsNonlinearContainer(
const NonlinearFactorGraph& graph, const Values& values,
const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR);
} // \namespace gtsam

230
tests/testSummarization.cpp
View File

@ -1,230 +0,0 @@
/**
* @file testSummarization.cpp
*
* @brief Test ported from MastSLAM for a simple batch summarization technique
*
* @date May 7, 2013
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/set.hpp>
#include <boost/assign/std/vector.hpp>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/LabeledSymbol.h>
#include <gtsam/nonlinear/summarization.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
using namespace std;
using namespace boost::assign;
using namespace gtsam;
const double tol=1e-5;
typedef gtsam::PriorFactor<gtsam::Pose2> PosePrior;
typedef gtsam::BetweenFactor<gtsam::Pose2> PoseBetween;
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2> PosePointBearingRange;
gtsam::noiseModel::Base::shared_ptr model2 = noiseModel::Unit::Create(2);
gtsam::noiseModel::Base::shared_ptr model3 = noiseModel::Unit::Create(3);
/* ************************************************************************* */
TEST( testSummarization, example_from_ddf1 ) {
Key xA0 = LabeledSymbol('x', 'A', 0),
xA1 = LabeledSymbol('x', 'A', 1),
xA2 = LabeledSymbol('x', 'A', 2);
Key lA3 = LabeledSymbol('l', 'A', 3), lA5 = LabeledSymbol('l', 'A', 5);
SharedDiagonal diagmodel2 = noiseModel::Unit::Create(2);
SharedDiagonal diagmodel4 = noiseModel::Unit::Create(4);
Pose2 pose0;
Pose2 pose1(1.0, 0.0, 0.0);
Pose2 pose2(2.0, 0.0, 0.0);
Point2 landmark3(3.0, 3.0);
Point2 landmark5(5.0, 5.0);
Values values;
values.insert(xA0, pose0);
values.insert(xA1, pose1);
values.insert(xA2, pose2);
values.insert(lA3, landmark3);
values.insert(lA5, landmark5);
// build from nonlinear graph/values
NonlinearFactorGraph graph;
graph += PosePrior(xA0, Pose2(), model3);
graph += PoseBetween(xA0, xA1, pose0.between(pose1), model3);
graph += PoseBetween(xA1, xA2, pose1.between(pose2), model3);
graph += PosePointBearingRange(xA0, lA3, pose0.bearing(landmark3), pose0.range(landmark3), model2);
graph += PosePointBearingRange(xA1, lA3, pose1.bearing(landmark3), pose1.range(landmark3), model2);
graph += PosePointBearingRange(xA2, lA5, pose2.bearing(landmark5), pose2.range(landmark5), model2);
KeySet saved_keys;
saved_keys += lA3, lA5;
{
// Summarize to a linear system
GaussianFactorGraph actLinGraph;
SummarizationMode mode = PARTIAL_QR;
actLinGraph = summarize(graph, values, saved_keys, mode);
// Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
// EXPECT(assert_equal(expSumOrdering, actOrdering));
// Does not split out subfactors where possible
GaussianFactorGraph expLinGraph;
expLinGraph += JacobianFactor(
lA3,
Matrix_(4,2,
0.595867, 0.605092,
0.0, -0.406109,
0.0, 0.0,
0.0, 0.0),
lA5,
Matrix_(4,2,
-0.125971, -0.160052,
0.13586, 0.301096,
0.268667, 0.31703,
0.0, -0.131698),
zero(4), diagmodel4);
EXPECT(assert_equal(expLinGraph, actLinGraph, tol));
// Summarize directly from a nonlinear graph to another nonlinear graph
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph);
EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
}
{
// Summarize to a linear system using cholesky - compare to previous version
GaussianFactorGraph actLinGraph;
SummarizationMode mode = PARTIAL_CHOLESKY;
actLinGraph = summarize(graph, values, saved_keys, mode);
// Does not split out subfactors where possible
GaussianFactorGraph expLinGraph;
expLinGraph += HessianFactor(JacobianFactor(
lA3,
Matrix_(4,2,
0.595867, 0.605092,
0.0, -0.406109,
0.0, 0.0,
0.0, 0.0),
lA5,
Matrix_(4,2,
-0.125971, -0.160052,
0.13586, 0.301096,
0.268667, 0.31703,
0.0, -0.131698),
zero(4), diagmodel4));
EXPECT(assert_equal(expLinGraph, actLinGraph, tol));
// Summarize directly from a nonlinear graph to another nonlinear graph
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph);
EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
}
{
// Summarize to a linear system with joint factor graph version
SummarizationMode mode = SEQUENTIAL_QR;
GaussianFactorGraph actLinGraph = summarize(graph, values, saved_keys, mode);
// Does not split out subfactors where possible
GaussianFactorGraph expLinGraph;
expLinGraph += JacobianFactor(
lA3,
Matrix_(2,2,
0.595867, 0.605092,
0.0, 0.406109),
lA5,
Matrix_(2,2,
-0.125971, -0.160052,
-0.13586, -0.301096),
zero(2), diagmodel2);
expLinGraph += JacobianFactor(
lA5,
Matrix_(2,2,
0.268667, 0.31703,
0.0, 0.131698),
zero(2), diagmodel2);
EXPECT(assert_equal(expLinGraph, actLinGraph, tol));
// Summarize directly from a nonlinear graph to another nonlinear graph
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph);
EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
}
{
// Summarize to a linear system with joint factor graph version
SummarizationMode mode = SEQUENTIAL_CHOLESKY;
GaussianFactorGraph actLinGraph = summarize(graph, values, saved_keys, mode);
// Does not split out subfactors where possible
GaussianFactorGraph expLinGraph;
expLinGraph += JacobianFactor(
lA3,
Matrix_(2,2,
0.595867, 0.605092,
0.0, 0.406109),
lA5,
Matrix_(2,2,
-0.125971, -0.160052,
-0.13586, -0.301096),
zero(2), diagmodel2);
expLinGraph += JacobianFactor(
lA5,
Matrix_(2,2,
0.268667, 0.31703,
0.0, 0.131698),
zero(2), diagmodel2);
EXPECT(assert_equal(expLinGraph, actLinGraph, tol));
// Summarize directly from a nonlinear graph to another nonlinear graph
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph);
EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
}
}
/* ************************************************************************* */
TEST( testSummarization, no_summarize_case ) {
// Checks a corner case in which no variables are being eliminated
gtsam::Key key = 7;
gtsam::KeySet saved_keys; saved_keys.insert(key);
NonlinearFactorGraph graph;
graph += PosePrior(key, Pose2(1.0, 2.0, 0.3), model3);
graph += PosePrior(key, Pose2(2.0, 3.0, 0.4), model3);
Values values;
values.insert(key, Pose2(0.0, 0.0, 0.1));
SummarizationMode mode = SEQUENTIAL_CHOLESKY;
GaussianFactorGraph actLinGraph = summarize(graph, values, saved_keys, mode);
GaussianFactorGraph expLinGraph = *graph.linearize(values);
EXPECT(assert_equal(expLinGraph, actLinGraph));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */