Updated unit tests for changes in the Concurrent Filtering and Smoothing classes. Currently the synchronization tests are disabled.
Stephen Williams
parent
0b5c07e543
commit
ed560aa13a
|
|
@ -17,24 +17,20 @@
|
|||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
|
||||
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
namespace {
|
||||
|
||||
// Set up initial pose, odometry difference, loop closure difference, and initialization errors
|
||||
const Pose3 poseInitial;
|
||||
|
|
@ -49,7 +45,7 @@ const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0
|
|||
// Create a derived class to allow testing protected member functions
|
||||
class ConcurrentBatchFilterTester : public ConcurrentBatchFilter {
|
||||
public:
|
||||
ConcurrentBatchFilterTester(const LevenbergMarquardtParams& parameters, double lag) : ConcurrentBatchFilter(parameters, lag) { };
|
||||
ConcurrentBatchFilterTester(const LevenbergMarquardtParams& parameters) : ConcurrentBatchFilter(parameters) { };
|
||||
virtual ~ConcurrentBatchFilterTester() { };
|
||||
|
||||
// Add accessors to the protected members
|
||||
|
|
@ -103,7 +99,7 @@ bool hessian_equal(const NonlinearFactorGraph& expected, const NonlinearFactorGr
|
|||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFilter::KeyTimestampMap& timestamps, size_t index1 = 0, size_t index2 = 1) {
|
||||
void CreateFactors(NonlinearFactorGraph& graph, Values& theta, size_t index1 = 0, size_t index2 = 1) {
|
||||
|
||||
// Calculate all poses
|
||||
Pose3 poses[20];
|
||||
|
|
@ -128,7 +124,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(PriorFactor<Pose3>(keys[0], poses[0], noisePrior));
|
||||
// Add new variables
|
||||
theta.insert(keys[0], poses[0].compose(poseError));
|
||||
timestamps[keys[0]] = double(0);
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
|
|
@ -138,7 +133,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(BetweenFactor<Pose3>(keys[0], keys[1], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[1], poses[1].compose(poseError));
|
||||
timestamps[keys[1]] = double(1);
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
|
|
@ -155,9 +149,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(BetweenFactor<Pose3>(keys[2], keys[3], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[2], poses[2].compose(poseError));
|
||||
timestamps[keys[2]] = double(2);
|
||||
theta.insert(keys[3], poses[3].compose(poseError));
|
||||
timestamps[keys[3]] = double(3);
|
||||
break;
|
||||
}
|
||||
case 4:
|
||||
|
|
@ -171,7 +163,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(BetweenFactor<Pose3>(keys[3], keys[5], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[5], poses[5].compose(poseError));
|
||||
timestamps[keys[5]] = double(5);
|
||||
break;
|
||||
}
|
||||
case 6:
|
||||
|
|
@ -184,7 +175,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(BetweenFactor<Pose3>(keys[5], keys[6], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[6], poses[6].compose(poseError));
|
||||
timestamps[keys[6]] = double(6);
|
||||
break;
|
||||
}
|
||||
case 7:
|
||||
|
|
@ -197,9 +187,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(BetweenFactor<Pose3>(keys[6], keys[7], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[4], poses[4].compose(poseError));
|
||||
timestamps[keys[4]] = double(4);
|
||||
theta.insert(keys[7], poses[7].compose(poseError));
|
||||
timestamps[keys[7]] = double(7);
|
||||
break;
|
||||
}
|
||||
case 8:
|
||||
|
|
@ -218,9 +206,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(BetweenFactor<Pose3>(keys[8], keys[9], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[8], poses[8].compose(poseError));
|
||||
timestamps[keys[8]] = double(8);
|
||||
theta.insert(keys[9], poses[9].compose(poseError));
|
||||
timestamps[keys[9]] = double(9);
|
||||
break;
|
||||
}
|
||||
case 10:
|
||||
|
|
@ -230,7 +216,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(BetweenFactor<Pose3>(keys[9], keys[10], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[10], poses[10].compose(poseError));
|
||||
timestamps[keys[10]] = double(10);
|
||||
break;
|
||||
}
|
||||
case 11:
|
||||
|
|
@ -240,7 +225,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(BetweenFactor<Pose3>(keys[10], keys[11], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[11], poses[11].compose(poseError));
|
||||
timestamps[keys[11]] = double(11);
|
||||
break;
|
||||
}
|
||||
case 12:
|
||||
|
|
@ -253,7 +237,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(BetweenFactor<Pose3>(keys[9], keys[12], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[12], poses[12].compose(poseError));
|
||||
timestamps[keys[12]] = double(12);
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -271,7 +254,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(BetweenFactor<Pose3>(keys[12], keys[13], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[13], poses[13].compose(poseError));
|
||||
timestamps[keys[13]] = double(13);
|
||||
break;
|
||||
}
|
||||
case 14:
|
||||
|
|
@ -284,7 +266,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(BetweenFactor<Pose3>(keys[13], keys[14], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[14], poses[14].compose(poseError));
|
||||
timestamps[keys[14]] = double(14);
|
||||
break;
|
||||
}
|
||||
case 15:
|
||||
|
|
@ -303,9 +284,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(BetweenFactor<Pose3>(keys[15], keys[16], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[15], poses[15].compose(poseError));
|
||||
timestamps[keys[15]] = double(15);
|
||||
theta.insert(keys[16], poses[16].compose(poseError));
|
||||
timestamps[keys[16]] = double(16);
|
||||
break;
|
||||
}
|
||||
case 17:
|
||||
|
|
@ -315,7 +294,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(BetweenFactor<Pose3>(keys[16], keys[17], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[17], poses[17].compose(poseError));
|
||||
timestamps[keys[17]] = double(17);
|
||||
break;
|
||||
}
|
||||
case 18:
|
||||
|
|
@ -325,7 +303,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(BetweenFactor<Pose3>(keys[17], keys[18], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[18], poses[18].compose(poseError));
|
||||
timestamps[keys[18]] = double(18);
|
||||
break;
|
||||
}
|
||||
case 19:
|
||||
|
|
@ -338,7 +315,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
graph.add(BetweenFactor<Pose3>(keys[16], keys[19], poseDelta, noiseOdometery));
|
||||
// Add new variables
|
||||
theta.insert(keys[19], poses[19].compose(poseError));
|
||||
timestamps[keys[19]] = double(19);
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -349,7 +325,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, const Values& rootValues = Values()) {
|
||||
Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, const Values& separatorValues = Values()) {
|
||||
|
||||
// Create an L-M optimizer
|
||||
LevenbergMarquardtParams parameters;
|
||||
|
|
@ -361,9 +337,9 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, con
|
|||
double currentError;
|
||||
do {
|
||||
// Force variables associated with root keys to keep the same linearization point
|
||||
if(rootValues.size() > 0) {
|
||||
if(separatorValues.size() > 0) {
|
||||
// Put the old values of the root keys back into the optimizer state
|
||||
optimizer.state().values.update(rootValues);
|
||||
optimizer.state().values.update(separatorValues);
|
||||
// Update the error value with the new theta
|
||||
optimizer.state().error = graph.error(optimizer.state().values);
|
||||
}
|
||||
|
|
@ -380,60 +356,60 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, con
|
|||
return optimizer.values();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void FindFactorsWithAny(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) {
|
||||
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) {
|
||||
NonlinearFactor::const_iterator key = factor->begin();
|
||||
while((key != factor->end()) && (!std::binary_search(keys.begin(), keys.end(), *key))) {
|
||||
++key;
|
||||
}
|
||||
if(key != factor->end()) {
|
||||
destinationFactors.push_back(factor);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
///* ************************************************************************* */
|
||||
//void FindFactorsWithAny(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) {
|
||||
//
|
||||
// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) {
|
||||
// NonlinearFactor::const_iterator key = factor->begin();
|
||||
// while((key != factor->end()) && (!std::binary_search(keys.begin(), keys.end(), *key))) {
|
||||
// ++key;
|
||||
// }
|
||||
// if(key != factor->end()) {
|
||||
// destinationFactors.push_back(factor);
|
||||
// }
|
||||
// }
|
||||
//
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//void FindFactorsWithOnly(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) {
|
||||
//
|
||||
// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) {
|
||||
// NonlinearFactor::const_iterator key = factor->begin();
|
||||
// while((key != factor->end()) && (std::binary_search(keys.begin(), keys.end(), *key))) {
|
||||
// ++key;
|
||||
// }
|
||||
// if(key == factor->end()) {
|
||||
// destinationFactors.push_back(factor);
|
||||
// }
|
||||
// }
|
||||
//
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//typedef BayesTree<GaussianConditional,ISAM2Clique>::sharedClique Clique;
|
||||
//void SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent = "") {
|
||||
// std::cout << indent << "P( ";
|
||||
// BOOST_FOREACH(Index index, clique->conditional()->frontals()){
|
||||
// std::cout << DefaultKeyFormatter(ordering.key(index)) << " ";
|
||||
// }
|
||||
// if(clique->conditional()->nrParents() > 0) {
|
||||
// std::cout << "| ";
|
||||
// }
|
||||
// BOOST_FOREACH(Index index, clique->conditional()->parents()){
|
||||
// std::cout << DefaultKeyFormatter(ordering.key(index)) << " ";
|
||||
// }
|
||||
// std::cout << ")" << std::endl;
|
||||
//
|
||||
// BOOST_FOREACH(const Clique& child, clique->children()) {
|
||||
// SymbolicPrintTree(child, ordering, indent+" ");
|
||||
// }
|
||||
//}
|
||||
//
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void FindFactorsWithOnly(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) {
|
||||
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) {
|
||||
NonlinearFactor::const_iterator key = factor->begin();
|
||||
while((key != factor->end()) && (std::binary_search(keys.begin(), keys.end(), *key))) {
|
||||
++key;
|
||||
}
|
||||
if(key == factor->end()) {
|
||||
destinationFactors.push_back(factor);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
typedef BayesTree<GaussianConditional,ISAM2Clique>::sharedClique Clique;
|
||||
void SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent = "") {
|
||||
std::cout << indent << "P( ";
|
||||
BOOST_FOREACH(Index index, clique->conditional()->frontals()){
|
||||
std::cout << DefaultKeyFormatter(ordering.key(index)) << " ";
|
||||
}
|
||||
if(clique->conditional()->nrParents() > 0) {
|
||||
std::cout << "| ";
|
||||
}
|
||||
BOOST_FOREACH(Index index, clique->conditional()->parents()){
|
||||
std::cout << DefaultKeyFormatter(ordering.key(index)) << " ";
|
||||
}
|
||||
std::cout << ")" << std::endl;
|
||||
|
||||
BOOST_FOREACH(const Clique& child, clique->children()) {
|
||||
SymbolicPrintTree(child, ordering, indent+" ");
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE( ConcurrentBatchFilter, update_Batch )
|
||||
TEST_UNSAFE( ConcurrentBatchFilter, update_batch )
|
||||
{
|
||||
// Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment.
|
||||
// Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical
|
||||
|
|
@ -441,23 +417,22 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_Batch )
|
|||
|
||||
// Create a set of optimizer parameters
|
||||
LevenbergMarquardtParams parameters;
|
||||
double lag = 4.0;
|
||||
|
||||
// Create a Concurrent Batch Filter
|
||||
ConcurrentBatchFilter filter(parameters, lag);
|
||||
ConcurrentBatchFilter filter(parameters);
|
||||
|
||||
// Create containers to keep the full graph
|
||||
Values fullTheta;
|
||||
NonlinearFactorGraph fullGraph;
|
||||
ConcurrentBatchFilter::KeyTimestampMap fullTimestamps;
|
||||
|
||||
// Create all factors
|
||||
CreateFactors(fullGraph, fullTheta, fullTimestamps, 0, 20);
|
||||
CreateFactors(fullGraph, fullTheta, 0, 20);
|
||||
|
||||
// Optimize with Concurrent Batch Filter
|
||||
filter.update(fullGraph, fullTheta, fullTimestamps);
|
||||
filter.update(fullGraph, fullTheta, boost::none);
|
||||
Values actual = filter.calculateEstimate();
|
||||
|
||||
|
||||
// Optimize with L-M
|
||||
Values expected = BatchOptimize(fullGraph, fullTheta);
|
||||
|
||||
|
|
@ -465,8 +440,50 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_Batch )
|
|||
CHECK(assert_equal(expected, actual, 1e-4));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//TEST_UNSAFE( ConcurrentBatchFilter, update_batch_with_marginalization )
|
||||
//{
|
||||
// // Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment.
|
||||
// // Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical
|
||||
// // This tests adds all of the factors to the filter at once (i.e. batch)
|
||||
//
|
||||
// // Create a set of optimizer parameters
|
||||
// LevenbergMarquardtParams parameters;
|
||||
//
|
||||
// // Create a Concurrent Batch Filter
|
||||
// ConcurrentBatchFilter filter(parameters);
|
||||
//
|
||||
// // Create containers to keep the full graph
|
||||
// Values fullTheta;
|
||||
// NonlinearFactorGraph fullGraph;
|
||||
//
|
||||
// // Create all factors
|
||||
// CreateFactors(fullGraph, fullTheta, 0, 20);
|
||||
//
|
||||
// // Create the set of key to marginalize out
|
||||
// FastList<Key> marginalizeKeys;
|
||||
// for(size_t j = 0; j < 15; ++j) {
|
||||
// marginalizeKeys.push_back(Symbol('X', j));
|
||||
// }
|
||||
//
|
||||
// // Optimize with Concurrent Batch Filter
|
||||
// filter.update(fullGraph, fullTheta, marginalizeKeys);
|
||||
// Values actual = filter.calculateEstimate();
|
||||
//
|
||||
//
|
||||
// // Optimize with L-M
|
||||
// Values expected = BatchOptimize(fullGraph, fullTheta);
|
||||
// // Remove the marginalized keys
|
||||
// for(size_t j = 0; j < 15; ++j) {
|
||||
// expected.erase(Symbol('X', j));
|
||||
// }
|
||||
//
|
||||
// // Check smoother versus batch
|
||||
// CHECK(assert_equal(expected, actual, 1e-4));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE( ConcurrentBatchFilter, update_Incremental )
|
||||
TEST_UNSAFE( ConcurrentBatchFilter, update_incremental )
|
||||
{
|
||||
// Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment.
|
||||
// Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical
|
||||
|
|
@ -474,10 +491,9 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_Incremental )
|
|||
|
||||
// Create a set of optimizer parameters
|
||||
LevenbergMarquardtParams parameters;
|
||||
double lag = 4.0;
|
||||
|
||||
// Create a Concurrent Batch Filter
|
||||
ConcurrentBatchFilter filter(parameters, lag);
|
||||
ConcurrentBatchFilter filter(parameters);
|
||||
|
||||
// Create containers to keep the full graph
|
||||
Values fullTheta;
|
||||
|
|
@ -488,13 +504,12 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_Incremental )
|
|||
// Create containers to keep the new factors
|
||||
Values newTheta;
|
||||
NonlinearFactorGraph newGraph;
|
||||
ConcurrentBatchFilter::KeyTimestampMap newTimestamps;
|
||||
|
||||
// Create factors
|
||||
CreateFactors(newGraph, newTheta, newTimestamps, i, i+1);
|
||||
CreateFactors(newGraph, newTheta, i, i+1);
|
||||
|
||||
// Add these entries to the filter
|
||||
filter.update(newGraph, newTheta, newTimestamps);
|
||||
filter.update(newGraph, newTheta, boost::none);
|
||||
Values actual = filter.calculateEstimate();
|
||||
|
||||
// Add these entries to the full batch version
|
||||
|
|
@ -509,278 +524,330 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_Incremental )
|
|||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE( ConcurrentBatchFilter, synchronize )
|
||||
{
|
||||
// Test the 'synchronize' function of the ConcurrentBatchFilter in a nonlinear environment.
|
||||
// The filter is operating on a known tree structure, so the factors and summarization can
|
||||
// be predicted for testing purposes
|
||||
///* ************************************************************************* */
|
||||
//TEST_UNSAFE( ConcurrentBatchFilter, update_incremental_with_marginalization )
|
||||
//{
|
||||
// // Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment.
|
||||
// // Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical
|
||||
// // This tests adds the factors to the filter as they are created (i.e. incrementally)
|
||||
//
|
||||
// // Create a set of optimizer parameters
|
||||
// LevenbergMarquardtParams parameters;
|
||||
//
|
||||
// // Create a Concurrent Batch Filter
|
||||
// ConcurrentBatchFilter filter(parameters);
|
||||
//
|
||||
// // Create containers to keep the full graph
|
||||
// Values fullTheta;
|
||||
// NonlinearFactorGraph fullGraph;
|
||||
//
|
||||
// // Add odometry from time 0 to time 10
|
||||
// for(size_t i = 0; i < 20; ++i) {
|
||||
// // Create containers to keep the new factors
|
||||
// Values newTheta;
|
||||
// NonlinearFactorGraph newGraph;
|
||||
//
|
||||
// // Create factors
|
||||
// CreateFactors(newGraph, newTheta, i, i+1);
|
||||
//
|
||||
// // Create the set of factors to marginalize
|
||||
// FastList<Key> marginalizeKeys;
|
||||
// if(i >= 4) {
|
||||
// marginalizeKeys.push_back(Symbol('X', i-4));
|
||||
// }
|
||||
//
|
||||
// // Add these entries to the filter
|
||||
// filter.update(newGraph, newTheta, marginalizeKeys);
|
||||
// Values actual = filter.calculateEstimate();
|
||||
//
|
||||
// // Add these entries to the full batch version
|
||||
// fullGraph.push_back(newGraph);
|
||||
// fullTheta.insert(newTheta);
|
||||
// Values expected = BatchOptimize(fullGraph, fullTheta);
|
||||
// fullTheta = expected;
|
||||
// // Remove marginalized keys
|
||||
// for(int j = 0; j < (int)i - 4; ++j) {
|
||||
// expected.erase(Symbol('X', j));
|
||||
// }
|
||||
//
|
||||
// // Compare filter solution with full batch
|
||||
// CHECK(assert_equal(expected, actual, 1e-4));
|
||||
// }
|
||||
//
|
||||
//}
|
||||
|
||||
// Create a set of optimizer parameters
|
||||
LevenbergMarquardtParams parameters;
|
||||
double lag = 4.0;
|
||||
|
||||
// Create a Concurrent Batch Filter
|
||||
ConcurrentBatchFilterTester filter(parameters, lag);
|
||||
|
||||
// Create containers to keep the full graph
|
||||
Values newTheta, fullTheta;
|
||||
NonlinearFactorGraph newGraph, fullGraph;
|
||||
ConcurrentBatchFilter::KeyTimestampMap newTimestamps;
|
||||
|
||||
// Create all factors
|
||||
CreateFactors(newGraph, newTheta, newTimestamps, 0, 13);
|
||||
fullTheta.insert(newTheta);
|
||||
fullGraph.push_back(newGraph);
|
||||
|
||||
// Optimize with Concurrent Batch Filter
|
||||
filter.update(newGraph, newTheta, newTimestamps);
|
||||
Values updatedTheta = filter.calculateEstimate();
|
||||
|
||||
// Eliminate the factor graph into a Bayes Tree to create the summarized factors
|
||||
// Create Ordering
|
||||
std::map<Key, int> constraints;
|
||||
constraints[Symbol('X', 8)] = 1;
|
||||
constraints[Symbol('X', 9)] = 1;
|
||||
constraints[Symbol('X', 10)] = 1;
|
||||
constraints[Symbol('X', 11)] = 1;
|
||||
constraints[Symbol('X', 12)] = 1;
|
||||
Ordering ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints);
|
||||
// Linearize into a Gaussian Factor Graph
|
||||
GaussianFactorGraph linearGraph = *fullGraph.linearize(fullTheta, ordering);
|
||||
// Eliminate into a Bayes Net with iSAM2-type cliques
|
||||
JunctionTree<GaussianFactorGraph, ISAM2Clique> jt(linearGraph);
|
||||
ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR);
|
||||
BayesTree<GaussianConditional, ISAM2Clique> bayesTree;
|
||||
bayesTree.insert(root);
|
||||
|
||||
// std::cout << "BAYES TREE:" << std::endl;
|
||||
// SymbolicPrintTree(root, ordering.invert(), " ");
|
||||
|
||||
// BAYES TREE:
|
||||
// P( X7 X9 X12 )
|
||||
// P( X10 | X9 )
|
||||
// P( X11 | X10 )
|
||||
// P( X8 | X9 )
|
||||
// P( X6 | X7 X9 )
|
||||
// P( X3 X5 | X6 )
|
||||
// P( X2 | X3 )
|
||||
// P( X1 | X3 )
|
||||
// P( X0 | X1 )
|
||||
// P( X4 | X7 )
|
||||
|
||||
// Extract the nonlinear factors that should be sent to the smoother
|
||||
std::vector<Key> smootherKeys;
|
||||
smootherKeys.push_back(Symbol('X', 0));
|
||||
smootherKeys.push_back(Symbol('X', 1));
|
||||
smootherKeys.push_back(Symbol('X', 2));
|
||||
smootherKeys.push_back(Symbol('X', 3));
|
||||
smootherKeys.push_back(Symbol('X', 4));
|
||||
smootherKeys.push_back(Symbol('X', 5));
|
||||
smootherKeys.push_back(Symbol('X', 6));
|
||||
NonlinearFactorGraph expectedSmootherFactors;
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
|
||||
if(std::find_first_of(factor->begin(), factor->end(), smootherKeys.begin(), smootherKeys.end()) != factor->end()) {
|
||||
expectedSmootherFactors.push_back(factor);
|
||||
}
|
||||
}
|
||||
|
||||
// Extract smoother values
|
||||
Values expectedSmootherValues;
|
||||
expectedSmootherValues.insert(Symbol('X', 0), updatedTheta.at(Symbol('X', 0)));
|
||||
expectedSmootherValues.insert(Symbol('X', 1), updatedTheta.at(Symbol('X', 1)));
|
||||
expectedSmootherValues.insert(Symbol('X', 2), updatedTheta.at(Symbol('X', 2)));
|
||||
expectedSmootherValues.insert(Symbol('X', 3), updatedTheta.at(Symbol('X', 3)));
|
||||
expectedSmootherValues.insert(Symbol('X', 4), updatedTheta.at(Symbol('X', 4)));
|
||||
expectedSmootherValues.insert(Symbol('X', 5), updatedTheta.at(Symbol('X', 5)));
|
||||
expectedSmootherValues.insert(Symbol('X', 6), updatedTheta.at(Symbol('X', 6)));
|
||||
|
||||
// Extract the filter summarized factors
|
||||
// Cached factors that represent the filter side (i.e. the X8 and X10 clique)
|
||||
NonlinearFactorGraph expectedFilterSumarization;
|
||||
expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 8)))->cachedFactor()), ordering, fullTheta));
|
||||
expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 10)))->cachedFactor()), ordering, fullTheta));
|
||||
// And any factors that involve only the root (X7, X9, X12)
|
||||
std::vector<Key> rootKeys;
|
||||
rootKeys.push_back(Symbol('X', 7));
|
||||
rootKeys.push_back(Symbol('X', 9));
|
||||
rootKeys.push_back(Symbol('X', 12));
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
|
||||
size_t count = 0;
|
||||
BOOST_FOREACH(Key key, *factor) {
|
||||
if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count;
|
||||
}
|
||||
if(count == factor->size()) expectedFilterSumarization.push_back(factor);
|
||||
}
|
||||
|
||||
// Extract the new root values
|
||||
Values expectedRootValues;
|
||||
expectedRootValues.insert(Symbol('X', 7), updatedTheta.at(Symbol('X', 7)));
|
||||
expectedRootValues.insert(Symbol('X', 9), updatedTheta.at(Symbol('X', 9)));
|
||||
expectedRootValues.insert(Symbol('X', 12), updatedTheta.at(Symbol('X', 12)));
|
||||
|
||||
|
||||
|
||||
// Start the synchronization process
|
||||
NonlinearFactorGraph actualSmootherFactors, actualFilterSumarization, smootherSummarization;
|
||||
Values actualSmootherValues, actualRootValues;
|
||||
filter.presync();
|
||||
filter.synchronize(smootherSummarization); // Supplying an empty factor graph here
|
||||
filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues);
|
||||
filter.getSummarizedFactors(actualFilterSumarization, actualRootValues);
|
||||
filter.postsync();
|
||||
|
||||
|
||||
|
||||
// Compare filter sync variables versus the expected
|
||||
CHECK(hessian_equal(expectedSmootherFactors, actualSmootherFactors, updatedTheta, 1e-8));
|
||||
CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-4));
|
||||
CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, updatedTheta, 1e-9));
|
||||
CHECK(assert_equal(expectedRootValues, actualRootValues, 1e-4));
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// Now add additional factors to the filter and re-sync
|
||||
newGraph.resize(0); newTheta.clear(); newTimestamps.clear();
|
||||
CreateFactors(newGraph, newTheta, newTimestamps, 13, 20);
|
||||
fullTheta.insert(newTheta);
|
||||
fullGraph.push_back(newGraph);
|
||||
|
||||
// Optimize with Concurrent Batch Filter
|
||||
filter.update(newGraph, newTheta, newTimestamps);
|
||||
updatedTheta = filter.calculateEstimate();
|
||||
|
||||
// Eliminate the factor graph into a Bayes Tree to create the summarized factors
|
||||
// Create Ordering
|
||||
constraints.clear();
|
||||
constraints[Symbol('X', 15)] = 1;
|
||||
constraints[Symbol('X', 16)] = 1;
|
||||
constraints[Symbol('X', 17)] = 1;
|
||||
constraints[Symbol('X', 18)] = 1;
|
||||
constraints[Symbol('X', 19)] = 1;
|
||||
ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints);
|
||||
// Linearize into a Gaussian Factor Graph
|
||||
linearGraph = *fullGraph.linearize(fullTheta, ordering);
|
||||
// Eliminate into a Bayes Net with iSAM2-type cliques
|
||||
jt = JunctionTree<GaussianFactorGraph, ISAM2Clique>(linearGraph);
|
||||
root = jt.eliminate(EliminateQR);
|
||||
bayesTree = BayesTree<GaussianConditional, ISAM2Clique>();
|
||||
bayesTree.insert(root);
|
||||
|
||||
// std::cout << "BAYES TREE:" << std::endl;
|
||||
// SymbolicPrintTree(root, ordering.invert(), " ");
|
||||
|
||||
// BAYES TREE:
|
||||
// P( X14 X16 X19 )
|
||||
// P( X17 | X16 )
|
||||
// P( X18 | X17 )
|
||||
// P( X15 | X16 )
|
||||
// P( X13 | X14 X16 )
|
||||
// P( X11 | X13 X14 )
|
||||
// P( X10 | X11 X13 )
|
||||
// P( X12 | X10 X13 )
|
||||
// P( X9 | X12 X10 )
|
||||
// P( X7 | X9 X12 )
|
||||
// P( X6 | X7 X9 )
|
||||
// P( X3 X5 | X6 )
|
||||
// P( X2 | X3 )
|
||||
// P( X1 | X3 )
|
||||
// P( X0 | X1 )
|
||||
// P( X4 | X7 )
|
||||
// P( X8 | X9 )
|
||||
|
||||
// Extract the cached factors for X4 and X6. These are the new smoother summarized factors
|
||||
// TODO: I'm concerned about the linearization point used to create these factors. It may need to be the updated lin points?
|
||||
smootherSummarization.resize(0);
|
||||
smootherSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 4)))->cachedFactor()), ordering, fullTheta));
|
||||
smootherSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 6)))->cachedFactor()), ordering, fullTheta));
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// Extract the nonlinear factors that should be sent to the smoother
|
||||
smootherKeys.clear();
|
||||
smootherKeys.push_back(Symbol('X', 8));
|
||||
smootherKeys.push_back(Symbol('X', 10));
|
||||
smootherKeys.push_back(Symbol('X', 11));
|
||||
smootherKeys.push_back(Symbol('X', 13));
|
||||
expectedSmootherFactors.resize(0);
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
|
||||
if(std::find_first_of(factor->begin(), factor->end(), smootherKeys.begin(), smootherKeys.end()) != factor->end()) {
|
||||
expectedSmootherFactors.push_back(factor);
|
||||
}
|
||||
}
|
||||
// And any factors that involve only the old root (X7, X9, X12)
|
||||
rootKeys.clear();
|
||||
rootKeys.push_back(Symbol('X', 7));
|
||||
rootKeys.push_back(Symbol('X', 9));
|
||||
rootKeys.push_back(Symbol('X', 12));
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
|
||||
size_t count = 0;
|
||||
BOOST_FOREACH(Key key, *factor) {
|
||||
if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count;
|
||||
}
|
||||
if(count == factor->size()) expectedSmootherFactors.push_back(factor);
|
||||
}
|
||||
|
||||
|
||||
// Extract smoother Values
|
||||
expectedSmootherValues.clear();
|
||||
expectedSmootherValues.insert(Symbol('X', 7), updatedTheta.at(Symbol('X', 7)));
|
||||
expectedSmootherValues.insert(Symbol('X', 8), updatedTheta.at(Symbol('X', 8)));
|
||||
expectedSmootherValues.insert(Symbol('X', 9), updatedTheta.at(Symbol('X', 9)));
|
||||
expectedSmootherValues.insert(Symbol('X', 10), updatedTheta.at(Symbol('X', 10)));
|
||||
expectedSmootherValues.insert(Symbol('X', 11), updatedTheta.at(Symbol('X', 11)));
|
||||
expectedSmootherValues.insert(Symbol('X', 12), updatedTheta.at(Symbol('X', 12)));
|
||||
expectedSmootherValues.insert(Symbol('X', 13), updatedTheta.at(Symbol('X', 13)));
|
||||
|
||||
// Extract the filter summarized factors
|
||||
// Cached factors that represent the filter side (i.e. the X15 and X17 clique)
|
||||
expectedFilterSumarization.resize(0);
|
||||
expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 15)))->cachedFactor()), ordering, fullTheta));
|
||||
expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 17)))->cachedFactor()), ordering, fullTheta));
|
||||
// And any factors that involve only the root (X14, X16, X17)
|
||||
rootKeys.clear();
|
||||
rootKeys.push_back(Symbol('X', 14));
|
||||
rootKeys.push_back(Symbol('X', 16));
|
||||
rootKeys.push_back(Symbol('X', 19));
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
|
||||
size_t count = 0;
|
||||
BOOST_FOREACH(Key key, *factor) {
|
||||
if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count;
|
||||
}
|
||||
if(count == factor->size()) expectedFilterSumarization.push_back(factor);
|
||||
}
|
||||
|
||||
// Extract the new root keys
|
||||
expectedRootValues.clear();
|
||||
expectedRootValues.insert(Symbol('X', 14), updatedTheta.at(Symbol('X', 14)));
|
||||
expectedRootValues.insert(Symbol('X', 16), updatedTheta.at(Symbol('X', 16)));
|
||||
expectedRootValues.insert(Symbol('X', 19), updatedTheta.at(Symbol('X', 19)));
|
||||
|
||||
|
||||
|
||||
// Start the synchronization process
|
||||
actualSmootherFactors.resize(0); actualFilterSumarization.resize(0);
|
||||
actualSmootherValues.clear(); actualRootValues.clear();
|
||||
filter.presync();
|
||||
filter.synchronize(smootherSummarization);
|
||||
filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues);
|
||||
filter.getSummarizedFactors(actualFilterSumarization, actualRootValues);
|
||||
filter.postsync();
|
||||
|
||||
|
||||
|
||||
// Compare filter sync variables versus the expected
|
||||
CHECK(hessian_equal(expectedSmootherFactors, actualSmootherFactors, updatedTheta, 1e-8));
|
||||
CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-3));
|
||||
CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, updatedTheta, 1e-8));
|
||||
CHECK(assert_equal(expectedRootValues, actualRootValues, 1e-4));
|
||||
}
|
||||
///* ************************************************************************* */
|
||||
//TEST_UNSAFE( ConcurrentBatchFilter, synchronize )
|
||||
//{
|
||||
// // Test the 'synchronize' function of the ConcurrentBatchFilter in a nonlinear environment.
|
||||
// // The filter is operating on a known tree structure, so the factors and summarization can
|
||||
// // be predicted for testing purposes
|
||||
//
|
||||
// // Create a set of optimizer parameters
|
||||
// LevenbergMarquardtParams parameters;
|
||||
// double lag = 4.0;
|
||||
//
|
||||
// // Create a Concurrent Batch Filter
|
||||
// ConcurrentBatchFilterTester filter(parameters, lag);
|
||||
//
|
||||
// // Create containers to keep the full graph
|
||||
// Values newTheta, fullTheta;
|
||||
// NonlinearFactorGraph newGraph, fullGraph;
|
||||
// ConcurrentBatchFilter::KeyTimestampMap newTimestamps;
|
||||
//
|
||||
// // Create all factors
|
||||
// CreateFactors(newGraph, newTheta, newTimestamps, 0, 13);
|
||||
// fullTheta.insert(newTheta);
|
||||
// fullGraph.push_back(newGraph);
|
||||
//
|
||||
// // Optimize with Concurrent Batch Filter
|
||||
// filter.update(newGraph, newTheta, newTimestamps);
|
||||
// Values updatedTheta = filter.calculateEstimate();
|
||||
//
|
||||
// // Eliminate the factor graph into a Bayes Tree to create the summarized factors
|
||||
// // Create Ordering
|
||||
// std::map<Key, int> constraints;
|
||||
// constraints[Symbol('X', 8)] = 1;
|
||||
// constraints[Symbol('X', 9)] = 1;
|
||||
// constraints[Symbol('X', 10)] = 1;
|
||||
// constraints[Symbol('X', 11)] = 1;
|
||||
// constraints[Symbol('X', 12)] = 1;
|
||||
// Ordering ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints);
|
||||
// // Linearize into a Gaussian Factor Graph
|
||||
// GaussianFactorGraph linearGraph = *fullGraph.linearize(fullTheta, ordering);
|
||||
// // Eliminate into a Bayes Net with iSAM2-type cliques
|
||||
// JunctionTree<GaussianFactorGraph, ISAM2Clique> jt(linearGraph);
|
||||
// ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR);
|
||||
// BayesTree<GaussianConditional, ISAM2Clique> bayesTree;
|
||||
// bayesTree.insert(root);
|
||||
//
|
||||
// // std::cout << "BAYES TREE:" << std::endl;
|
||||
// // SymbolicPrintTree(root, ordering.invert(), " ");
|
||||
//
|
||||
// // BAYES TREE:
|
||||
// // P( X7 X9 X12 )
|
||||
// // P( X10 | X9 )
|
||||
// // P( X11 | X10 )
|
||||
// // P( X8 | X9 )
|
||||
// // P( X6 | X7 X9 )
|
||||
// // P( X3 X5 | X6 )
|
||||
// // P( X2 | X3 )
|
||||
// // P( X1 | X3 )
|
||||
// // P( X0 | X1 )
|
||||
// // P( X4 | X7 )
|
||||
//
|
||||
// // Extract the nonlinear factors that should be sent to the smoother
|
||||
// std::vector<Key> smootherKeys;
|
||||
// smootherKeys.push_back(Symbol('X', 0));
|
||||
// smootherKeys.push_back(Symbol('X', 1));
|
||||
// smootherKeys.push_back(Symbol('X', 2));
|
||||
// smootherKeys.push_back(Symbol('X', 3));
|
||||
// smootherKeys.push_back(Symbol('X', 4));
|
||||
// smootherKeys.push_back(Symbol('X', 5));
|
||||
// smootherKeys.push_back(Symbol('X', 6));
|
||||
// NonlinearFactorGraph expectedSmootherFactors;
|
||||
// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
|
||||
// if(std::find_first_of(factor->begin(), factor->end(), smootherKeys.begin(), smootherKeys.end()) != factor->end()) {
|
||||
// expectedSmootherFactors.push_back(factor);
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// // Extract smoother values
|
||||
// Values expectedSmootherValues;
|
||||
// expectedSmootherValues.insert(Symbol('X', 0), updatedTheta.at(Symbol('X', 0)));
|
||||
// expectedSmootherValues.insert(Symbol('X', 1), updatedTheta.at(Symbol('X', 1)));
|
||||
// expectedSmootherValues.insert(Symbol('X', 2), updatedTheta.at(Symbol('X', 2)));
|
||||
// expectedSmootherValues.insert(Symbol('X', 3), updatedTheta.at(Symbol('X', 3)));
|
||||
// expectedSmootherValues.insert(Symbol('X', 4), updatedTheta.at(Symbol('X', 4)));
|
||||
// expectedSmootherValues.insert(Symbol('X', 5), updatedTheta.at(Symbol('X', 5)));
|
||||
// expectedSmootherValues.insert(Symbol('X', 6), updatedTheta.at(Symbol('X', 6)));
|
||||
//
|
||||
// // Extract the filter summarized factors
|
||||
// // Cached factors that represent the filter side (i.e. the X8 and X10 clique)
|
||||
// NonlinearFactorGraph expectedFilterSumarization;
|
||||
// expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 8)))->cachedFactor()), ordering, fullTheta));
|
||||
// expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 10)))->cachedFactor()), ordering, fullTheta));
|
||||
// // And any factors that involve only the root (X7, X9, X12)
|
||||
// std::vector<Key> rootKeys;
|
||||
// rootKeys.push_back(Symbol('X', 7));
|
||||
// rootKeys.push_back(Symbol('X', 9));
|
||||
// rootKeys.push_back(Symbol('X', 12));
|
||||
// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
|
||||
// size_t count = 0;
|
||||
// BOOST_FOREACH(Key key, *factor) {
|
||||
// if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count;
|
||||
// }
|
||||
// if(count == factor->size()) expectedFilterSumarization.push_back(factor);
|
||||
// }
|
||||
//
|
||||
// // Extract the new root values
|
||||
// Values expectedRootValues;
|
||||
// expectedRootValues.insert(Symbol('X', 7), updatedTheta.at(Symbol('X', 7)));
|
||||
// expectedRootValues.insert(Symbol('X', 9), updatedTheta.at(Symbol('X', 9)));
|
||||
// expectedRootValues.insert(Symbol('X', 12), updatedTheta.at(Symbol('X', 12)));
|
||||
//
|
||||
//
|
||||
//
|
||||
// // Start the synchronization process
|
||||
// NonlinearFactorGraph actualSmootherFactors, actualFilterSumarization, smootherSummarization;
|
||||
// Values actualSmootherValues, actualRootValues;
|
||||
// filter.presync();
|
||||
// filter.synchronize(smootherSummarization); // Supplying an empty factor graph here
|
||||
// filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues);
|
||||
// filter.getSummarizedFactors(actualFilterSumarization, actualRootValues);
|
||||
// filter.postsync();
|
||||
//
|
||||
//
|
||||
//
|
||||
// // Compare filter sync variables versus the expected
|
||||
// CHECK(hessian_equal(expectedSmootherFactors, actualSmootherFactors, updatedTheta, 1e-8));
|
||||
// CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-4));
|
||||
// CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, updatedTheta, 1e-9));
|
||||
// CHECK(assert_equal(expectedRootValues, actualRootValues, 1e-4));
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
// // Now add additional factors to the filter and re-sync
|
||||
// newGraph.resize(0); newTheta.clear(); newTimestamps.clear();
|
||||
// CreateFactors(newGraph, newTheta, newTimestamps, 13, 20);
|
||||
// fullTheta.insert(newTheta);
|
||||
// fullGraph.push_back(newGraph);
|
||||
//
|
||||
// // Optimize with Concurrent Batch Filter
|
||||
// filter.update(newGraph, newTheta, newTimestamps);
|
||||
// updatedTheta = filter.calculateEstimate();
|
||||
//
|
||||
// // Eliminate the factor graph into a Bayes Tree to create the summarized factors
|
||||
// // Create Ordering
|
||||
// constraints.clear();
|
||||
// constraints[Symbol('X', 15)] = 1;
|
||||
// constraints[Symbol('X', 16)] = 1;
|
||||
// constraints[Symbol('X', 17)] = 1;
|
||||
// constraints[Symbol('X', 18)] = 1;
|
||||
// constraints[Symbol('X', 19)] = 1;
|
||||
// ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints);
|
||||
// // Linearize into a Gaussian Factor Graph
|
||||
// linearGraph = *fullGraph.linearize(fullTheta, ordering);
|
||||
// // Eliminate into a Bayes Net with iSAM2-type cliques
|
||||
// jt = JunctionTree<GaussianFactorGraph, ISAM2Clique>(linearGraph);
|
||||
// root = jt.eliminate(EliminateQR);
|
||||
// bayesTree = BayesTree<GaussianConditional, ISAM2Clique>();
|
||||
// bayesTree.insert(root);
|
||||
//
|
||||
// // std::cout << "BAYES TREE:" << std::endl;
|
||||
// // SymbolicPrintTree(root, ordering.invert(), " ");
|
||||
//
|
||||
// // BAYES TREE:
|
||||
// // P( X14 X16 X19 )
|
||||
// // P( X17 | X16 )
|
||||
// // P( X18 | X17 )
|
||||
// // P( X15 | X16 )
|
||||
// // P( X13 | X14 X16 )
|
||||
// // P( X11 | X13 X14 )
|
||||
// // P( X10 | X11 X13 )
|
||||
// // P( X12 | X10 X13 )
|
||||
// // P( X9 | X12 X10 )
|
||||
// // P( X7 | X9 X12 )
|
||||
// // P( X6 | X7 X9 )
|
||||
// // P( X3 X5 | X6 )
|
||||
// // P( X2 | X3 )
|
||||
// // P( X1 | X3 )
|
||||
// // P( X0 | X1 )
|
||||
// // P( X4 | X7 )
|
||||
// // P( X8 | X9 )
|
||||
//
|
||||
// // Extract the cached factors for X4 and X6. These are the new smoother summarized factors
|
||||
// // TODO: I'm concerned about the linearization point used to create these factors. It may need to be the updated lin points?
|
||||
// smootherSummarization.resize(0);
|
||||
// smootherSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 4)))->cachedFactor()), ordering, fullTheta));
|
||||
// smootherSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 6)))->cachedFactor()), ordering, fullTheta));
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
// // Extract the nonlinear factors that should be sent to the smoother
|
||||
// smootherKeys.clear();
|
||||
// smootherKeys.push_back(Symbol('X', 8));
|
||||
// smootherKeys.push_back(Symbol('X', 10));
|
||||
// smootherKeys.push_back(Symbol('X', 11));
|
||||
// smootherKeys.push_back(Symbol('X', 13));
|
||||
// expectedSmootherFactors.resize(0);
|
||||
// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
|
||||
// if(std::find_first_of(factor->begin(), factor->end(), smootherKeys.begin(), smootherKeys.end()) != factor->end()) {
|
||||
// expectedSmootherFactors.push_back(factor);
|
||||
// }
|
||||
// }
|
||||
// // And any factors that involve only the old root (X7, X9, X12)
|
||||
// rootKeys.clear();
|
||||
// rootKeys.push_back(Symbol('X', 7));
|
||||
// rootKeys.push_back(Symbol('X', 9));
|
||||
// rootKeys.push_back(Symbol('X', 12));
|
||||
// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
|
||||
// size_t count = 0;
|
||||
// BOOST_FOREACH(Key key, *factor) {
|
||||
// if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count;
|
||||
// }
|
||||
// if(count == factor->size()) expectedSmootherFactors.push_back(factor);
|
||||
// }
|
||||
//
|
||||
//
|
||||
// // Extract smoother Values
|
||||
// expectedSmootherValues.clear();
|
||||
// expectedSmootherValues.insert(Symbol('X', 7), updatedTheta.at(Symbol('X', 7)));
|
||||
// expectedSmootherValues.insert(Symbol('X', 8), updatedTheta.at(Symbol('X', 8)));
|
||||
// expectedSmootherValues.insert(Symbol('X', 9), updatedTheta.at(Symbol('X', 9)));
|
||||
// expectedSmootherValues.insert(Symbol('X', 10), updatedTheta.at(Symbol('X', 10)));
|
||||
// expectedSmootherValues.insert(Symbol('X', 11), updatedTheta.at(Symbol('X', 11)));
|
||||
// expectedSmootherValues.insert(Symbol('X', 12), updatedTheta.at(Symbol('X', 12)));
|
||||
// expectedSmootherValues.insert(Symbol('X', 13), updatedTheta.at(Symbol('X', 13)));
|
||||
//
|
||||
// // Extract the filter summarized factors
|
||||
// // Cached factors that represent the filter side (i.e. the X15 and X17 clique)
|
||||
// expectedFilterSumarization.resize(0);
|
||||
// expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 15)))->cachedFactor()), ordering, fullTheta));
|
||||
// expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 17)))->cachedFactor()), ordering, fullTheta));
|
||||
// // And any factors that involve only the root (X14, X16, X17)
|
||||
// rootKeys.clear();
|
||||
// rootKeys.push_back(Symbol('X', 14));
|
||||
// rootKeys.push_back(Symbol('X', 16));
|
||||
// rootKeys.push_back(Symbol('X', 19));
|
||||
// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
|
||||
// size_t count = 0;
|
||||
// BOOST_FOREACH(Key key, *factor) {
|
||||
// if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count;
|
||||
// }
|
||||
// if(count == factor->size()) expectedFilterSumarization.push_back(factor);
|
||||
// }
|
||||
//
|
||||
// // Extract the new root keys
|
||||
// expectedRootValues.clear();
|
||||
// expectedRootValues.insert(Symbol('X', 14), updatedTheta.at(Symbol('X', 14)));
|
||||
// expectedRootValues.insert(Symbol('X', 16), updatedTheta.at(Symbol('X', 16)));
|
||||
// expectedRootValues.insert(Symbol('X', 19), updatedTheta.at(Symbol('X', 19)));
|
||||
//
|
||||
//
|
||||
//
|
||||
// // Start the synchronization process
|
||||
// actualSmootherFactors.resize(0); actualFilterSumarization.resize(0);
|
||||
// actualSmootherValues.clear(); actualRootValues.clear();
|
||||
// filter.presync();
|
||||
// filter.synchronize(smootherSummarization);
|
||||
// filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues);
|
||||
// filter.getSummarizedFactors(actualFilterSumarization, actualRootValues);
|
||||
// filter.postsync();
|
||||
//
|
||||
//
|
||||
//
|
||||
// // Compare filter sync variables versus the expected
|
||||
// CHECK(hessian_equal(expectedSmootherFactors, actualSmootherFactors, updatedTheta, 1e-8));
|
||||
// CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-3));
|
||||
// CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, updatedTheta, 1e-8));
|
||||
// CHECK(assert_equal(expectedRootValues, actualRootValues, 1e-4));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
|
|
|
|||
|
|
@ -17,25 +17,20 @@
|
|||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
|
||||
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
namespace {
|
||||
|
||||
// Set up initial pose, odometry difference, loop closure difference, and initialization errors
|
||||
const Pose3 poseInitial;
|
||||
const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
|
||||
|
|
@ -336,7 +331,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, size_t index1 = 0
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, const Values& rootValues = Values()) {
|
||||
Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, const Values& separatorValues = Values()) {
|
||||
|
||||
// Create an L-M optimizer
|
||||
LevenbergMarquardtParams parameters;
|
||||
|
|
@ -348,9 +343,9 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, con
|
|||
double currentError;
|
||||
do {
|
||||
// Force variables associated with root keys to keep the same linearization point
|
||||
if(rootValues.size() > 0) {
|
||||
if(separatorValues.size() > 0) {
|
||||
// Put the old values of the root keys back into the optimizer state
|
||||
optimizer.state().values.update(rootValues);
|
||||
optimizer.state().values.update(separatorValues);
|
||||
// Update the error value with the new theta
|
||||
optimizer.state().error = graph.error(optimizer.state().values);
|
||||
}
|
||||
|
|
@ -367,60 +362,60 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, con
|
|||
return optimizer.values();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void FindFactorsWithAny(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) {
|
||||
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) {
|
||||
NonlinearFactor::const_iterator key = factor->begin();
|
||||
while((key != factor->end()) && (!std::binary_search(keys.begin(), keys.end(), *key))) {
|
||||
++key;
|
||||
}
|
||||
if(key != factor->end()) {
|
||||
destinationFactors.push_back(factor);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
///* ************************************************************************* */
|
||||
//void FindFactorsWithAny(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) {
|
||||
//
|
||||
// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) {
|
||||
// NonlinearFactor::const_iterator key = factor->begin();
|
||||
// while((key != factor->end()) && (!std::binary_search(keys.begin(), keys.end(), *key))) {
|
||||
// ++key;
|
||||
// }
|
||||
// if(key != factor->end()) {
|
||||
// destinationFactors.push_back(factor);
|
||||
// }
|
||||
// }
|
||||
//
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//void FindFactorsWithOnly(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) {
|
||||
//
|
||||
// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) {
|
||||
// NonlinearFactor::const_iterator key = factor->begin();
|
||||
// while((key != factor->end()) && (std::binary_search(keys.begin(), keys.end(), *key))) {
|
||||
// ++key;
|
||||
// }
|
||||
// if(key == factor->end()) {
|
||||
// destinationFactors.push_back(factor);
|
||||
// }
|
||||
// }
|
||||
//
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//typedef BayesTree<GaussianConditional,ISAM2Clique>::sharedClique Clique;
|
||||
//void SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent = "") {
|
||||
// std::cout << indent << "P( ";
|
||||
// BOOST_FOREACH(Index index, clique->conditional()->frontals()){
|
||||
// std::cout << DefaultKeyFormatter(ordering.key(index)) << " ";
|
||||
// }
|
||||
// if(clique->conditional()->nrParents() > 0) {
|
||||
// std::cout << "| ";
|
||||
// }
|
||||
// BOOST_FOREACH(Index index, clique->conditional()->parents()){
|
||||
// std::cout << DefaultKeyFormatter(ordering.key(index)) << " ";
|
||||
// }
|
||||
// std::cout << ")" << std::endl;
|
||||
//
|
||||
// BOOST_FOREACH(const Clique& child, clique->children()) {
|
||||
// SymbolicPrintTree(child, ordering, indent+" ");
|
||||
// }
|
||||
//}
|
||||
//
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void FindFactorsWithOnly(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) {
|
||||
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) {
|
||||
NonlinearFactor::const_iterator key = factor->begin();
|
||||
while((key != factor->end()) && (std::binary_search(keys.begin(), keys.end(), *key))) {
|
||||
++key;
|
||||
}
|
||||
if(key == factor->end()) {
|
||||
destinationFactors.push_back(factor);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
typedef BayesTree<GaussianConditional,ISAM2Clique>::sharedClique Clique;
|
||||
void SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent = "") {
|
||||
std::cout << indent << "P( ";
|
||||
BOOST_FOREACH(Index index, clique->conditional()->frontals()){
|
||||
std::cout << DefaultKeyFormatter(ordering.key(index)) << " ";
|
||||
}
|
||||
if(clique->conditional()->nrParents() > 0) {
|
||||
std::cout << "| ";
|
||||
}
|
||||
BOOST_FOREACH(Index index, clique->conditional()->parents()){
|
||||
std::cout << DefaultKeyFormatter(ordering.key(index)) << " ";
|
||||
}
|
||||
std::cout << ")" << std::endl;
|
||||
|
||||
BOOST_FOREACH(const Clique& child, clique->children()) {
|
||||
SymbolicPrintTree(child, ordering, indent+" ");
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE( ConcurrentBatchSmoother, update_Batch )
|
||||
TEST_UNSAFE( ConcurrentBatchSmoother, update_batch )
|
||||
{
|
||||
// Test the 'update' function of the ConcurrentBatchSmoother in a nonlinear environment.
|
||||
// Thus, a full L-M optimization and the ConcurrentBatchSmoother results should be identical
|
||||
|
|
@ -451,7 +446,7 @@ TEST_UNSAFE( ConcurrentBatchSmoother, update_Batch )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE( ConcurrentBatchSmoother, update_Incremental )
|
||||
TEST_UNSAFE( ConcurrentBatchSmoother, update_incremental )
|
||||
{
|
||||
// Test the 'update' function of the ConcurrentBatchSmoother in a nonlinear environment.
|
||||
// Thus, a full L-M optimization and the ConcurrentBatchSmoother results should be identical
|
||||
|
|
@ -492,220 +487,220 @@ TEST_UNSAFE( ConcurrentBatchSmoother, update_Incremental )
|
|||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE( ConcurrentBatchSmoother, synchronize )
|
||||
{
|
||||
// Test the 'synchronize' function of the ConcurrentBatchSmoother in a nonlinear environment.
|
||||
// The smoother is operating on a known tree structure, so the factors and summarization can
|
||||
// be predicted for testing purposes
|
||||
|
||||
// Create a set of optimizer parameters
|
||||
LevenbergMarquardtParams parameters;
|
||||
|
||||
// Create a Concurrent Batch Smoother
|
||||
ConcurrentBatchSmootherTester smoother(parameters);
|
||||
|
||||
// Create containers to keep the full graph
|
||||
Values fullTheta;
|
||||
NonlinearFactorGraph fullGraph;
|
||||
|
||||
// Create factors for times 0 - 12
|
||||
// When eliminated with ordering (X2 X0 X1 X4 X5 X3 X6 X8 X11 X10 X7 X9 X12)augmentedHessian
|
||||
// ... this Bayes Tree is produced:
|
||||
// Bayes Tree:
|
||||
// P( X7 X9 X12 )
|
||||
// P( X10 | X9 )
|
||||
// P( X11 | X10 )
|
||||
// P( X8 | X9 )
|
||||
// P( X6 | X7 X9 )
|
||||
// P( X5 X3 | X6 )
|
||||
// P( X1 | X3 )
|
||||
// P( X0 | X1 )
|
||||
// P( X2 | X3 )
|
||||
// P( X4 | X7 )
|
||||
// We then produce the inputs necessary for the 'synchronize' function.
|
||||
// The smoother is branches X4 and X6, the filter is branches X8 and X10, and the root is (X7 X9 X12)
|
||||
CreateFactors(fullGraph, fullTheta, 0, 13);
|
||||
|
||||
// Optimize the full graph
|
||||
Values optimalTheta = BatchOptimize(fullGraph, fullTheta);
|
||||
|
||||
// Re-eliminate to create the Bayes Tree
|
||||
Ordering ordering;
|
||||
ordering.push_back(Symbol('X', 2));
|
||||
ordering.push_back(Symbol('X', 0));
|
||||
ordering.push_back(Symbol('X', 1));
|
||||
ordering.push_back(Symbol('X', 4));
|
||||
ordering.push_back(Symbol('X', 5));
|
||||
ordering.push_back(Symbol('X', 3));
|
||||
ordering.push_back(Symbol('X', 6));
|
||||
ordering.push_back(Symbol('X', 8));
|
||||
ordering.push_back(Symbol('X', 11));
|
||||
ordering.push_back(Symbol('X', 10));
|
||||
ordering.push_back(Symbol('X', 7));
|
||||
ordering.push_back(Symbol('X', 9));
|
||||
ordering.push_back(Symbol('X', 12));
|
||||
Values linpoint;
|
||||
linpoint.insert(optimalTheta);
|
||||
GaussianFactorGraph linearGraph = *fullGraph.linearize(linpoint, ordering);
|
||||
JunctionTree<GaussianFactorGraph, ISAM2Clique> jt(linearGraph);
|
||||
ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR);
|
||||
BayesTree<GaussianConditional, ISAM2Clique> bayesTree;
|
||||
bayesTree.insert(root);
|
||||
|
||||
// Extract the values for the smoother keys. This consists of the branches: X4 and X6
|
||||
// Extract the non-root values from the initial values to test the smoother optimization
|
||||
Values smootherValues;
|
||||
smootherValues.insert(Symbol('X', 0), fullTheta.at(Symbol('X', 0)));
|
||||
smootherValues.insert(Symbol('X', 1), fullTheta.at(Symbol('X', 1)));
|
||||
smootherValues.insert(Symbol('X', 2), fullTheta.at(Symbol('X', 2)));
|
||||
smootherValues.insert(Symbol('X', 3), fullTheta.at(Symbol('X', 3)));
|
||||
smootherValues.insert(Symbol('X', 4), fullTheta.at(Symbol('X', 4)));
|
||||
smootherValues.insert(Symbol('X', 5), fullTheta.at(Symbol('X', 5)));
|
||||
smootherValues.insert(Symbol('X', 6), fullTheta.at(Symbol('X', 6)));
|
||||
|
||||
// Extract the optimal root values
|
||||
Values rootValues;
|
||||
rootValues.insert(Symbol('X', 7), optimalTheta.at(Symbol('X', 7)));
|
||||
rootValues.insert(Symbol('X', 9), optimalTheta.at(Symbol('X', 9)));
|
||||
rootValues.insert(Symbol('X', 12), optimalTheta.at(Symbol('X', 12)));
|
||||
|
||||
// Extract the nonlinear smoother factors as any factor with a non-root smoother key
|
||||
std::set<Key> smootherKeys;
|
||||
smootherKeys.insert(Symbol('X', 0));
|
||||
smootherKeys.insert(Symbol('X', 1));
|
||||
smootherKeys.insert(Symbol('X', 2));
|
||||
smootherKeys.insert(Symbol('X', 3));
|
||||
smootherKeys.insert(Symbol('X', 4));
|
||||
smootherKeys.insert(Symbol('X', 5));
|
||||
smootherKeys.insert(Symbol('X', 6));
|
||||
NonlinearFactorGraph smootherFactors;
|
||||
FindFactorsWithAny(smootherKeys, fullGraph, smootherFactors);
|
||||
|
||||
// Extract the filter summarized factors. This consists of the linear cached factors from
|
||||
// the filter branches X8 and X10, as well as any nonlinear factor that involves only root keys
|
||||
NonlinearFactorGraph filterSummarization;
|
||||
filterSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 8)))->cachedFactor()), ordering, linpoint));
|
||||
filterSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 10)))->cachedFactor()), ordering, linpoint));
|
||||
std::set<Key> rootKeys;
|
||||
rootKeys.insert(Symbol('X', 7));
|
||||
rootKeys.insert(Symbol('X', 9));
|
||||
rootKeys.insert(Symbol('X', 12));
|
||||
FindFactorsWithOnly(rootKeys, fullGraph, filterSummarization);
|
||||
|
||||
|
||||
|
||||
// Perform the synchronization procedure
|
||||
NonlinearFactorGraph actualSmootherSummarization;
|
||||
smoother.presync();
|
||||
smoother.getSummarizedFactors(actualSmootherSummarization);
|
||||
smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues);
|
||||
smoother.postsync();
|
||||
|
||||
// Verify the returned smoother values is empty in the first iteration
|
||||
NonlinearFactorGraph expectedSmootherSummarization;
|
||||
CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-4));
|
||||
|
||||
|
||||
|
||||
// Perform a full update of the smoother. Since the root values/summarized filter factors were
|
||||
// created at the optimal values, the smoother should be identical to the batch optimization
|
||||
smoother.update();
|
||||
Values actualSmootherTheta = smoother.calculateEstimate();
|
||||
|
||||
// Create the expected values as the optimal set
|
||||
Values expectedSmootherTheta;
|
||||
expectedSmootherTheta.insert(Symbol('X', 0), optimalTheta.at(Symbol('X', 0)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 1), optimalTheta.at(Symbol('X', 1)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 2), optimalTheta.at(Symbol('X', 2)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 3), optimalTheta.at(Symbol('X', 3)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 4), optimalTheta.at(Symbol('X', 4)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 5), optimalTheta.at(Symbol('X', 5)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 6), optimalTheta.at(Symbol('X', 6)));
|
||||
|
||||
// Compare filter solution with full batch
|
||||
CHECK(assert_equal(expectedSmootherTheta, actualSmootherTheta, 1e-4));
|
||||
|
||||
|
||||
|
||||
// Add a loop closure factor to the smoother and re-check. Since the filter
|
||||
// factors were created at the optimal linpoint, and since the new loop closure
|
||||
// does not involve filter keys, the smoother should still yeild the optimal solution
|
||||
// The new Bayes Tree is:
|
||||
// Bayes Tree:
|
||||
// P( X7 X9 X12 )
|
||||
// P( X10 | X9 )
|
||||
// P( X11 | X10 )
|
||||
// P( X8 | X9 )
|
||||
// P( X6 | X7 X9 )
|
||||
// P( X4 | X6 X7 )
|
||||
// P( X3 X5 | X4 X6 )
|
||||
// P( X2 | X3 )
|
||||
// P( X1 | X3 X4 )
|
||||
// P( X0 | X1 )
|
||||
Pose3 poseDelta = fullTheta.at<Pose3>(Symbol('X', 1)).between(fullTheta.at<Pose3>(Symbol('X', 4)));
|
||||
NonlinearFactor::shared_ptr loopClosure = NonlinearFactor::shared_ptr(new BetweenFactor<Pose3>(Symbol('X', 1), Symbol('X', 4), poseDelta, noiseOdometery));
|
||||
fullGraph.push_back(loopClosure);
|
||||
optimalTheta = BatchOptimize(fullGraph, fullTheta, rootValues);
|
||||
|
||||
// Recreate the Bayes Tree
|
||||
linpoint.clear();
|
||||
linpoint.insert(optimalTheta);
|
||||
linpoint.update(rootValues);
|
||||
linearGraph = *fullGraph.linearize(linpoint, ordering);
|
||||
jt = JunctionTree<GaussianFactorGraph, ISAM2Clique>(linearGraph);
|
||||
root = jt.eliminate(EliminateQR);
|
||||
bayesTree = BayesTree<GaussianConditional, ISAM2Clique>();
|
||||
bayesTree.insert(root);
|
||||
|
||||
// Add the loop closure to the smoother
|
||||
NonlinearFactorGraph newFactors;
|
||||
newFactors.push_back(loopClosure);
|
||||
smoother.update(newFactors);
|
||||
actualSmootherTheta = smoother.calculateEstimate();
|
||||
|
||||
// Create the expected values as the optimal set
|
||||
expectedSmootherTheta.clear();
|
||||
expectedSmootherTheta.insert(Symbol('X', 0), optimalTheta.at(Symbol('X', 0)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 1), optimalTheta.at(Symbol('X', 1)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 2), optimalTheta.at(Symbol('X', 2)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 3), optimalTheta.at(Symbol('X', 3)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 4), optimalTheta.at(Symbol('X', 4)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 5), optimalTheta.at(Symbol('X', 5)));
|
||||
expectedSmootherTheta.insert(Symbol('X', 6), optimalTheta.at(Symbol('X', 6)));
|
||||
|
||||
// Compare filter solution with full batch
|
||||
// TODO: Check This
|
||||
///* ************************************************************************* */
|
||||
//TEST_UNSAFE( ConcurrentBatchSmoother, synchronize )
|
||||
//{
|
||||
// // Test the 'synchronize' function of the ConcurrentBatchSmoother in a nonlinear environment.
|
||||
// // The smoother is operating on a known tree structure, so the factors and summarization can
|
||||
// // be predicted for testing purposes
|
||||
//
|
||||
// // Create a set of optimizer parameters
|
||||
// LevenbergMarquardtParams parameters;
|
||||
//
|
||||
// // Create a Concurrent Batch Smoother
|
||||
// ConcurrentBatchSmootherTester smoother(parameters);
|
||||
//
|
||||
// // Create containers to keep the full graph
|
||||
// Values fullTheta;
|
||||
// NonlinearFactorGraph fullGraph;
|
||||
//
|
||||
// // Create factors for times 0 - 12
|
||||
// // When eliminated with ordering (X2 X0 X1 X4 X5 X3 X6 X8 X11 X10 X7 X9 X12)augmentedHessian
|
||||
// // ... this Bayes Tree is produced:
|
||||
// // Bayes Tree:
|
||||
// // P( X7 X9 X12 )
|
||||
// // P( X10 | X9 )
|
||||
// // P( X11 | X10 )
|
||||
// // P( X8 | X9 )
|
||||
// // P( X6 | X7 X9 )
|
||||
// // P( X5 X3 | X6 )
|
||||
// // P( X1 | X3 )
|
||||
// // P( X0 | X1 )
|
||||
// // P( X2 | X3 )
|
||||
// // P( X4 | X7 )
|
||||
// // We then produce the inputs necessary for the 'synchronize' function.
|
||||
// // The smoother is branches X4 and X6, the filter is branches X8 and X10, and the root is (X7 X9 X12)
|
||||
// CreateFactors(fullGraph, fullTheta, 0, 13);
|
||||
//
|
||||
// // Optimize the full graph
|
||||
// Values optimalTheta = BatchOptimize(fullGraph, fullTheta);
|
||||
//
|
||||
// // Re-eliminate to create the Bayes Tree
|
||||
// Ordering ordering;
|
||||
// ordering.push_back(Symbol('X', 2));
|
||||
// ordering.push_back(Symbol('X', 0));
|
||||
// ordering.push_back(Symbol('X', 1));
|
||||
// ordering.push_back(Symbol('X', 4));
|
||||
// ordering.push_back(Symbol('X', 5));
|
||||
// ordering.push_back(Symbol('X', 3));
|
||||
// ordering.push_back(Symbol('X', 6));
|
||||
// ordering.push_back(Symbol('X', 8));
|
||||
// ordering.push_back(Symbol('X', 11));
|
||||
// ordering.push_back(Symbol('X', 10));
|
||||
// ordering.push_back(Symbol('X', 7));
|
||||
// ordering.push_back(Symbol('X', 9));
|
||||
// ordering.push_back(Symbol('X', 12));
|
||||
// Values linpoint;
|
||||
// linpoint.insert(optimalTheta);
|
||||
// GaussianFactorGraph linearGraph = *fullGraph.linearize(linpoint, ordering);
|
||||
// JunctionTree<GaussianFactorGraph, ISAM2Clique> jt(linearGraph);
|
||||
// ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR);
|
||||
// BayesTree<GaussianConditional, ISAM2Clique> bayesTree;
|
||||
// bayesTree.insert(root);
|
||||
//
|
||||
// // Extract the values for the smoother keys. This consists of the branches: X4 and X6
|
||||
// // Extract the non-root values from the initial values to test the smoother optimization
|
||||
// Values smootherValues;
|
||||
// smootherValues.insert(Symbol('X', 0), fullTheta.at(Symbol('X', 0)));
|
||||
// smootherValues.insert(Symbol('X', 1), fullTheta.at(Symbol('X', 1)));
|
||||
// smootherValues.insert(Symbol('X', 2), fullTheta.at(Symbol('X', 2)));
|
||||
// smootherValues.insert(Symbol('X', 3), fullTheta.at(Symbol('X', 3)));
|
||||
// smootherValues.insert(Symbol('X', 4), fullTheta.at(Symbol('X', 4)));
|
||||
// smootherValues.insert(Symbol('X', 5), fullTheta.at(Symbol('X', 5)));
|
||||
// smootherValues.insert(Symbol('X', 6), fullTheta.at(Symbol('X', 6)));
|
||||
//
|
||||
// // Extract the optimal root values
|
||||
// Values rootValues;
|
||||
// rootValues.insert(Symbol('X', 7), optimalTheta.at(Symbol('X', 7)));
|
||||
// rootValues.insert(Symbol('X', 9), optimalTheta.at(Symbol('X', 9)));
|
||||
// rootValues.insert(Symbol('X', 12), optimalTheta.at(Symbol('X', 12)));
|
||||
//
|
||||
// // Extract the nonlinear smoother factors as any factor with a non-root smoother key
|
||||
// std::set<Key> smootherKeys;
|
||||
// smootherKeys.insert(Symbol('X', 0));
|
||||
// smootherKeys.insert(Symbol('X', 1));
|
||||
// smootherKeys.insert(Symbol('X', 2));
|
||||
// smootherKeys.insert(Symbol('X', 3));
|
||||
// smootherKeys.insert(Symbol('X', 4));
|
||||
// smootherKeys.insert(Symbol('X', 5));
|
||||
// smootherKeys.insert(Symbol('X', 6));
|
||||
// NonlinearFactorGraph smootherFactors;
|
||||
// FindFactorsWithAny(smootherKeys, fullGraph, smootherFactors);
|
||||
//
|
||||
// // Extract the filter summarized factors. This consists of the linear cached factors from
|
||||
// // the filter branches X8 and X10, as well as any nonlinear factor that involves only root keys
|
||||
// NonlinearFactorGraph filterSummarization;
|
||||
// filterSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 8)))->cachedFactor()), ordering, linpoint));
|
||||
// filterSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 10)))->cachedFactor()), ordering, linpoint));
|
||||
// std::set<Key> rootKeys;
|
||||
// rootKeys.insert(Symbol('X', 7));
|
||||
// rootKeys.insert(Symbol('X', 9));
|
||||
// rootKeys.insert(Symbol('X', 12));
|
||||
// FindFactorsWithOnly(rootKeys, fullGraph, filterSummarization);
|
||||
//
|
||||
//
|
||||
//
|
||||
// // Perform the synchronization procedure
|
||||
// NonlinearFactorGraph actualSmootherSummarization;
|
||||
// smoother.presync();
|
||||
// smoother.getSummarizedFactors(actualSmootherSummarization);
|
||||
// smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues);
|
||||
// smoother.postsync();
|
||||
//
|
||||
// // Verify the returned smoother values is empty in the first iteration
|
||||
// NonlinearFactorGraph expectedSmootherSummarization;
|
||||
// CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-4));
|
||||
//
|
||||
//
|
||||
//
|
||||
// // Perform a full update of the smoother. Since the root values/summarized filter factors were
|
||||
// // created at the optimal values, the smoother should be identical to the batch optimization
|
||||
// smoother.update();
|
||||
// Values actualSmootherTheta = smoother.calculateEstimate();
|
||||
//
|
||||
// // Create the expected values as the optimal set
|
||||
// Values expectedSmootherTheta;
|
||||
// expectedSmootherTheta.insert(Symbol('X', 0), optimalTheta.at(Symbol('X', 0)));
|
||||
// expectedSmootherTheta.insert(Symbol('X', 1), optimalTheta.at(Symbol('X', 1)));
|
||||
// expectedSmootherTheta.insert(Symbol('X', 2), optimalTheta.at(Symbol('X', 2)));
|
||||
// expectedSmootherTheta.insert(Symbol('X', 3), optimalTheta.at(Symbol('X', 3)));
|
||||
// expectedSmootherTheta.insert(Symbol('X', 4), optimalTheta.at(Symbol('X', 4)));
|
||||
// expectedSmootherTheta.insert(Symbol('X', 5), optimalTheta.at(Symbol('X', 5)));
|
||||
// expectedSmootherTheta.insert(Symbol('X', 6), optimalTheta.at(Symbol('X', 6)));
|
||||
//
|
||||
// // Compare filter solution with full batch
|
||||
// CHECK(assert_equal(expectedSmootherTheta, actualSmootherTheta, 1e-4));
|
||||
|
||||
|
||||
|
||||
// Now perform a second synchronization to test the smoother-calculated summarization
|
||||
actualSmootherSummarization.resize(0);
|
||||
smootherFactors.resize(0);
|
||||
smootherValues.clear();
|
||||
smoother.presync();
|
||||
smoother.getSummarizedFactors(actualSmootherSummarization);
|
||||
smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues);
|
||||
smoother.postsync();
|
||||
|
||||
// Extract the expected smoother summarization from the Bayes Tree
|
||||
// The smoother branches after the addition of the loop closure is only X6
|
||||
expectedSmootherSummarization.resize(0);
|
||||
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 6)))->cachedFactor());
|
||||
LinearizedJacobianFactor::shared_ptr ljf(new LinearizedJacobianFactor(jf, ordering, linpoint));
|
||||
expectedSmootherSummarization.push_back(ljf);
|
||||
|
||||
// Compare smoother factors with the expected factors by computing the hessian information matrix
|
||||
// TODO: Check This
|
||||
// CHECK(hessian_equal(expectedSmootherSummarization, actualSmootherSummarization, linpoint, 1e-4));
|
||||
|
||||
|
||||
|
||||
// TODO: Modify the second synchronization so that the filter sends an additional set of factors.
|
||||
// I'm not sure what additional code this will exercise, but just for good measure.
|
||||
|
||||
}
|
||||
//
|
||||
//
|
||||
//
|
||||
// // Add a loop closure factor to the smoother and re-check. Since the filter
|
||||
// // factors were created at the optimal linpoint, and since the new loop closure
|
||||
// // does not involve filter keys, the smoother should still yeild the optimal solution
|
||||
// // The new Bayes Tree is:
|
||||
// // Bayes Tree:
|
||||
// // P( X7 X9 X12 )
|
||||
// // P( X10 | X9 )
|
||||
// // P( X11 | X10 )
|
||||
// // P( X8 | X9 )
|
||||
// // P( X6 | X7 X9 )
|
||||
// // P( X4 | X6 X7 )
|
||||
// // P( X3 X5 | X4 X6 )
|
||||
// // P( X2 | X3 )
|
||||
// // P( X1 | X3 X4 )
|
||||
// // P( X0 | X1 )
|
||||
// Pose3 poseDelta = fullTheta.at<Pose3>(Symbol('X', 1)).between(fullTheta.at<Pose3>(Symbol('X', 4)));
|
||||
// NonlinearFactor::shared_ptr loopClosure = NonlinearFactor::shared_ptr(new BetweenFactor<Pose3>(Symbol('X', 1), Symbol('X', 4), poseDelta, noiseOdometery));
|
||||
// fullGraph.push_back(loopClosure);
|
||||
// optimalTheta = BatchOptimize(fullGraph, fullTheta, rootValues);
|
||||
//
|
||||
// // Recreate the Bayes Tree
|
||||
// linpoint.clear();
|
||||
// linpoint.insert(optimalTheta);
|
||||
// linpoint.update(rootValues);
|
||||
// linearGraph = *fullGraph.linearize(linpoint, ordering);
|
||||
// jt = JunctionTree<GaussianFactorGraph, ISAM2Clique>(linearGraph);
|
||||
// root = jt.eliminate(EliminateQR);
|
||||
// bayesTree = BayesTree<GaussianConditional, ISAM2Clique>();
|
||||
// bayesTree.insert(root);
|
||||
//
|
||||
// // Add the loop closure to the smoother
|
||||
// NonlinearFactorGraph newFactors;
|
||||
// newFactors.push_back(loopClosure);
|
||||
// smoother.update(newFactors);
|
||||
// actualSmootherTheta = smoother.calculateEstimate();
|
||||
//
|
||||
// // Create the expected values as the optimal set
|
||||
// expectedSmootherTheta.clear();
|
||||
// expectedSmootherTheta.insert(Symbol('X', 0), optimalTheta.at(Symbol('X', 0)));
|
||||
// expectedSmootherTheta.insert(Symbol('X', 1), optimalTheta.at(Symbol('X', 1)));
|
||||
// expectedSmootherTheta.insert(Symbol('X', 2), optimalTheta.at(Symbol('X', 2)));
|
||||
// expectedSmootherTheta.insert(Symbol('X', 3), optimalTheta.at(Symbol('X', 3)));
|
||||
// expectedSmootherTheta.insert(Symbol('X', 4), optimalTheta.at(Symbol('X', 4)));
|
||||
// expectedSmootherTheta.insert(Symbol('X', 5), optimalTheta.at(Symbol('X', 5)));
|
||||
// expectedSmootherTheta.insert(Symbol('X', 6), optimalTheta.at(Symbol('X', 6)));
|
||||
//
|
||||
// // Compare filter solution with full batch
|
||||
// // TODO: Check This
|
||||
//// CHECK(assert_equal(expectedSmootherTheta, actualSmootherTheta, 1e-4));
|
||||
//
|
||||
//
|
||||
//
|
||||
// // Now perform a second synchronization to test the smoother-calculated summarization
|
||||
// actualSmootherSummarization.resize(0);
|
||||
// smootherFactors.resize(0);
|
||||
// smootherValues.clear();
|
||||
// smoother.presync();
|
||||
// smoother.getSummarizedFactors(actualSmootherSummarization);
|
||||
// smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues);
|
||||
// smoother.postsync();
|
||||
//
|
||||
// // Extract the expected smoother summarization from the Bayes Tree
|
||||
// // The smoother branches after the addition of the loop closure is only X6
|
||||
// expectedSmootherSummarization.resize(0);
|
||||
// JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 6)))->cachedFactor());
|
||||
// LinearizedJacobianFactor::shared_ptr ljf(new LinearizedJacobianFactor(jf, ordering, linpoint));
|
||||
// expectedSmootherSummarization.push_back(ljf);
|
||||
//
|
||||
// // Compare smoother factors with the expected factors by computing the hessian information matrix
|
||||
// // TODO: Check This
|
||||
//// CHECK(hessian_equal(expectedSmootherSummarization, actualSmootherSummarization, linpoint, 1e-4));
|
||||
//
|
||||
//
|
||||
//
|
||||
// // TODO: Modify the second synchronization so that the filter sends an additional set of factors.
|
||||
// // I'm not sure what additional code this will exercise, but just for good measure.
|
||||
//
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
|
|
|
|||
Loading…
Reference in New Issue