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

Updated unit tests for changes in the Concurrent Filtering and Smoothing classes. Currently the synchronization tests are disabled.

release/4.3a0
Stephen Williams 2013-04-09 21:48:48 +00:00
parent 0b5c07e543
commit ed560aa13a
2 changed files with 699 additions and 637 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

793
gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
View File

@ -17,24 +17,20 @@
*/ */
#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h> #include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Key.h> #include <gtsam/nonlinear/Key.h>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
namespace {
// Set up initial pose, odometry difference, loop closure difference, and initialization errors // Set up initial pose, odometry difference, loop closure difference, and initialization errors
const Pose3 poseInitial; 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 // Create a derived class to allow testing protected member functions
class ConcurrentBatchFilterTester : public ConcurrentBatchFilter { class ConcurrentBatchFilterTester : public ConcurrentBatchFilter {
public: public:
ConcurrentBatchFilterTester(const LevenbergMarquardtParams& parameters, double lag) : ConcurrentBatchFilter(parameters, lag) { }; ConcurrentBatchFilterTester(const LevenbergMarquardtParams& parameters) : ConcurrentBatchFilter(parameters) { };
virtual ~ConcurrentBatchFilterTester() { }; virtual ~ConcurrentBatchFilterTester() { };
// Add accessors to the protected members // 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 // Calculate all poses
Pose3 poses[20]; Pose3 poses[20];
@ -128,7 +124,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(PriorFactor<Pose3>(keys[0], poses[0], noisePrior)); graph.add(PriorFactor<Pose3>(keys[0], poses[0], noisePrior));
// Add new variables // Add new variables
theta.insert(keys[0], poses[0].compose(poseError)); theta.insert(keys[0], poses[0].compose(poseError));
timestamps[keys[0]] = double(0);
break; break;
} }
case 1: case 1:
@ -138,7 +133,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(BetweenFactor<Pose3>(keys[0], keys[1], poseDelta, noiseOdometery)); graph.add(BetweenFactor<Pose3>(keys[0], keys[1], poseDelta, noiseOdometery));
// Add new variables // Add new variables
theta.insert(keys[1], poses[1].compose(poseError)); theta.insert(keys[1], poses[1].compose(poseError));
timestamps[keys[1]] = double(1);
break; break;
} }
case 2: case 2:
@ -155,9 +149,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(BetweenFactor<Pose3>(keys[2], keys[3], poseDelta, noiseOdometery)); graph.add(BetweenFactor<Pose3>(keys[2], keys[3], poseDelta, noiseOdometery));
// Add new variables // Add new variables
theta.insert(keys[2], poses[2].compose(poseError)); theta.insert(keys[2], poses[2].compose(poseError));
timestamps[keys[2]] = double(2);
theta.insert(keys[3], poses[3].compose(poseError)); theta.insert(keys[3], poses[3].compose(poseError));
timestamps[keys[3]] = double(3);
break; break;
} }
case 4: case 4:
@ -171,7 +163,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(BetweenFactor<Pose3>(keys[3], keys[5], poseDelta, noiseOdometery)); graph.add(BetweenFactor<Pose3>(keys[3], keys[5], poseDelta, noiseOdometery));
// Add new variables // Add new variables
theta.insert(keys[5], poses[5].compose(poseError)); theta.insert(keys[5], poses[5].compose(poseError));
timestamps[keys[5]] = double(5);
break; break;
} }
case 6: case 6:
@ -184,7 +175,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(BetweenFactor<Pose3>(keys[5], keys[6], poseDelta, noiseOdometery)); graph.add(BetweenFactor<Pose3>(keys[5], keys[6], poseDelta, noiseOdometery));
// Add new variables // Add new variables
theta.insert(keys[6], poses[6].compose(poseError)); theta.insert(keys[6], poses[6].compose(poseError));
timestamps[keys[6]] = double(6);
break; break;
} }
case 7: case 7:
@ -197,9 +187,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(BetweenFactor<Pose3>(keys[6], keys[7], poseDelta, noiseOdometery)); graph.add(BetweenFactor<Pose3>(keys[6], keys[7], poseDelta, noiseOdometery));
// Add new variables // Add new variables
theta.insert(keys[4], poses[4].compose(poseError)); theta.insert(keys[4], poses[4].compose(poseError));
timestamps[keys[4]] = double(4);
theta.insert(keys[7], poses[7].compose(poseError)); theta.insert(keys[7], poses[7].compose(poseError));
timestamps[keys[7]] = double(7);
break; break;
} }
case 8: case 8:
@ -218,9 +206,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(BetweenFactor<Pose3>(keys[8], keys[9], poseDelta, noiseOdometery)); graph.add(BetweenFactor<Pose3>(keys[8], keys[9], poseDelta, noiseOdometery));
// Add new variables // Add new variables
theta.insert(keys[8], poses[8].compose(poseError)); theta.insert(keys[8], poses[8].compose(poseError));
timestamps[keys[8]] = double(8);
theta.insert(keys[9], poses[9].compose(poseError)); theta.insert(keys[9], poses[9].compose(poseError));
timestamps[keys[9]] = double(9);
break; break;
} }
case 10: case 10:
@ -230,7 +216,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(BetweenFactor<Pose3>(keys[9], keys[10], poseDelta, noiseOdometery)); graph.add(BetweenFactor<Pose3>(keys[9], keys[10], poseDelta, noiseOdometery));
// Add new variables // Add new variables
theta.insert(keys[10], poses[10].compose(poseError)); theta.insert(keys[10], poses[10].compose(poseError));
timestamps[keys[10]] = double(10);
break; break;
} }
case 11: case 11:
@ -240,7 +225,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(BetweenFactor<Pose3>(keys[10], keys[11], poseDelta, noiseOdometery)); graph.add(BetweenFactor<Pose3>(keys[10], keys[11], poseDelta, noiseOdometery));
// Add new variables // Add new variables
theta.insert(keys[11], poses[11].compose(poseError)); theta.insert(keys[11], poses[11].compose(poseError));
timestamps[keys[11]] = double(11);
break; break;
} }
case 12: case 12:
@ -253,7 +237,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(BetweenFactor<Pose3>(keys[9], keys[12], poseDelta, noiseOdometery)); graph.add(BetweenFactor<Pose3>(keys[9], keys[12], poseDelta, noiseOdometery));
// Add new variables // Add new variables
theta.insert(keys[12], poses[12].compose(poseError)); theta.insert(keys[12], poses[12].compose(poseError));
timestamps[keys[12]] = double(12);
break; break;
} }
@ -271,7 +254,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(BetweenFactor<Pose3>(keys[12], keys[13], poseDelta, noiseOdometery)); graph.add(BetweenFactor<Pose3>(keys[12], keys[13], poseDelta, noiseOdometery));
// Add new variables // Add new variables
theta.insert(keys[13], poses[13].compose(poseError)); theta.insert(keys[13], poses[13].compose(poseError));
timestamps[keys[13]] = double(13);
break; break;
} }
case 14: case 14:
@ -284,7 +266,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(BetweenFactor<Pose3>(keys[13], keys[14], poseDelta, noiseOdometery)); graph.add(BetweenFactor<Pose3>(keys[13], keys[14], poseDelta, noiseOdometery));
// Add new variables // Add new variables
theta.insert(keys[14], poses[14].compose(poseError)); theta.insert(keys[14], poses[14].compose(poseError));
timestamps[keys[14]] = double(14);
break; break;
} }
case 15: case 15:
@ -303,9 +284,7 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(BetweenFactor<Pose3>(keys[15], keys[16], poseDelta, noiseOdometery)); graph.add(BetweenFactor<Pose3>(keys[15], keys[16], poseDelta, noiseOdometery));
// Add new variables // Add new variables
theta.insert(keys[15], poses[15].compose(poseError)); theta.insert(keys[15], poses[15].compose(poseError));
timestamps[keys[15]] = double(15);
theta.insert(keys[16], poses[16].compose(poseError)); theta.insert(keys[16], poses[16].compose(poseError));
timestamps[keys[16]] = double(16);
break; break;
} }
case 17: case 17:
@ -315,7 +294,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(BetweenFactor<Pose3>(keys[16], keys[17], poseDelta, noiseOdometery)); graph.add(BetweenFactor<Pose3>(keys[16], keys[17], poseDelta, noiseOdometery));
// Add new variables // Add new variables
theta.insert(keys[17], poses[17].compose(poseError)); theta.insert(keys[17], poses[17].compose(poseError));
timestamps[keys[17]] = double(17);
break; break;
} }
case 18: case 18:
@ -325,7 +303,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(BetweenFactor<Pose3>(keys[17], keys[18], poseDelta, noiseOdometery)); graph.add(BetweenFactor<Pose3>(keys[17], keys[18], poseDelta, noiseOdometery));
// Add new variables // Add new variables
theta.insert(keys[18], poses[18].compose(poseError)); theta.insert(keys[18], poses[18].compose(poseError));
timestamps[keys[18]] = double(18);
break; break;
} }
case 19: case 19:
@ -338,7 +315,6 @@ void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFi
graph.add(BetweenFactor<Pose3>(keys[16], keys[19], poseDelta, noiseOdometery)); graph.add(BetweenFactor<Pose3>(keys[16], keys[19], poseDelta, noiseOdometery));
// Add new variables // Add new variables
theta.insert(keys[19], poses[19].compose(poseError)); theta.insert(keys[19], poses[19].compose(poseError));
timestamps[keys[19]] = double(19);
break; 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 // Create an L-M optimizer
LevenbergMarquardtParams parameters; LevenbergMarquardtParams parameters;
@ -361,9 +337,9 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, con
double currentError; double currentError;
do { do {
// Force variables associated with root keys to keep the same linearization point // 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 // 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 // Update the error value with the new theta
optimizer.state().error = graph.error(optimizer.state().values); optimizer.state().error = graph.error(optimizer.state().values);
} }
@ -380,60 +356,60 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, con
return optimizer.values(); return optimizer.values();
} }
/* ************************************************************************* */ ///* ************************************************************************* */
void FindFactorsWithAny(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) { //void FindFactorsWithAny(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) {
//
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) { // BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) {
NonlinearFactor::const_iterator key = factor->begin(); // NonlinearFactor::const_iterator key = factor->begin();
while((key != factor->end()) && (!std::binary_search(keys.begin(), keys.end(), *key))) { // while((key != factor->end()) && (!std::binary_search(keys.begin(), keys.end(), *key))) {
++key; // ++key;
} // }
if(key != factor->end()) { // if(key != factor->end()) {
destinationFactors.push_back(factor); // 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) { TEST_UNSAFE( ConcurrentBatchFilter, update_batch )
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 the 'update' function of the ConcurrentBatchFilter in a nonlinear environment. // Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment.
// Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical // 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 // Create a set of optimizer parameters
LevenbergMarquardtParams parameters; LevenbergMarquardtParams parameters;
double lag = 4.0;
// Create a Concurrent Batch Filter // Create a Concurrent Batch Filter
ConcurrentBatchFilter filter(parameters, lag); ConcurrentBatchFilter filter(parameters);
// Create containers to keep the full graph // Create containers to keep the full graph
Values fullTheta; Values fullTheta;
NonlinearFactorGraph fullGraph; NonlinearFactorGraph fullGraph;
ConcurrentBatchFilter::KeyTimestampMap fullTimestamps;
// Create all factors // Create all factors
CreateFactors(fullGraph, fullTheta, fullTimestamps, 0, 20); CreateFactors(fullGraph, fullTheta, 0, 20);
// Optimize with Concurrent Batch Filter // Optimize with Concurrent Batch Filter
filter.update(fullGraph, fullTheta, fullTimestamps); filter.update(fullGraph, fullTheta, boost::none);
Values actual = filter.calculateEstimate(); Values actual = filter.calculateEstimate();
// Optimize with L-M // Optimize with L-M
Values expected = BatchOptimize(fullGraph, fullTheta); Values expected = BatchOptimize(fullGraph, fullTheta);
@ -465,8 +440,50 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_Batch )
CHECK(assert_equal(expected, actual, 1e-4)); 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. // Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment.
// Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical // 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 // Create a set of optimizer parameters
LevenbergMarquardtParams parameters; LevenbergMarquardtParams parameters;
double lag = 4.0;
// Create a Concurrent Batch Filter // Create a Concurrent Batch Filter
ConcurrentBatchFilter filter(parameters, lag); ConcurrentBatchFilter filter(parameters);
// Create containers to keep the full graph // Create containers to keep the full graph
Values fullTheta; Values fullTheta;
@ -488,13 +504,12 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_Incremental )
// Create containers to keep the new factors // Create containers to keep the new factors
Values newTheta; Values newTheta;
NonlinearFactorGraph newGraph; NonlinearFactorGraph newGraph;
ConcurrentBatchFilter::KeyTimestampMap newTimestamps;
// Create factors // Create factors
CreateFactors(newGraph, newTheta, newTimestamps, i, i+1); CreateFactors(newGraph, newTheta, i, i+1);
// Add these entries to the filter // Add these entries to the filter
filter.update(newGraph, newTheta, newTimestamps); filter.update(newGraph, newTheta, boost::none);
Values actual = filter.calculateEstimate(); Values actual = filter.calculateEstimate();
// Add these entries to the full batch version // Add these entries to the full batch version
@ -509,278 +524,330 @@ TEST_UNSAFE( ConcurrentBatchFilter, update_Incremental )
} }
/* ************************************************************************* */ ///* ************************************************************************* */
TEST_UNSAFE( ConcurrentBatchFilter, synchronize ) //TEST_UNSAFE( ConcurrentBatchFilter, update_incremental_with_marginalization )
{ //{
// Test the 'synchronize' function of the ConcurrentBatchFilter in a nonlinear environment. // // Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment.
// The filter is operating on a known tree structure, so the factors and summarization can // // Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical
// be predicted for testing purposes // // 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; //TEST_UNSAFE( ConcurrentBatchFilter, synchronize )
double lag = 4.0; //{
// // Test the 'synchronize' function of the ConcurrentBatchFilter in a nonlinear environment.
// Create a Concurrent Batch Filter // // The filter is operating on a known tree structure, so the factors and summarization can
ConcurrentBatchFilterTester filter(parameters, lag); // // be predicted for testing purposes
//
// Create containers to keep the full graph // // Create a set of optimizer parameters
Values newTheta, fullTheta; // LevenbergMarquardtParams parameters;
NonlinearFactorGraph newGraph, fullGraph; // double lag = 4.0;
ConcurrentBatchFilter::KeyTimestampMap newTimestamps; //
// // Create a Concurrent Batch Filter
// Create all factors // ConcurrentBatchFilterTester filter(parameters, lag);
CreateFactors(newGraph, newTheta, newTimestamps, 0, 13); //
fullTheta.insert(newTheta); // // Create containers to keep the full graph
fullGraph.push_back(newGraph); // Values newTheta, fullTheta;
// NonlinearFactorGraph newGraph, fullGraph;
// Optimize with Concurrent Batch Filter // ConcurrentBatchFilter::KeyTimestampMap newTimestamps;
filter.update(newGraph, newTheta, newTimestamps); //
Values updatedTheta = filter.calculateEstimate(); // // Create all factors
// CreateFactors(newGraph, newTheta, newTimestamps, 0, 13);
// Eliminate the factor graph into a Bayes Tree to create the summarized factors // fullTheta.insert(newTheta);
// Create Ordering // fullGraph.push_back(newGraph);
std::map<Key, int> constraints; //
constraints[Symbol('X', 8)] = 1; // // Optimize with Concurrent Batch Filter
constraints[Symbol('X', 9)] = 1; // filter.update(newGraph, newTheta, newTimestamps);
constraints[Symbol('X', 10)] = 1; // Values updatedTheta = filter.calculateEstimate();
constraints[Symbol('X', 11)] = 1; //
constraints[Symbol('X', 12)] = 1; // // Eliminate the factor graph into a Bayes Tree to create the summarized factors
Ordering ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints); // // Create Ordering
// Linearize into a Gaussian Factor Graph // std::map<Key, int> constraints;
GaussianFactorGraph linearGraph = *fullGraph.linearize(fullTheta, ordering); // constraints[Symbol('X', 8)] = 1;
// Eliminate into a Bayes Net with iSAM2-type cliques // constraints[Symbol('X', 9)] = 1;
JunctionTree<GaussianFactorGraph, ISAM2Clique> jt(linearGraph); // constraints[Symbol('X', 10)] = 1;
ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR); // constraints[Symbol('X', 11)] = 1;
BayesTree<GaussianConditional, ISAM2Clique> bayesTree; // constraints[Symbol('X', 12)] = 1;
bayesTree.insert(root); // Ordering ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints);
// // Linearize into a Gaussian Factor Graph
// std::cout << "BAYES TREE:" << std::endl; // GaussianFactorGraph linearGraph = *fullGraph.linearize(fullTheta, ordering);
// SymbolicPrintTree(root, ordering.invert(), " "); // // Eliminate into a Bayes Net with iSAM2-type cliques
// JunctionTree<GaussianFactorGraph, ISAM2Clique> jt(linearGraph);
// BAYES TREE: // ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR);
// P( X7 X9 X12 ) // BayesTree<GaussianConditional, ISAM2Clique> bayesTree;
// P( X10 | X9 ) // bayesTree.insert(root);
// P( X11 | X10 ) //
// P( X8 | X9 ) // // std::cout << "BAYES TREE:" << std::endl;
// P( X6 | X7 X9 ) // // SymbolicPrintTree(root, ordering.invert(), " ");
// P( X3 X5 | X6 ) //
// P( X2 | X3 ) // // BAYES TREE:
// P( X1 | X3 ) // // P( X7 X9 X12 )
// P( X0 | X1 ) // // P( X10 | X9 )
// P( X4 | X7 ) // // P( X11 | X10 )
// // P( X8 | X9 )
// Extract the nonlinear factors that should be sent to the smoother // // P( X6 | X7 X9 )
std::vector<Key> smootherKeys; // // P( X3 X5 | X6 )
smootherKeys.push_back(Symbol('X', 0)); // // P( X2 | X3 )
smootherKeys.push_back(Symbol('X', 1)); // // P( X1 | X3 )
smootherKeys.push_back(Symbol('X', 2)); // // P( X0 | X1 )
smootherKeys.push_back(Symbol('X', 3)); // // P( X4 | X7 )
smootherKeys.push_back(Symbol('X', 4)); //
smootherKeys.push_back(Symbol('X', 5)); // // Extract the nonlinear factors that should be sent to the smoother
smootherKeys.push_back(Symbol('X', 6)); // std::vector<Key> smootherKeys;
NonlinearFactorGraph expectedSmootherFactors; // smootherKeys.push_back(Symbol('X', 0));
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) { // smootherKeys.push_back(Symbol('X', 1));
if(std::find_first_of(factor->begin(), factor->end(), smootherKeys.begin(), smootherKeys.end()) != factor->end()) { // smootherKeys.push_back(Symbol('X', 2));
expectedSmootherFactors.push_back(factor); // smootherKeys.push_back(Symbol('X', 3));
} // smootherKeys.push_back(Symbol('X', 4));
} // smootherKeys.push_back(Symbol('X', 5));
// smootherKeys.push_back(Symbol('X', 6));
// Extract smoother values // NonlinearFactorGraph expectedSmootherFactors;
Values expectedSmootherValues; // BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
expectedSmootherValues.insert(Symbol('X', 0), updatedTheta.at(Symbol('X', 0))); // if(std::find_first_of(factor->begin(), factor->end(), smootherKeys.begin(), smootherKeys.end()) != factor->end()) {
expectedSmootherValues.insert(Symbol('X', 1), updatedTheta.at(Symbol('X', 1))); // expectedSmootherFactors.push_back(factor);
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))); // // Extract smoother values
expectedSmootherValues.insert(Symbol('X', 6), updatedTheta.at(Symbol('X', 6))); // Values expectedSmootherValues;
// expectedSmootherValues.insert(Symbol('X', 0), updatedTheta.at(Symbol('X', 0)));
// Extract the filter summarized factors // expectedSmootherValues.insert(Symbol('X', 1), updatedTheta.at(Symbol('X', 1)));
// Cached factors that represent the filter side (i.e. the X8 and X10 clique) // expectedSmootherValues.insert(Symbol('X', 2), updatedTheta.at(Symbol('X', 2)));
NonlinearFactorGraph expectedFilterSumarization; // expectedSmootherValues.insert(Symbol('X', 3), updatedTheta.at(Symbol('X', 3)));
expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 8)))->cachedFactor()), ordering, fullTheta)); // expectedSmootherValues.insert(Symbol('X', 4), updatedTheta.at(Symbol('X', 4)));
expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 10)))->cachedFactor()), ordering, fullTheta)); // expectedSmootherValues.insert(Symbol('X', 5), updatedTheta.at(Symbol('X', 5)));
// And any factors that involve only the root (X7, X9, X12) // expectedSmootherValues.insert(Symbol('X', 6), updatedTheta.at(Symbol('X', 6)));
std::vector<Key> rootKeys; //
rootKeys.push_back(Symbol('X', 7)); // // Extract the filter summarized factors
rootKeys.push_back(Symbol('X', 9)); // // Cached factors that represent the filter side (i.e. the X8 and X10 clique)
rootKeys.push_back(Symbol('X', 12)); // NonlinearFactorGraph expectedFilterSumarization;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) { // expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 8)))->cachedFactor()), ordering, fullTheta));
size_t count = 0; // expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 10)))->cachedFactor()), ordering, fullTheta));
BOOST_FOREACH(Key key, *factor) { // // And any factors that involve only the root (X7, X9, X12)
if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count; // std::vector<Key> rootKeys;
} // rootKeys.push_back(Symbol('X', 7));
if(count == factor->size()) expectedFilterSumarization.push_back(factor); // rootKeys.push_back(Symbol('X', 9));
} // rootKeys.push_back(Symbol('X', 12));
// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
// Extract the new root values // size_t count = 0;
Values expectedRootValues; // BOOST_FOREACH(Key key, *factor) {
expectedRootValues.insert(Symbol('X', 7), updatedTheta.at(Symbol('X', 7))); // if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count;
expectedRootValues.insert(Symbol('X', 9), updatedTheta.at(Symbol('X', 9))); // }
expectedRootValues.insert(Symbol('X', 12), updatedTheta.at(Symbol('X', 12))); // if(count == factor->size()) expectedFilterSumarization.push_back(factor);
// }
//
// // Extract the new root values
// Start the synchronization process // Values expectedRootValues;
NonlinearFactorGraph actualSmootherFactors, actualFilterSumarization, smootherSummarization; // expectedRootValues.insert(Symbol('X', 7), updatedTheta.at(Symbol('X', 7)));
Values actualSmootherValues, actualRootValues; // expectedRootValues.insert(Symbol('X', 9), updatedTheta.at(Symbol('X', 9)));
filter.presync(); // expectedRootValues.insert(Symbol('X', 12), updatedTheta.at(Symbol('X', 12)));
filter.synchronize(smootherSummarization); // Supplying an empty factor graph here //
filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues); //
filter.getSummarizedFactors(actualFilterSumarization, actualRootValues); //
filter.postsync(); // // Start the synchronization process
// NonlinearFactorGraph actualSmootherFactors, actualFilterSumarization, smootherSummarization;
// Values actualSmootherValues, actualRootValues;
// filter.presync();
// Compare filter sync variables versus the expected // filter.synchronize(smootherSummarization); // Supplying an empty factor graph here
CHECK(hessian_equal(expectedSmootherFactors, actualSmootherFactors, updatedTheta, 1e-8)); // filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues);
CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-4)); // filter.getSummarizedFactors(actualFilterSumarization, actualRootValues);
CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, updatedTheta, 1e-9)); // filter.postsync();
CHECK(assert_equal(expectedRootValues, actualRootValues, 1e-4)); //
//
//
// // 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));
// Now add additional factors to the filter and re-sync // CHECK(assert_equal(expectedRootValues, actualRootValues, 1e-4));
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); // // Now add additional factors to the filter and re-sync
updatedTheta = filter.calculateEstimate(); // newGraph.resize(0); newTheta.clear(); newTimestamps.clear();
// CreateFactors(newGraph, newTheta, newTimestamps, 13, 20);
// Eliminate the factor graph into a Bayes Tree to create the summarized factors // fullTheta.insert(newTheta);
// Create Ordering // fullGraph.push_back(newGraph);
constraints.clear(); //
constraints[Symbol('X', 15)] = 1; // // Optimize with Concurrent Batch Filter
constraints[Symbol('X', 16)] = 1; // filter.update(newGraph, newTheta, newTimestamps);
constraints[Symbol('X', 17)] = 1; // updatedTheta = filter.calculateEstimate();
constraints[Symbol('X', 18)] = 1; //
constraints[Symbol('X', 19)] = 1; // // Eliminate the factor graph into a Bayes Tree to create the summarized factors
ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints); // // Create Ordering
// Linearize into a Gaussian Factor Graph // constraints.clear();
linearGraph = *fullGraph.linearize(fullTheta, ordering); // constraints[Symbol('X', 15)] = 1;
// Eliminate into a Bayes Net with iSAM2-type cliques // constraints[Symbol('X', 16)] = 1;
jt = JunctionTree<GaussianFactorGraph, ISAM2Clique>(linearGraph); // constraints[Symbol('X', 17)] = 1;
root = jt.eliminate(EliminateQR); // constraints[Symbol('X', 18)] = 1;
bayesTree = BayesTree<GaussianConditional, ISAM2Clique>(); // constraints[Symbol('X', 19)] = 1;
bayesTree.insert(root); // ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints);
// // Linearize into a Gaussian Factor Graph
// std::cout << "BAYES TREE:" << std::endl; // linearGraph = *fullGraph.linearize(fullTheta, ordering);
// SymbolicPrintTree(root, ordering.invert(), " "); // // Eliminate into a Bayes Net with iSAM2-type cliques
// jt = JunctionTree<GaussianFactorGraph, ISAM2Clique>(linearGraph);
// BAYES TREE: // root = jt.eliminate(EliminateQR);
// P( X14 X16 X19 ) // bayesTree = BayesTree<GaussianConditional, ISAM2Clique>();
// P( X17 | X16 ) // bayesTree.insert(root);
// P( X18 | X17 ) //
// P( X15 | X16 ) // // std::cout << "BAYES TREE:" << std::endl;
// P( X13 | X14 X16 ) // // SymbolicPrintTree(root, ordering.invert(), " ");
// P( X11 | X13 X14 ) //
// P( X10 | X11 X13 ) // // BAYES TREE:
// P( X12 | X10 X13 ) // // P( X14 X16 X19 )
// P( X9 | X12 X10 ) // // P( X17 | X16 )
// P( X7 | X9 X12 ) // // P( X18 | X17 )
// P( X6 | X7 X9 ) // // P( X15 | X16 )
// P( X3 X5 | X6 ) // // P( X13 | X14 X16 )
// P( X2 | X3 ) // // P( X11 | X13 X14 )
// P( X1 | X3 ) // // P( X10 | X11 X13 )
// P( X0 | X1 ) // // P( X12 | X10 X13 )
// P( X4 | X7 ) // // P( X9 | X12 X10 )
// P( X8 | X9 ) // // P( X7 | X9 X12 )
// // P( X6 | X7 X9 )
// Extract the cached factors for X4 and X6. These are the new smoother summarized factors // // P( X3 X5 | X6 )
// TODO: I'm concerned about the linearization point used to create these factors. It may need to be the updated lin points? // // P( X2 | X3 )
smootherSummarization.resize(0); // // P( X1 | X3 )
smootherSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 4)))->cachedFactor()), ordering, fullTheta)); // // P( X0 | X1 )
smootherSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 6)))->cachedFactor()), ordering, fullTheta)); // // 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);
// Extract the nonlinear factors that should be sent to the smoother // smootherSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 4)))->cachedFactor()), ordering, fullTheta));
smootherKeys.clear(); // smootherSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 6)))->cachedFactor()), ordering, fullTheta));
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) { // // Extract the nonlinear factors that should be sent to the smoother
if(std::find_first_of(factor->begin(), factor->end(), smootherKeys.begin(), smootherKeys.end()) != factor->end()) { // smootherKeys.clear();
expectedSmootherFactors.push_back(factor); // smootherKeys.push_back(Symbol('X', 8));
} // smootherKeys.push_back(Symbol('X', 10));
} // smootherKeys.push_back(Symbol('X', 11));
// And any factors that involve only the old root (X7, X9, X12) // smootherKeys.push_back(Symbol('X', 13));
rootKeys.clear(); // expectedSmootherFactors.resize(0);
rootKeys.push_back(Symbol('X', 7)); // BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
rootKeys.push_back(Symbol('X', 9)); // if(std::find_first_of(factor->begin(), factor->end(), smootherKeys.begin(), smootherKeys.end()) != factor->end()) {
rootKeys.push_back(Symbol('X', 12)); // expectedSmootherFactors.push_back(factor);
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) { // }
size_t count = 0; // }
BOOST_FOREACH(Key key, *factor) { // // And any factors that involve only the old root (X7, X9, X12)
if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count; // rootKeys.clear();
} // rootKeys.push_back(Symbol('X', 7));
if(count == factor->size()) expectedSmootherFactors.push_back(factor); // rootKeys.push_back(Symbol('X', 9));
} // rootKeys.push_back(Symbol('X', 12));
// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
// size_t count = 0;
// Extract smoother Values // BOOST_FOREACH(Key key, *factor) {
expectedSmootherValues.clear(); // if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count;
expectedSmootherValues.insert(Symbol('X', 7), updatedTheta.at(Symbol('X', 7))); // }
expectedSmootherValues.insert(Symbol('X', 8), updatedTheta.at(Symbol('X', 8))); // if(count == factor->size()) expectedSmootherFactors.push_back(factor);
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))); // // Extract smoother Values
expectedSmootherValues.insert(Symbol('X', 13), updatedTheta.at(Symbol('X', 13))); // expectedSmootherValues.clear();
// expectedSmootherValues.insert(Symbol('X', 7), updatedTheta.at(Symbol('X', 7)));
// Extract the filter summarized factors // expectedSmootherValues.insert(Symbol('X', 8), updatedTheta.at(Symbol('X', 8)));
// Cached factors that represent the filter side (i.e. the X15 and X17 clique) // expectedSmootherValues.insert(Symbol('X', 9), updatedTheta.at(Symbol('X', 9)));
expectedFilterSumarization.resize(0); // expectedSmootherValues.insert(Symbol('X', 10), updatedTheta.at(Symbol('X', 10)));
expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 15)))->cachedFactor()), ordering, fullTheta)); // expectedSmootherValues.insert(Symbol('X', 11), updatedTheta.at(Symbol('X', 11)));
expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 17)))->cachedFactor()), ordering, fullTheta)); // expectedSmootherValues.insert(Symbol('X', 12), updatedTheta.at(Symbol('X', 12)));
// And any factors that involve only the root (X14, X16, X17) // expectedSmootherValues.insert(Symbol('X', 13), updatedTheta.at(Symbol('X', 13)));
rootKeys.clear(); //
rootKeys.push_back(Symbol('X', 14)); // // Extract the filter summarized factors
rootKeys.push_back(Symbol('X', 16)); // // Cached factors that represent the filter side (i.e. the X15 and X17 clique)
rootKeys.push_back(Symbol('X', 19)); // expectedFilterSumarization.resize(0);
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) { // expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 15)))->cachedFactor()), ordering, fullTheta));
size_t count = 0; // expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 17)))->cachedFactor()), ordering, fullTheta));
BOOST_FOREACH(Key key, *factor) { // // And any factors that involve only the root (X14, X16, X17)
if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count; // rootKeys.clear();
} // rootKeys.push_back(Symbol('X', 14));
if(count == factor->size()) expectedFilterSumarization.push_back(factor); // rootKeys.push_back(Symbol('X', 16));
} // rootKeys.push_back(Symbol('X', 19));
// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) {
// Extract the new root keys // size_t count = 0;
expectedRootValues.clear(); // BOOST_FOREACH(Key key, *factor) {
expectedRootValues.insert(Symbol('X', 14), updatedTheta.at(Symbol('X', 14))); // if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count;
expectedRootValues.insert(Symbol('X', 16), updatedTheta.at(Symbol('X', 16))); // }
expectedRootValues.insert(Symbol('X', 19), updatedTheta.at(Symbol('X', 19))); // if(count == factor->size()) expectedFilterSumarization.push_back(factor);
// }
//
// // Extract the new root keys
// Start the synchronization process // expectedRootValues.clear();
actualSmootherFactors.resize(0); actualFilterSumarization.resize(0); // expectedRootValues.insert(Symbol('X', 14), updatedTheta.at(Symbol('X', 14)));
actualSmootherValues.clear(); actualRootValues.clear(); // expectedRootValues.insert(Symbol('X', 16), updatedTheta.at(Symbol('X', 16)));
filter.presync(); // expectedRootValues.insert(Symbol('X', 19), updatedTheta.at(Symbol('X', 19)));
filter.synchronize(smootherSummarization); //
filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues); //
filter.getSummarizedFactors(actualFilterSumarization, actualRootValues); //
filter.postsync(); // // Start the synchronization process
// actualSmootherFactors.resize(0); actualFilterSumarization.resize(0);
// actualSmootherValues.clear(); actualRootValues.clear();
// filter.presync();
// Compare filter sync variables versus the expected // filter.synchronize(smootherSummarization);
CHECK(hessian_equal(expectedSmootherFactors, actualSmootherFactors, updatedTheta, 1e-8)); // filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues);
CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-3)); // filter.getSummarizedFactors(actualFilterSumarization, actualRootValues);
CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, updatedTheta, 1e-8)); // filter.postsync();
CHECK(assert_equal(expectedRootValues, actualRootValues, 1e-4)); //
} //
//
// // 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);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}

