Changes in CFS for unordered (still work in progress)
Luca Carlone
parent
6337a65ce7
commit
3701dc6322
|
|
@ -23,10 +23,9 @@
|
|||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/LinearContainerFactor.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
|
|
@ -96,19 +95,19 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
|
|||
ordering.push_back(key);
|
||||
}
|
||||
|
||||
GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint, ordering);
|
||||
|
||||
// Create the set of marginalizable variables
|
||||
std::vector<Index> linearIndices;
|
||||
BOOST_FOREACH(Key key, keysToMarginalize) {
|
||||
linearIndices.push_back(ordering.at(key));
|
||||
GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint);
|
||||
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
|
||||
|
||||
NonlinearFactorGraph LinearContainerForGaussianMarginals;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
|
||||
LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, linPoint));
|
||||
}
|
||||
|
||||
std::pair<GaussianConditional::shared_ptr, GaussianFactorGraph> marginal = linearGraph.eliminate(linearIndices, EliminateCholesky);
|
||||
|
||||
NonlinearFactorGraph LinearContainerForGaussianMarginals;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal.second) {
|
||||
LinearContainerForGaussianMarginals.add(LinearContainerFactor(factor, ordering, linPoint));
|
||||
LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, ordering, linPoint));
|
||||
}
|
||||
return LinearContainerForGaussianMarginals;
|
||||
}
|
||||
|
|
@ -158,8 +157,8 @@ TEST( ConcurrentIncrementalFilter, getFactors )
|
|||
|
||||
// Add some factors to the filter
|
||||
NonlinearFactorGraph newFactors1;
|
||||
newFactors1.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors1.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
Values newValues1;
|
||||
newValues1.insert(1, Pose3());
|
||||
newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
|
||||
|
|
@ -175,8 +174,8 @@ TEST( ConcurrentIncrementalFilter, getFactors )
|
|||
|
||||
// Add some more factors to the filter
|
||||
NonlinearFactorGraph newFactors2;
|
||||
newFactors2.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors2.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
Values newValues2;
|
||||
newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
|
||||
newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
|
||||
|
|
@ -208,8 +207,8 @@ TEST( ConcurrentIncrementalFilter, getLinearizationPoint )
|
|||
|
||||
// Add some factors to the filter
|
||||
NonlinearFactorGraph newFactors1;
|
||||
newFactors1.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors1.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
Values newValues1;
|
||||
newValues1.insert(1, Pose3());
|
||||
newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
|
||||
|
|
@ -225,8 +224,8 @@ TEST( ConcurrentIncrementalFilter, getLinearizationPoint )
|
|||
|
||||
// Add some more factors to the filter
|
||||
NonlinearFactorGraph newFactors2;
|
||||
newFactors2.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors2.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
Values newValues2;
|
||||
newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
|
||||
newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
|
||||
|
|
@ -242,12 +241,6 @@ TEST( ConcurrentIncrementalFilter, getLinearizationPoint )
|
|||
CHECK(assert_equal(expected3, actual3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ConcurrentIncrementalFilter, getOrdering )
|
||||
{
|
||||
// TODO: Think about how to check ordering...
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ConcurrentIncrementalFilter, getDelta )
|
||||
{
|
||||
|
|
@ -270,8 +263,8 @@ TEST( ConcurrentIncrementalFilter, calculateEstimate )
|
|||
|
||||
// Add some factors to the filter
|
||||
NonlinearFactorGraph newFactors2;
|
||||
newFactors2.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors2.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
Values newValues2;
|
||||
newValues2.insert(1, Pose3().compose(poseError));
|
||||
newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
|
||||
|
|
@ -290,8 +283,8 @@ TEST( ConcurrentIncrementalFilter, calculateEstimate )
|
|||
|
||||
// Add some more factors to the filter
|
||||
NonlinearFactorGraph newFactors3;
|
||||
newFactors3.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors3.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
Values newValues3;
|
||||
newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
|
||||
newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
|
||||
|
|
@ -354,8 +347,8 @@ TEST( ConcurrentIncrementalFilter, update_multiple )
|
|||
|
||||
// Add some factors to the filter
|
||||
NonlinearFactorGraph newFactors2;
|
||||
newFactors2.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors2.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
Values newValues2;
|
||||
newValues2.insert(1, Pose3().compose(poseError));
|
||||
newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
|
||||
|
|
@ -374,8 +367,8 @@ TEST( ConcurrentIncrementalFilter, update_multiple )
|
|||
|
||||
// Add some more factors to the filter
|
||||
NonlinearFactorGraph newFactors3;
|
||||
newFactors3.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors3.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
Values newValues3;
|
||||
newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
|
||||
newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
|
||||
|
|
@ -404,10 +397,10 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
|
|||
|
||||
// Add some factors to the filter
|
||||
NonlinearFactorGraph newFactors;
|
||||
newFactors.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
Values newValues;
|
||||
newValues.insert(1, Pose3().compose(poseError));
|
||||
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
|
||||
|
|
@ -434,9 +427,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
|
|||
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
NonlinearFactorGraph partialGraph;
|
||||
partialGraph.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
partialGraph.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
partialGraph.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
partialGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
partialGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
partialGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
|
||||
Values partialValues;
|
||||
// ISAM2 after the update is computing the delta, without applying them to the current lin point
|
||||
|
|
@ -444,19 +437,10 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
|
|||
partialValues.insert(2, newValues.at(2));
|
||||
partialValues.insert(3, newValues.at(3));
|
||||
|
||||
// Create an ordering
|
||||
Ordering ordering;
|
||||
ordering.push_back(1);
|
||||
ordering.push_back(2);
|
||||
ordering.push_back(3);
|
||||
|
||||
// Create the set of marginalizable variables
|
||||
std::vector<Index> linearIndices;
|
||||
linearIndices.push_back(ordering.at(1));
|
||||
linearIndices.push_back(ordering.at(2));
|
||||
GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint);
|
||||
|
||||
GaussianFactorGraph linearPartialGraph = *partialGraph.linearize(partialValues, ordering);
|
||||
std::pair<GaussianConditional::shared_ptr, GaussianFactorGraph> result = linearPartialGraph.eliminate(linearIndices, EliminateCholesky);
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
|
||||
|
||||
NonlinearFactorGraph expectedGraph;
|
||||
|
||||
|
|
@ -468,13 +452,13 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
|
|||
expectedGraph.push_back(NonlinearFactor::shared_ptr());
|
||||
expectedGraph.push_back(NonlinearFactor::shared_ptr());
|
||||
// ==========================================================
|
||||
expectedGraph.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) {
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
|
||||
// the linearization point for the linear container is optional, but it is not used in the filter,
|
||||
// therefore if we add it here it will not pass the test
|
||||
// expectedGraph.add(LinearContainerFactor(factor, ordering, partialValues));
|
||||
expectedGraph.add(LinearContainerFactor(factor, ordering));
|
||||
// expectedGraph.push_back(LinearContainerFactor(factor, ordering, partialValues));
|
||||
expectedGraph.push_back(LinearContainerFactor(factor));
|
||||
}
|
||||
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
|
|
@ -503,10 +487,10 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
|
|||
|
||||
// Add some factors to the filter
|
||||
NonlinearFactorGraph newFactors;
|
||||
newFactors.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
Values newValues;
|
||||
newValues.insert(1, Pose3().compose(poseError));
|
||||
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
|
||||
|
|
@ -534,9 +518,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
|
|||
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
NonlinearFactorGraph partialGraph;
|
||||
partialGraph.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
partialGraph.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
partialGraph.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
partialGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
partialGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
partialGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
|
||||
Values partialValues;
|
||||
// ISAM2 after the update is computing the delta, without applying them to the current lin point
|
||||
|
|
@ -544,38 +528,30 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
|
|||
partialValues.insert(2, optimalValues.at(2));
|
||||
partialValues.insert(3, optimalValues.at(3));
|
||||
|
||||
// Create an ordering
|
||||
Ordering ordering;
|
||||
ordering.push_back(1);
|
||||
ordering.push_back(2);
|
||||
ordering.push_back(3);
|
||||
|
||||
// Create the set of marginalizable variables
|
||||
std::vector<Index> linearIndices;
|
||||
linearIndices.push_back(ordering.at(1));
|
||||
linearIndices.push_back(ordering.at(2));
|
||||
|
||||
GaussianFactorGraph linearPartialGraph = *partialGraph.linearize(partialValues, ordering);
|
||||
std::pair<GaussianConditional::shared_ptr, GaussianFactorGraph> result = linearPartialGraph.eliminate(linearIndices, EliminateCholesky);
|
||||
GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint);
|
||||
|
||||
NonlinearFactorGraph expectedGraph;
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
|
||||
|
||||
// These three lines create three empty factors in expectedGraph:
|
||||
// this is done since the equal function in NonlinearFactorGraph also cares about the ordering of the factors
|
||||
// and in the actualGraph produced by the HMF we first insert 5 nonlinear factors, then we delete 3 of them, by
|
||||
// substituting them with a linear marginal
|
||||
expectedGraph.push_back(NonlinearFactor::shared_ptr());
|
||||
expectedGraph.push_back(NonlinearFactor::shared_ptr());
|
||||
expectedGraph.push_back(NonlinearFactor::shared_ptr());
|
||||
// ==========================================================
|
||||
expectedGraph.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
NonlinearFactorGraph expectedGraph;
|
||||
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) {
|
||||
// the linearization point for the linear container is optional, but it is not used in the filter,
|
||||
// therefore if we add it here it will not pass the test
|
||||
// expectedGraph.add(LinearContainerFactor(factor, ordering, partialValues));
|
||||
expectedGraph.add(LinearContainerFactor(factor, ordering));
|
||||
}
|
||||
// These three lines create three empty factors in expectedGraph:
|
||||
// this is done since the equal function in NonlinearFactorGraph also cares about the ordering of the factors
|
||||
// and in the actualGraph produced by the HMF we first insert 5 nonlinear factors, then we delete 3 of them, by
|
||||
// substituting them with a linear marginal
|
||||
expectedGraph.push_back(NonlinearFactor::shared_ptr());
|
||||
expectedGraph.push_back(NonlinearFactor::shared_ptr());
|
||||
expectedGraph.push_back(NonlinearFactor::shared_ptr());
|
||||
// ==========================================================
|
||||
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
|
||||
// the linearization point for the linear container is optional, but it is not used in the filter,
|
||||
// therefore if we add it here it will not pass the test
|
||||
// expectedGraph.push_back(LinearContainerFactor(factor, ordering, partialValues));
|
||||
expectedGraph.push_back(LinearContainerFactor(factor));
|
||||
}
|
||||
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
|
||||
|
|
@ -650,8 +626,8 @@ TEST( ConcurrentIncrementalFilter, synchronize_1 )
|
|||
// Insert factors into the filter, but do not marginalize out any variables.
|
||||
// The synchronization should still be empty
|
||||
NonlinearFactorGraph newFactors;
|
||||
newFactors.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
Values newValues;
|
||||
newValues.insert(1, Pose3().compose(poseError));
|
||||
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
|
||||
|
|
@ -813,7 +789,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_3 )
|
|||
expectedFilterSeparatorValues.insert(2, newValues.at(2));
|
||||
// ------------------------------------------------------------------------------
|
||||
NonlinearFactorGraph partialGraph;
|
||||
partialGraph.add(factor3);
|
||||
partialGraph.push_back(factor3);
|
||||
|
||||
Values partialValues;
|
||||
partialValues.insert(2, newValues.at<Pose3>(2));
|
||||
|
|
@ -902,7 +878,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_4 )
|
|||
expectedFilterSeparatorValues.insert(2, newValues.at<Pose3>(2));
|
||||
// ------------------------------------------------------------------------------
|
||||
NonlinearFactorGraph partialGraphFilter;
|
||||
partialGraphFilter.add(factor3);
|
||||
partialGraphFilter.push_back(factor3);
|
||||
|
||||
Values partialValuesFilter;
|
||||
partialValuesFilter.insert(2, newValues.at<Pose3>(2));
|
||||
|
|
@ -995,8 +971,8 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 )
|
|||
filter.postsync();
|
||||
|
||||
NonlinearFactorGraph expectedSmootherFactors;
|
||||
expectedSmootherFactors.add(factor1);
|
||||
expectedSmootherFactors.add(factor2);
|
||||
expectedSmootherFactors.push_back(factor1);
|
||||
expectedSmootherFactors.push_back(factor2);
|
||||
|
||||
Values optimalValues = BatchOptimize(newFactors, newValues, 1);
|
||||
Values expectedSmootherValues;
|
||||
|
|
@ -1026,11 +1002,11 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 )
|
|||
// currently the smoother contains factor 1 and 2 and node 1 and 2
|
||||
|
||||
NonlinearFactorGraph partialGraph;
|
||||
partialGraph.add(factor1);
|
||||
partialGraph.add(factor2);
|
||||
partialGraph.push_back(factor1);
|
||||
partialGraph.push_back(factor2);
|
||||
|
||||
// we also assume that the smoother received an extra factor (e.g., a prior on 1)
|
||||
partialGraph.add(factor1);
|
||||
partialGraph.push_back(factor1);
|
||||
|
||||
Values partialValues;
|
||||
// Incrementaloptimization updates all linearization points but the ones of the separator
|
||||
|
|
@ -1059,8 +1035,8 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 )
|
|||
filter.postsync();
|
||||
|
||||
NonlinearFactorGraph expectedSmootherFactors2;
|
||||
expectedSmootherFactors2.add(factor3);
|
||||
expectedSmootherFactors2.add(factor4);
|
||||
expectedSmootherFactors2.push_back(factor3);
|
||||
expectedSmootherFactors2.push_back(factor4);
|
||||
|
||||
Values expectedSmootherValues2;
|
||||
expectedSmootherValues2.insert(2, newValues.at(2));
|
||||
|
|
@ -1075,7 +1051,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 )
|
|||
// ------------------------------------------------------------------------------
|
||||
// This cannot be nonempty for the first call to synchronize
|
||||
NonlinearFactorGraph partialGraphFilter;
|
||||
partialGraphFilter.add(factor5);
|
||||
partialGraphFilter.push_back(factor5);
|
||||
|
||||
|
||||
Values partialValuesFilter;
|
||||
|
|
@ -1096,8 +1072,8 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 )
|
|||
// Now we should check that the smooother summarization on the old separator is correctly propagated
|
||||
// on the new separator by the filter
|
||||
NonlinearFactorGraph partialGraphTransition;
|
||||
partialGraphTransition.add(factor3);
|
||||
partialGraphTransition.add(factor4);
|
||||
partialGraphTransition.push_back(factor3);
|
||||
partialGraphTransition.push_back(factor4);
|
||||
partialGraphTransition.push_back(smootherSummarization2);
|
||||
|
||||
Values partialValuesTransition;
|
||||
|
|
@ -1119,7 +1095,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 )
|
|||
expectedFilterGraph.push_back(factorEmpty);
|
||||
expectedFilterGraph.push_back(factorEmpty);
|
||||
expectedFilterGraph.push_back(factorEmpty);
|
||||
expectedFilterGraph.add(factor5);
|
||||
expectedFilterGraph.push_back(factor5);
|
||||
expectedFilterGraph.push_back(factorEmpty);
|
||||
expectedFilterGraph.push_back(factorEmpty);
|
||||
expectedFilterGraph.push_back(factorEmpty);
|
||||
|
|
@ -1143,10 +1119,10 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
|
|||
NonlinearFactor::shared_ptr factor4(new BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
|
||||
NonlinearFactorGraph factorGraph;
|
||||
factorGraph.add(factor1);
|
||||
factorGraph.add(factor2);
|
||||
factorGraph.add(factor3);
|
||||
factorGraph.add(factor4);
|
||||
factorGraph.push_back(factor1);
|
||||
factorGraph.push_back(factor2);
|
||||
factorGraph.push_back(factor3);
|
||||
factorGraph.push_back(factor4);
|
||||
|
||||
Values newValues;
|
||||
Pose3 value1 = Pose3().compose(poseError);
|
||||
|
|
@ -1157,26 +1133,18 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
|
|||
newValues.insert(2, value2);
|
||||
newValues.insert(3, value3);
|
||||
|
||||
// We first manually
|
||||
// Create an ordering
|
||||
Ordering ordering;
|
||||
ordering.push_back(1);
|
||||
ordering.push_back(2);
|
||||
ordering.push_back(3);
|
||||
|
||||
GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues, ordering);
|
||||
|
||||
// Create the set of marginalizable variables
|
||||
std::vector<Index> linearIndices;
|
||||
linearIndices.push_back(ordering.at(1));
|
||||
std::vector<Key> linearIndices;
|
||||
linearIndices.push_back(1);
|
||||
|
||||
std::pair<GaussianConditional::shared_ptr, GaussianFactorGraph> result = linearFactorGraph.eliminate(linearIndices, EliminateCholesky);
|
||||
GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
|
||||
|
||||
NonlinearFactorGraph expectedMarginals;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) {
|
||||
expectedMarginals.add(LinearContainerFactor(factor, ordering, newValues));
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
|
||||
|
||||
}
|
||||
NonlinearFactorGraph expectedMarginals;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
|
||||
expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
|
||||
}
|
||||
|
||||
FastList<Key> keysToMarginalize;
|
||||
keysToMarginalize.push_back(1);
|
||||
|
|
@ -1201,10 +1169,10 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
|
|||
NonlinearFactor::shared_ptr factor4(new BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
|
||||
NonlinearFactorGraph factorGraph;
|
||||
factorGraph.add(factor1);
|
||||
factorGraph.add(factor2);
|
||||
factorGraph.add(factor3);
|
||||
factorGraph.add(factor4);
|
||||
factorGraph.push_back(factor1);
|
||||
factorGraph.push_back(factor2);
|
||||
factorGraph.push_back(factor3);
|
||||
factorGraph.push_back(factor4);
|
||||
|
||||
Values newValues;
|
||||
Pose3 value1 = Pose3().compose(poseError);
|
||||
|
|
@ -1215,28 +1183,24 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
|
|||
newValues.insert(2, value2);
|
||||
newValues.insert(3, value3);
|
||||
|
||||
// We first manually
|
||||
// Create an ordering
|
||||
Ordering ordering;
|
||||
ordering.push_back(1);
|
||||
ordering.push_back(2);
|
||||
ordering.push_back(3);
|
||||
|
||||
GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues, ordering);
|
||||
|
||||
// Create the set of marginalizable variables
|
||||
std::vector<Index> linearIndices;
|
||||
linearIndices.push_back(ordering.at(1));
|
||||
linearIndices.push_back(ordering.at(2));
|
||||
// Create the set of marginalizable variables
|
||||
std::vector<Key> linearIndices;
|
||||
linearIndices.push_back(1);
|
||||
linearIndices.push_back(2);
|
||||
|
||||
std::pair<GaussianConditional::shared_ptr, GaussianFactorGraph> result = linearFactorGraph.eliminate(linearIndices, EliminateCholesky);
|
||||
|
||||
GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
|
||||
|
||||
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
|
||||
|
||||
NonlinearFactorGraph expectedMarginals;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) {
|
||||
expectedMarginals.add(LinearContainerFactor(factor, ordering, newValues));
|
||||
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
|
||||
expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
|
||||
}
|
||||
|
||||
|
||||
FastList<Key> keysToMarginalize;
|
||||
keysToMarginalize.push_back(1);
|
||||
keysToMarginalize.push_back(2);
|
||||
|
|
|
|||
|
|
@ -115,8 +115,8 @@ TEST( ConcurrentIncrementalSmootherDL, getFactors )
|
|||
|
||||
// Add some factors to the smoother
|
||||
NonlinearFactorGraph newFactors1;
|
||||
newFactors1.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors1.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
Values newValues1;
|
||||
newValues1.insert(1, Pose3());
|
||||
newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
|
||||
|
|
@ -132,8 +132,8 @@ TEST( ConcurrentIncrementalSmootherDL, getFactors )
|
|||
|
||||
// Add some more factors to the smoother
|
||||
NonlinearFactorGraph newFactors2;
|
||||
newFactors2.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors2.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
Values newValues2;
|
||||
newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
|
||||
newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
|
||||
|
|
@ -166,8 +166,8 @@ TEST( ConcurrentIncrementalSmootherDL, getLinearizationPoint )
|
|||
|
||||
// Add some factors to the smoother
|
||||
NonlinearFactorGraph newFactors1;
|
||||
newFactors1.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors1.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
Values newValues1;
|
||||
newValues1.insert(1, Pose3());
|
||||
newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
|
||||
|
|
@ -183,8 +183,8 @@ TEST( ConcurrentIncrementalSmootherDL, getLinearizationPoint )
|
|||
|
||||
// Add some more factors to the smoother
|
||||
NonlinearFactorGraph newFactors2;
|
||||
newFactors2.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors2.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
Values newValues2;
|
||||
newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
|
||||
newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
|
||||
|
|
@ -200,12 +200,6 @@ TEST( ConcurrentIncrementalSmootherDL, getLinearizationPoint )
|
|||
CHECK(assert_equal(expected3, actual3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ConcurrentIncrementalSmootherDL, getOrdering )
|
||||
{
|
||||
// TODO: Think about how to check ordering...
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ConcurrentIncrementalSmootherDL, getDelta )
|
||||
{
|
||||
|
|
@ -229,8 +223,8 @@ TEST( ConcurrentIncrementalSmootherDL, calculateEstimate )
|
|||
|
||||
// Add some factors to the smoother
|
||||
NonlinearFactorGraph newFactors2;
|
||||
newFactors2.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors2.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
Values newValues2;
|
||||
newValues2.insert(1, Pose3().compose(poseError));
|
||||
newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
|
||||
|
|
@ -249,8 +243,8 @@ TEST( ConcurrentIncrementalSmootherDL, calculateEstimate )
|
|||
|
||||
// Add some more factors to the smoother
|
||||
NonlinearFactorGraph newFactors3;
|
||||
newFactors3.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors3.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
Values newValues3;
|
||||
newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
|
||||
newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
|
||||
|
|
@ -316,8 +310,8 @@ TEST( ConcurrentIncrementalSmootherDL, update_multiple )
|
|||
|
||||
// Add some factors to the smoother
|
||||
NonlinearFactorGraph newFactors2;
|
||||
newFactors2.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors2.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
Values newValues2;
|
||||
newValues2.insert(1, Pose3().compose(poseError));
|
||||
newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
|
||||
|
|
@ -336,8 +330,8 @@ TEST( ConcurrentIncrementalSmootherDL, update_multiple )
|
|||
|
||||
// Add some more factors to the smoother
|
||||
NonlinearFactorGraph newFactors3;
|
||||
newFactors3.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors3.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
Values newValues3;
|
||||
newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
|
||||
newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
|
||||
|
|
@ -403,11 +397,9 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_1 )
|
|||
Values smootherValues, filterSeparatorValues;
|
||||
filterSeparatorValues.insert(1, Pose3().compose(poseError));
|
||||
filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
|
||||
Ordering ordering;
|
||||
ordering.push_back(1);
|
||||
ordering.push_back(2);
|
||||
filterSumarization.add(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
|
||||
filterSumarization.add(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
|
||||
|
||||
filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
|
||||
filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
|
||||
|
||||
// Create expected values: the smoother output will be empty for this case
|
||||
NonlinearFactorGraph expectedSmootherSummarization;
|
||||
|
|
@ -467,15 +459,13 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_2 )
|
|||
// Create a separator and cached smoother factors *from* the filter
|
||||
NonlinearFactorGraph smootherFactors, filterSumarization;
|
||||
Values smootherValues, filterSeparatorValues;
|
||||
Ordering ordering;
|
||||
ordering.push_back(1);
|
||||
ordering.push_back(2);
|
||||
|
||||
filterSeparatorValues.insert(1, Pose3().compose(poseError));
|
||||
filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
|
||||
filterSumarization.add(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
|
||||
filterSumarization.add(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
|
||||
smootherFactors.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
smootherFactors.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
|
||||
filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues,), filterSeparatorValues));
|
||||
smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
|
||||
smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
|
||||
|
||||
|
|
@ -548,16 +538,14 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
|
|||
// Create a separator and cached smoother factors *from* the filter
|
||||
NonlinearFactorGraph smootherFactors, filterSumarization;
|
||||
Values smootherValues, filterSeparatorValues;
|
||||
Ordering ordering;
|
||||
ordering.push_back(1);
|
||||
ordering.push_back(2);
|
||||
|
||||
filterSeparatorValues.insert(1, Pose3().compose(poseError));
|
||||
filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
|
||||
filterSumarization.add(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
|
||||
filterSumarization.add(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
|
||||
smootherFactors.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
smootherFactors.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
smootherFactors.add(PriorFactor<Pose3>(4, poseInitial, noisePrior));
|
||||
filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
|
||||
filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
|
||||
smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
smootherFactors.push_back(PriorFactor<Pose3>(4, poseInitial, noisePrior));
|
||||
smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
|
||||
smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
|
||||
|
||||
|
|
@ -574,7 +562,6 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
|
|||
|
||||
// Check
|
||||
CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
|
||||
CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
|
||||
|
||||
|
||||
// Update the smoother
|
||||
|
|
@ -602,7 +589,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
|
|||
|
||||
expectedSmootherSummarization.resize(0);
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) {
|
||||
expectedSmootherSummarization.add(LinearContainerFactor(factor, ordering, allValues));
|
||||
expectedSmootherSummarization.push_back(LinearContainerFactor(factor, ordering, allValues));
|
||||
}
|
||||
|
||||
CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
|
||||
|
|
|
|||
|
|
@ -23,10 +23,9 @@
|
|||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/LinearContainerFactor.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
|
|
@ -115,8 +114,8 @@ TEST( ConcurrentIncrementalSmootherGN, getFactors )
|
|||
|
||||
// Add some factors to the smoother
|
||||
NonlinearFactorGraph newFactors1;
|
||||
newFactors1.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors1.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
Values newValues1;
|
||||
newValues1.insert(1, Pose3());
|
||||
newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
|
||||
|
|
@ -132,8 +131,8 @@ TEST( ConcurrentIncrementalSmootherGN, getFactors )
|
|||
|
||||
// Add some more factors to the smoother
|
||||
NonlinearFactorGraph newFactors2;
|
||||
newFactors2.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors2.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
Values newValues2;
|
||||
newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
|
||||
newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
|
||||
|
|
@ -166,8 +165,8 @@ TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint )
|
|||
|
||||
// Add some factors to the smoother
|
||||
NonlinearFactorGraph newFactors1;
|
||||
newFactors1.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors1.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
Values newValues1;
|
||||
newValues1.insert(1, Pose3());
|
||||
newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
|
||||
|
|
@ -183,8 +182,8 @@ TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint )
|
|||
|
||||
// Add some more factors to the smoother
|
||||
NonlinearFactorGraph newFactors2;
|
||||
newFactors2.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors2.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
Values newValues2;
|
||||
newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
|
||||
newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
|
||||
|
|
@ -200,12 +199,6 @@ TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint )
|
|||
CHECK(assert_equal(expected3, actual3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ConcurrentIncrementalSmootherGN, getOrdering )
|
||||
{
|
||||
// TODO: Think about how to check ordering...
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ConcurrentIncrementalSmootherGN, getDelta )
|
||||
{
|
||||
|
|
@ -229,8 +222,8 @@ TEST( ConcurrentIncrementalSmootherGN, calculateEstimate )
|
|||
|
||||
// Add some factors to the smoother
|
||||
NonlinearFactorGraph newFactors2;
|
||||
newFactors2.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors2.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
Values newValues2;
|
||||
newValues2.insert(1, Pose3().compose(poseError));
|
||||
newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
|
||||
|
|
@ -249,8 +242,8 @@ TEST( ConcurrentIncrementalSmootherGN, calculateEstimate )
|
|||
|
||||
// Add some more factors to the smoother
|
||||
NonlinearFactorGraph newFactors3;
|
||||
newFactors3.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors3.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
Values newValues3;
|
||||
newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
|
||||
newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
|
||||
|
|
@ -316,8 +309,8 @@ TEST( ConcurrentIncrementalSmootherGN, update_multiple )
|
|||
|
||||
// Add some factors to the smoother
|
||||
NonlinearFactorGraph newFactors2;
|
||||
newFactors2.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors2.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
|
||||
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
|
||||
Values newValues2;
|
||||
newValues2.insert(1, Pose3().compose(poseError));
|
||||
newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
|
||||
|
|
@ -336,8 +329,8 @@ TEST( ConcurrentIncrementalSmootherGN, update_multiple )
|
|||
|
||||
// Add some more factors to the smoother
|
||||
NonlinearFactorGraph newFactors3;
|
||||
newFactors3.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors3.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
Values newValues3;
|
||||
newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
|
||||
newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
|
||||
|
|
@ -406,8 +399,8 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_1 )
|
|||
Ordering ordering;
|
||||
ordering.push_back(1);
|
||||
ordering.push_back(2);
|
||||
filterSumarization.add(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
|
||||
filterSumarization.add(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
|
||||
filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
|
||||
filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
|
||||
|
||||
// Create expected values: the smoother output will be empty for this case
|
||||
NonlinearFactorGraph expectedSmootherSummarization;
|
||||
|
|
@ -467,15 +460,13 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_2 )
|
|||
// Create a separator and cached smoother factors *from* the filter
|
||||
NonlinearFactorGraph smootherFactors, filterSumarization;
|
||||
Values smootherValues, filterSeparatorValues;
|
||||
Ordering ordering;
|
||||
ordering.push_back(1);
|
||||
ordering.push_back(2);
|
||||
|
||||
filterSeparatorValues.insert(1, Pose3().compose(poseError));
|
||||
filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
|
||||
filterSumarization.add(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
|
||||
filterSumarization.add(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
|
||||
smootherFactors.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
smootherFactors.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
|
||||
filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
|
||||
smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
|
||||
smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
|
||||
|
||||
|
|
@ -548,16 +539,14 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
|
|||
// Create a separator and cached smoother factors *from* the filter
|
||||
NonlinearFactorGraph smootherFactors, filterSumarization;
|
||||
Values smootherValues, filterSeparatorValues;
|
||||
Ordering ordering;
|
||||
ordering.push_back(1);
|
||||
ordering.push_back(2);
|
||||
|
||||
filterSeparatorValues.insert(1, Pose3().compose(poseError));
|
||||
filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
|
||||
filterSumarization.add(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
|
||||
filterSumarization.add(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
|
||||
smootherFactors.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
smootherFactors.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
smootherFactors.add(PriorFactor<Pose3>(4, poseInitial, noisePrior));
|
||||
filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
|
||||
filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
|
||||
smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
|
||||
smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
|
||||
smootherFactors.push_back(PriorFactor<Pose3>(4, poseInitial, noisePrior));
|
||||
smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
|
||||
smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
|
||||
|
||||
|
|
@ -586,9 +575,9 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
|
|||
// Check the optimized value of smoother state
|
||||
NonlinearFactorGraph allFactors = smootherFactors;
|
||||
Values allValues = smoother.getLinearizationPoint();
|
||||
ordering = smoother.getOrdering(); // I'm really hoping this is an acceptable ordering...
|
||||
// ordering = smoother.getOrdering(); // I'm really hoping this is an acceptable ordering...
|
||||
|
||||
GaussianFactorGraph::shared_ptr LinFactorGraph = allFactors.linearize(allValues, ordering);
|
||||
GaussianFactorGraph::shared_ptr LinFactorGraph = allFactors.linearize(allValues);
|
||||
// GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph);
|
||||
// GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
|
||||
|
||||
|
|
@ -602,7 +591,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
|
|||
|
||||
expectedSmootherSummarization.resize(0);
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) {
|
||||
expectedSmootherSummarization.add(LinearContainerFactor(factor, ordering, allValues));
|
||||
expectedSmootherSummarization.push_back(LinearContainerFactor(factor, ordering, allValues));
|
||||
}
|
||||
|
||||
CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
|
||||
|
|
|
|||
Loading…
Reference in New Issue