543
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
View File

@ -17,25 +17,20 @@
*/ */
#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h> #include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Key.h> #include <gtsam/nonlinear/Key.h>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
namespace {
// Set up initial pose, odometry difference, loop closure difference, and initialization errors // Set up initial pose, odometry difference, loop closure difference, and initialization errors
const Pose3 poseInitial; const Pose3 poseInitial;
const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); 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 // Create an L-M optimizer
LevenbergMarquardtParams parameters; LevenbergMarquardtParams parameters;
@ -348,9 +343,9 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, con
double currentError; double currentError;
do { do {
// Force variables associated with root keys to keep the same linearization point // 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 // 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 // Update the error value with the new theta
optimizer.state().error = graph.error(optimizer.state().values); optimizer.state().error = graph.error(optimizer.state().values);
} }
@ -367,60 +362,60 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, con
return optimizer.values(); return optimizer.values();
} }
/* ************************************************************************* */ ///* ************************************************************************* */
void FindFactorsWithAny(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) { //void FindFactorsWithAny(const std::set<Key>& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) {
//
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) { // BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) {
NonlinearFactor::const_iterator key = factor->begin(); // NonlinearFactor::const_iterator key = factor->begin();
while((key != factor->end()) && (!std::binary_search(keys.begin(), keys.end(), *key))) { // while((key != factor->end()) && (!std::binary_search(keys.begin(), keys.end(), *key))) {
++key; // ++key;
} // }
if(key != factor->end()) { // if(key != factor->end()) {
destinationFactors.push_back(factor); // 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) { TEST_UNSAFE( ConcurrentBatchSmoother, update_batch )
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 the 'update' function of the ConcurrentBatchSmoother in a nonlinear environment. // Test the 'update' function of the ConcurrentBatchSmoother in a nonlinear environment.
// Thus, a full L-M optimization and the ConcurrentBatchSmoother results should be identical // 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. // Test the 'update' function of the ConcurrentBatchSmoother in a nonlinear environment.
// Thus, a full L-M optimization and the ConcurrentBatchSmoother results should be identical // 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_UNSAFE( ConcurrentBatchSmoother, synchronize )
{ //{
// Test the 'synchronize' function of the ConcurrentBatchSmoother in a nonlinear environment. // // 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 // // The smoother is operating on a known tree structure, so the factors and summarization can
// be predicted for testing purposes // // be predicted for testing purposes
//
// Create a set of optimizer parameters // // Create a set of optimizer parameters
LevenbergMarquardtParams parameters; // LevenbergMarquardtParams parameters;
//
// Create a Concurrent Batch Smoother // // Create a Concurrent Batch Smoother
ConcurrentBatchSmootherTester smoother(parameters); // ConcurrentBatchSmootherTester smoother(parameters);
//
// Create containers to keep the full graph // // Create containers to keep the full graph
Values fullTheta; // Values fullTheta;
NonlinearFactorGraph fullGraph; // NonlinearFactorGraph fullGraph;
//
// Create factors for times 0 - 12 // // Create factors for times 0 - 12
// When eliminated with ordering (X2 X0 X1 X4 X5 X3 X6 X8 X11 X10 X7 X9 X12)augmentedHessian // // When eliminated with ordering (X2 X0 X1 X4 X5 X3 X6 X8 X11 X10 X7 X9 X12)augmentedHessian
// ... this Bayes Tree is produced: // // ... this Bayes Tree is produced:
// Bayes Tree: // // Bayes Tree:
// P( X7 X9 X12 ) // // P( X7 X9 X12 )
// P( X10 | X9 ) // // P( X10 | X9 )
// P( X11 | X10 ) // // P( X11 | X10 )
// P( X8 | X9 ) // // P( X8 | X9 )
// P( X6 | X7 X9 ) // // P( X6 | X7 X9 )
// P( X5 X3 | X6 ) // // P( X5 X3 | X6 )
// P( X1 | X3 ) // // P( X1 | X3 )
// P( X0 | X1 ) // // P( X0 | X1 )
// P( X2 | X3 ) // // P( X2 | X3 )
// P( X4 | X7 ) // // P( X4 | X7 )
// We then produce the inputs necessary for the 'synchronize' function. // // 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) // // 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); // CreateFactors(fullGraph, fullTheta, 0, 13);
//
// Optimize the full graph // // Optimize the full graph
Values optimalTheta = BatchOptimize(fullGraph, fullTheta); // Values optimalTheta = BatchOptimize(fullGraph, fullTheta);
//
// Re-eliminate to create the Bayes Tree // // Re-eliminate to create the Bayes Tree
Ordering ordering; // Ordering ordering;
ordering.push_back(Symbol('X', 2)); // ordering.push_back(Symbol('X', 2));
ordering.push_back(Symbol('X', 0)); // ordering.push_back(Symbol('X', 0));
ordering.push_back(Symbol('X', 1)); // ordering.push_back(Symbol('X', 1));
ordering.push_back(Symbol('X', 4)); // ordering.push_back(Symbol('X', 4));
ordering.push_back(Symbol('X', 5)); // ordering.push_back(Symbol('X', 5));
ordering.push_back(Symbol('X', 3)); // ordering.push_back(Symbol('X', 3));
ordering.push_back(Symbol('X', 6)); // ordering.push_back(Symbol('X', 6));
ordering.push_back(Symbol('X', 8)); // ordering.push_back(Symbol('X', 8));
ordering.push_back(Symbol('X', 11)); // ordering.push_back(Symbol('X', 11));
ordering.push_back(Symbol('X', 10)); // ordering.push_back(Symbol('X', 10));
ordering.push_back(Symbol('X', 7)); // ordering.push_back(Symbol('X', 7));
ordering.push_back(Symbol('X', 9)); // ordering.push_back(Symbol('X', 9));
ordering.push_back(Symbol('X', 12)); // ordering.push_back(Symbol('X', 12));
Values linpoint; // Values linpoint;
linpoint.insert(optimalTheta); // linpoint.insert(optimalTheta);
GaussianFactorGraph linearGraph = *fullGraph.linearize(linpoint, ordering); // GaussianFactorGraph linearGraph = *fullGraph.linearize(linpoint, ordering);
JunctionTree<GaussianFactorGraph, ISAM2Clique> jt(linearGraph); // JunctionTree<GaussianFactorGraph, ISAM2Clique> jt(linearGraph);
ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR); // ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR);
BayesTree<GaussianConditional, ISAM2Clique> bayesTree; // BayesTree<GaussianConditional, ISAM2Clique> bayesTree;
bayesTree.insert(root); // bayesTree.insert(root);
//
// Extract the values for the smoother keys. This consists of the branches: X4 and X6 // // 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 // // Extract the non-root values from the initial values to test the smoother optimization
Values smootherValues; // Values smootherValues;
smootherValues.insert(Symbol('X', 0), fullTheta.at(Symbol('X', 0))); // smootherValues.insert(Symbol('X', 0), fullTheta.at(Symbol('X', 0)));
smootherValues.insert(Symbol('X', 1), fullTheta.at(Symbol('X', 1))); // smootherValues.insert(Symbol('X', 1), fullTheta.at(Symbol('X', 1)));
smootherValues.insert(Symbol('X', 2), fullTheta.at(Symbol('X', 2))); // smootherValues.insert(Symbol('X', 2), fullTheta.at(Symbol('X', 2)));
smootherValues.insert(Symbol('X', 3), fullTheta.at(Symbol('X', 3))); // smootherValues.insert(Symbol('X', 3), fullTheta.at(Symbol('X', 3)));
smootherValues.insert(Symbol('X', 4), fullTheta.at(Symbol('X', 4))); // smootherValues.insert(Symbol('X', 4), fullTheta.at(Symbol('X', 4)));
smootherValues.insert(Symbol('X', 5), fullTheta.at(Symbol('X', 5))); // smootherValues.insert(Symbol('X', 5), fullTheta.at(Symbol('X', 5)));
smootherValues.insert(Symbol('X', 6), fullTheta.at(Symbol('X', 6))); // smootherValues.insert(Symbol('X', 6), fullTheta.at(Symbol('X', 6)));
//
// Extract the optimal root values // // Extract the optimal root values
Values rootValues; // Values rootValues;
rootValues.insert(Symbol('X', 7), optimalTheta.at(Symbol('X', 7))); // rootValues.insert(Symbol('X', 7), optimalTheta.at(Symbol('X', 7)));
rootValues.insert(Symbol('X', 9), optimalTheta.at(Symbol('X', 9))); // rootValues.insert(Symbol('X', 9), optimalTheta.at(Symbol('X', 9)));
rootValues.insert(Symbol('X', 12), optimalTheta.at(Symbol('X', 12))); // rootValues.insert(Symbol('X', 12), optimalTheta.at(Symbol('X', 12)));
//
// Extract the nonlinear smoother factors as any factor with a non-root smoother key // // Extract the nonlinear smoother factors as any factor with a non-root smoother key
std::set<Key> smootherKeys; // std::set<Key> smootherKeys;
smootherKeys.insert(Symbol('X', 0)); // smootherKeys.insert(Symbol('X', 0));
smootherKeys.insert(Symbol('X', 1)); // smootherKeys.insert(Symbol('X', 1));
smootherKeys.insert(Symbol('X', 2)); // smootherKeys.insert(Symbol('X', 2));
smootherKeys.insert(Symbol('X', 3)); // smootherKeys.insert(Symbol('X', 3));
smootherKeys.insert(Symbol('X', 4)); // smootherKeys.insert(Symbol('X', 4));
smootherKeys.insert(Symbol('X', 5)); // smootherKeys.insert(Symbol('X', 5));
smootherKeys.insert(Symbol('X', 6)); // smootherKeys.insert(Symbol('X', 6));
NonlinearFactorGraph smootherFactors; // NonlinearFactorGraph smootherFactors;
FindFactorsWithAny(smootherKeys, fullGraph, smootherFactors); // FindFactorsWithAny(smootherKeys, fullGraph, smootherFactors);
//
// Extract the filter summarized factors. This consists of the linear cached factors from // // 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 // // the filter branches X8 and X10, as well as any nonlinear factor that involves only root keys
NonlinearFactorGraph filterSummarization; // 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', 8)))->cachedFactor()), ordering, linpoint));
filterSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 10)))->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; // std::set<Key> rootKeys;
rootKeys.insert(Symbol('X', 7)); // rootKeys.insert(Symbol('X', 7));
rootKeys.insert(Symbol('X', 9)); // rootKeys.insert(Symbol('X', 9));
rootKeys.insert(Symbol('X', 12)); // rootKeys.insert(Symbol('X', 12));
FindFactorsWithOnly(rootKeys, fullGraph, filterSummarization); // FindFactorsWithOnly(rootKeys, fullGraph, filterSummarization);
//
//
//
// Perform the synchronization procedure // // Perform the synchronization procedure
NonlinearFactorGraph actualSmootherSummarization; // NonlinearFactorGraph actualSmootherSummarization;
smoother.presync(); // smoother.presync();
smoother.getSummarizedFactors(actualSmootherSummarization); // smoother.getSummarizedFactors(actualSmootherSummarization);
smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues); // smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues);
smoother.postsync(); // smoother.postsync();
//
// Verify the returned smoother values is empty in the first iteration // // Verify the returned smoother values is empty in the first iteration
NonlinearFactorGraph expectedSmootherSummarization; // NonlinearFactorGraph expectedSmootherSummarization;
CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-4)); // CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-4));
//
//
//
// Perform a full update of the smoother. Since the root values/summarized filter factors were // // 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 // // created at the optimal values, the smoother should be identical to the batch optimization
smoother.update(); // smoother.update();
Values actualSmootherTheta = smoother.calculateEstimate(); // Values actualSmootherTheta = smoother.calculateEstimate();
//
// Create the expected values as the optimal set // // Create the expected values as the optimal set
Values expectedSmootherTheta; // Values expectedSmootherTheta;
expectedSmootherTheta.insert(Symbol('X', 0), optimalTheta.at(Symbol('X', 0))); // expectedSmootherTheta.insert(Symbol('X', 0), optimalTheta.at(Symbol('X', 0)));
expectedSmootherTheta.insert(Symbol('X', 1), optimalTheta.at(Symbol('X', 1))); // expectedSmootherTheta.insert(Symbol('X', 1), optimalTheta.at(Symbol('X', 1)));
expectedSmootherTheta.insert(Symbol('X', 2), optimalTheta.at(Symbol('X', 2))); // expectedSmootherTheta.insert(Symbol('X', 2), optimalTheta.at(Symbol('X', 2)));
expectedSmootherTheta.insert(Symbol('X', 3), optimalTheta.at(Symbol('X', 3))); // expectedSmootherTheta.insert(Symbol('X', 3), optimalTheta.at(Symbol('X', 3)));
expectedSmootherTheta.insert(Symbol('X', 4), optimalTheta.at(Symbol('X', 4))); // expectedSmootherTheta.insert(Symbol('X', 4), optimalTheta.at(Symbol('X', 4)));
expectedSmootherTheta.insert(Symbol('X', 5), optimalTheta.at(Symbol('X', 5))); // expectedSmootherTheta.insert(Symbol('X', 5), optimalTheta.at(Symbol('X', 5)));
expectedSmootherTheta.insert(Symbol('X', 6), optimalTheta.at(Symbol('X', 6))); // expectedSmootherTheta.insert(Symbol('X', 6), optimalTheta.at(Symbol('X', 6)));
//
// Compare filter solution with full batch // // 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
// CHECK(assert_equal(expectedSmootherTheta, actualSmootherTheta, 1e-4)); // CHECK(assert_equal(expectedSmootherTheta, actualSmootherTheta, 1e-4));
//
//
//
// Now perform a second synchronization to test the smoother-calculated summarization // // Add a loop closure factor to the smoother and re-check. Since the filter
actualSmootherSummarization.resize(0); // // factors were created at the optimal linpoint, and since the new loop closure
smootherFactors.resize(0); // // does not involve filter keys, the smoother should still yeild the optimal solution
smootherValues.clear(); // // The new Bayes Tree is:
smoother.presync(); // // Bayes Tree:
smoother.getSummarizedFactors(actualSmootherSummarization); // // P( X7 X9 X12 )
smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues); // // P( X10 | X9 )
smoother.postsync(); // // P( X11 | X10 )
// // P( X8 | X9 )
// Extract the expected smoother summarization from the Bayes Tree // // P( X6 | X7 X9 )
// The smoother branches after the addition of the loop closure is only X6 // // P( X4 | X6 X7 )
expectedSmootherSummarization.resize(0); // // P( X3 X5 | X4 X6 )
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(bayesTree.nodes().at(ordering.at(Symbol('X', 6)))->cachedFactor()); // // P( X2 | X3 )
LinearizedJacobianFactor::shared_ptr ljf(new LinearizedJacobianFactor(jf, ordering, linpoint)); // // P( X1 | X3 X4 )
expectedSmootherSummarization.push_back(ljf); // // P( X0 | X1 )
// Pose3 poseDelta = fullTheta.at<Pose3>(Symbol('X', 1)).between(fullTheta.at<Pose3>(Symbol('X', 4)));
// Compare smoother factors with the expected factors by computing the hessian information matrix // NonlinearFactor::shared_ptr loopClosure = NonlinearFactor::shared_ptr(new BetweenFactor<Pose3>(Symbol('X', 1), Symbol('X', 4), poseDelta, noiseOdometery));
// TODO: Check This // fullGraph.push_back(loopClosure);
// CHECK(hessian_equal(expectedSmootherSummarization, actualSmootherSummarization, linpoint, 1e-4)); // optimalTheta = BatchOptimize(fullGraph, fullTheta, rootValues);
//
// // Recreate the Bayes Tree
// linpoint.clear();
// TODO: Modify the second synchronization so that the filter sends an additional set of factors. // linpoint.insert(optimalTheta);
// I'm not sure what additional code this will exercise, but just for good measure. // 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);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}