gtsam/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testConcurrentIncrementalSmoother.cpp
 * @brief   Unit tests for the Concurrent Batch Smoother
 * @author  Stephen Williams (swilliams8@gatech.edu)
 * @date    Jan 5, 2013
 */

#include <gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>

using namespace std;
using namespace gtsam;

namespace {

// Set up initial pose, odometry difference, loop closure difference, and initialization errors
const Pose3 poseInitial;
const Pose3 poseOdometry( Rot3::RzRyRx(Vector3(0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
const Pose3 poseError( Rot3::RzRyRx(Vector3(0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );

// Set up noise models for the factors
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished());
const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0).finished());

/* ************************************************************************* */
Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) {

  // Create an L-M optimizer
  ISAM2Params parameters;
  parameters.optimizationParams = ISAM2GaussNewtonParams();
//  parameters.maxIterations = maxIter;
//  parameters.lambdaUpperBound = 1;
//  parameters.lambdaInitial = 1;
//  parameters.verbosity = NonlinearOptimizerParams::ERROR;
//  parameters.verbosityLM = ISAM2Params::DAMPED;
//  parameters.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_QR;

  ISAM2 optimizer(parameters);
  optimizer.update( graph, theta );
  Values result = optimizer.calculateEstimate();
  return result;

}

} // end namespace




/* ************************************************************************* */
TEST( ConcurrentIncrementalSmootherGN, equals )
{
  // TODO: Test 'equals' more vigorously

  // Create a Concurrent Batch Smoother
  ISAM2Params parameters1;
  parameters1.optimizationParams = ISAM2GaussNewtonParams();
  ConcurrentIncrementalSmoother smoother1(parameters1);

  // Create an identical Concurrent Batch Smoother
  ISAM2Params parameters2;
  parameters2.optimizationParams = ISAM2GaussNewtonParams();
  ConcurrentIncrementalSmoother smoother2(parameters2);

  // Create a different Concurrent Batch Smoother
  ISAM2Params parameters3;
  parameters3.optimizationParams = ISAM2GaussNewtonParams();
//  parameters3.maxIterations = 1;
  ConcurrentIncrementalSmoother smoother3(parameters3);

  CHECK(assert_equal(smoother1, smoother1));
  CHECK(assert_equal(smoother1, smoother2));
//  CHECK(assert_inequal(smoother1, smoother3));
}

/* ************************************************************************* */
TEST( ConcurrentIncrementalSmootherGN, getFactors )
{
  // Create a Concurrent Batch Smoother
  ISAM2Params parameters;
  parameters.optimizationParams = ISAM2GaussNewtonParams();
  ConcurrentIncrementalSmoother smoother(parameters);

  // Expected graph is empty
  NonlinearFactorGraph expected1;
  // Get actual graph
  NonlinearFactorGraph actual1 = smoother.getFactors();
  // Check
  CHECK(assert_equal(expected1, actual1));

  // Add some factors to the smoother
  NonlinearFactorGraph newFactors1;
  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));
  smoother.update(newFactors1, newValues1);

  // Expected graph
  NonlinearFactorGraph expected2;
  expected2.push_back(newFactors1);
  // Get actual graph
  NonlinearFactorGraph actual2 = smoother.getFactors();
  // Check
  CHECK(assert_equal(expected2, actual2));

  // Add some more factors to the smoother
  NonlinearFactorGraph newFactors2;
  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));
  smoother.update(newFactors2, newValues2);

  // Expected graph
  NonlinearFactorGraph expected3;
  expected3.push_back(newFactors1);
  expected3.push_back(newFactors2);
  // Get actual graph
  NonlinearFactorGraph actual3 = smoother.getFactors();
  // Check
  CHECK(assert_equal(expected3, actual3));
}

/* ************************************************************************* */
TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint )
{
  // Create a Concurrent Batch Smoother
  ISAM2Params parameters;
  parameters.optimizationParams = ISAM2GaussNewtonParams();
  ConcurrentIncrementalSmoother smoother(parameters);

  // Expected values is empty
  Values expected1;
  // Get Linearization Point
  Values actual1 = smoother.getLinearizationPoint();
  // Check
  CHECK(assert_equal(expected1, actual1));

  // Add some factors to the smoother
  NonlinearFactorGraph newFactors1;
  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));
  smoother.update(newFactors1, newValues1);

  // Expected values is equivalent to the provided values only because the provided linearization points were optimal
  Values expected2;
  expected2.insert(newValues1);
  // Get actual linearization point
  Values actual2 = smoother.getLinearizationPoint();
  // Check
  CHECK(assert_equal(expected2, actual2));

  // Add some more factors to the smoother
  NonlinearFactorGraph newFactors2;
  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));
  smoother.update(newFactors2, newValues2);

  // Expected values is equivalent to the provided values only because the provided linearization points were optimal
  Values expected3;
  expected3.insert(newValues1);
  expected3.insert(newValues2);
  // Get actual linearization point
  Values actual3 = smoother.getLinearizationPoint();
  // Check
  CHECK(assert_equal(expected3, actual3));
}

/* ************************************************************************* */
TEST( ConcurrentIncrementalSmootherGN, getDelta )
{
  // TODO: Think about how to check ordering...
}

/* ************************************************************************* */
TEST( ConcurrentIncrementalSmootherGN, calculateEstimate )
{
  // Create a Concurrent Batch Smoother
  ISAM2Params parameters;
  parameters.optimizationParams = ISAM2GaussNewtonParams();
  ConcurrentIncrementalSmoother smoother(parameters);

  // Expected values is empty
  Values expected1;
  // Get Linearization Point
  Values actual1 = smoother.calculateEstimate();
  // Check
  CHECK(assert_equal(expected1, actual1));

  // Add some factors to the smoother
  NonlinearFactorGraph newFactors2;
  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));
  smoother.update(newFactors2, newValues2);

  // Expected values from full batch optimization
  NonlinearFactorGraph allFactors2;
  allFactors2.push_back(newFactors2);
  Values allValues2;
  allValues2.insert(newValues2);
  Values expected2 = BatchOptimize(allFactors2, allValues2);
  // Get actual linearization point
  Values actual2 = smoother.calculateEstimate();
  // Check
  CHECK(assert_equal(expected2, actual2, 1e-6));

  // Add some more factors to the smoother
  NonlinearFactorGraph newFactors3;
  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));
  smoother.update(newFactors3, newValues3);

  // Expected values from full batch optimization
  NonlinearFactorGraph allFactors3;
  allFactors3.push_back(newFactors2);
  allFactors3.push_back(newFactors3);
  Values allValues3;
  allValues3.insert(newValues2);
  allValues3.insert(newValues3);
  Values expected3 = BatchOptimize(allFactors3, allValues3);
  // Get actual linearization point
  Values actual3 = smoother.calculateEstimate();
  // Check
  CHECK(assert_equal(expected3, actual3, 1e-6));

  // Also check the single-variable version
  Pose3 expectedPose1 = expected3.at<Pose3>(1);
  Pose3 expectedPose2 = expected3.at<Pose3>(2);
  Pose3 expectedPose3 = expected3.at<Pose3>(3);
  Pose3 expectedPose4 = expected3.at<Pose3>(4);
  // Also check the single-variable version
  Pose3 actualPose1 = smoother.calculateEstimate<Pose3>(1);
  Pose3 actualPose2 = smoother.calculateEstimate<Pose3>(2);
  Pose3 actualPose3 = smoother.calculateEstimate<Pose3>(3);
  Pose3 actualPose4 = smoother.calculateEstimate<Pose3>(4);
  // Check
  CHECK(assert_equal(expectedPose1, actualPose1, 1e-6));
  CHECK(assert_equal(expectedPose2, actualPose2, 1e-6));
  CHECK(assert_equal(expectedPose3, actualPose3, 1e-6));
  CHECK(assert_equal(expectedPose4, actualPose4, 1e-6));
}

/* ************************************************************************* */
TEST( ConcurrentIncrementalSmootherGN, update_empty )
{
  // Create a set of optimizer parameters
  ISAM2Params parameters;
  parameters.optimizationParams = ISAM2GaussNewtonParams();
  // Create a Concurrent Batch Smoother
  ConcurrentIncrementalSmoother smoother(parameters);

  // Call update
  smoother.update();
}

/* ************************************************************************* */
TEST( ConcurrentIncrementalSmootherGN, update_multiple )
{
  // Create a Concurrent Batch Smoother
  ISAM2Params parameters;
  parameters.optimizationParams = ISAM2GaussNewtonParams();
  ConcurrentIncrementalSmoother smoother(parameters);

  // Expected values is empty
  Values expected1;
  // Get Linearization Point
  Values actual1 = smoother.calculateEstimate();
  // Check
  CHECK(assert_equal(expected1, actual1));

  // Add some factors to the smoother
  NonlinearFactorGraph newFactors2;
  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));
  smoother.update(newFactors2, newValues2);

  // Expected values from full batch optimization
  NonlinearFactorGraph allFactors2;
  allFactors2.push_back(newFactors2);
  Values allValues2;
  allValues2.insert(newValues2);
  Values expected2 = BatchOptimize(allFactors2, allValues2);
  // Get actual linearization point
  Values actual2 = smoother.calculateEstimate();
  // Check
  CHECK(assert_equal(expected2, actual2, 1e-6));

  // Add some more factors to the smoother
  NonlinearFactorGraph newFactors3;
  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));
  smoother.update(newFactors3, newValues3);

  // Expected values from full batch optimization
  NonlinearFactorGraph allFactors3;
  allFactors3.push_back(newFactors2);
  allFactors3.push_back(newFactors3);
  Values allValues3;
  allValues3.insert(newValues2);
  allValues3.insert(newValues3);
  Values expected3 = BatchOptimize(allFactors3, allValues3);
  // Get actual linearization point
  Values actual3 = smoother.calculateEstimate();
  // Check
  CHECK(assert_equal(expected3, actual3, 1e-6));
}

/* ************************************************************************* */
TEST( ConcurrentIncrementalSmootherGN, synchronize_empty )
{
  // Create a set of optimizer parameters
  ISAM2Params parameters;
  parameters.optimizationParams = ISAM2GaussNewtonParams();
  // Create a Concurrent Batch Smoother
  ConcurrentIncrementalSmoother smoother(parameters);

  // Create empty containers *from* the filter
  NonlinearFactorGraph smootherFactors, filterSumarization;
  Values smootherValues, filterSeparatorValues;

  // Create expected values: these will be empty for this case
  NonlinearFactorGraph expectedSmootherSummarization;
  Values expectedSmootherSeparatorValues;

  // Synchronize
  NonlinearFactorGraph actualSmootherSummarization;
  Values actualSmootherSeparatorValues;
  smoother.presync();
  smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
  smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
  smoother.postsync();

  // Check
  CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
  CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
}

/* ************************************************************************* */
TEST( ConcurrentIncrementalSmootherGN, synchronize_1 )
{
  // Create a set of optimizer parameters
  ISAM2Params parameters;
  parameters.optimizationParams = ISAM2GaussNewtonParams();
//  parameters.maxIterations = 1;

  // Create a Concurrent Batch Smoother
  ConcurrentIncrementalSmoother smoother(parameters);

  // Create a simple separator *from* the filter
  NonlinearFactorGraph smootherFactors, filterSumarization;
  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.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;
  Values expectedSmootherSeparatorValues;

  NonlinearFactorGraph actualSmootherSummarization;
  Values actualSmootherSeparatorValues;
  smoother.presync();
  smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
  smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
  smoother.postsync();

  // Check
  CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
  CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));


  // Update the smoother
  smoother.update();

  // Check the factor graph. It should contain only the filter-provided factors
  NonlinearFactorGraph expectedFactorGraph = filterSumarization;
  NonlinearFactorGraph actualFactorGraph = smoother.getFactors();
  CHECK(assert_equal(expectedFactorGraph, actualFactorGraph, 1e-6));

  // Check the optimized value of smoother state
  NonlinearFactorGraph allFactors;
  allFactors.push_back(filterSumarization);
  Values allValues;
  allValues.insert(filterSeparatorValues);
  Values expectedValues = BatchOptimize(allFactors, allValues,1);
  Values actualValues = smoother.calculateEstimate();
  CHECK(assert_equal(expectedValues, actualValues, 1e-6));

  // Check the linearization point. The separator should remain identical to the filter provided values
  Values expectedLinearizationPoint = filterSeparatorValues;
  Values actualLinearizationPoint = smoother.getLinearizationPoint();
  CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
}


/* ************************************************************************* */
TEST( ConcurrentIncrementalSmootherGN, synchronize_2 )
{
  // Create a set of optimizer parameters
  ISAM2Params parameters;
  parameters.optimizationParams = ISAM2GaussNewtonParams();
//  parameters.maxIterations = 1;
//  parameters.lambdaUpperBound = 1;
//  parameters.lambdaInitial = 1;
//  parameters.verbosity = NonlinearOptimizerParams::ERROR;
//  parameters.verbosityLM = ISAM2Params::DAMPED;

  // Create a Concurrent Batch Smoother
  ConcurrentIncrementalSmoother smoother(parameters);

  // Create a separator and cached smoother factors *from* the filter
  NonlinearFactorGraph smootherFactors, filterSumarization;
  Values smootherValues, filterSeparatorValues;

  filterSeparatorValues.insert(1, Pose3().compose(poseError));
  filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
  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));

  // Create expected values: the smoother output will be empty for this case
  NonlinearFactorGraph expectedSmootherSummarization;
  Values expectedSmootherSeparatorValues;

  NonlinearFactorGraph actualSmootherSummarization;
  Values actualSmootherSeparatorValues;
  smoother.presync();
  smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
  smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
  smoother.postsync();

  // Check
  CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
  CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));


  // Update the smoother
  smoother.update();

  // Check the factor graph. It should contain only the filter-provided factors
  NonlinearFactorGraph expectedFactorGraph;
  expectedFactorGraph.push_back(smootherFactors);
  expectedFactorGraph.push_back(filterSumarization);
  NonlinearFactorGraph actualFactorGraph = smoother.getFactors();
  CHECK(assert_equal(expectedFactorGraph, actualFactorGraph, 1e-6));

  // Check the optimized value of smoother state
  NonlinearFactorGraph allFactors;
  allFactors.push_back(filterSumarization);
  allFactors.push_back(smootherFactors);
  Values allValues;
  allValues.insert(filterSeparatorValues);
  allValues.insert(smootherValues);
//  allValues.print("Batch LinPoint:\n");
  Values expectedValues = BatchOptimize(allFactors, allValues, 1);
  Values actualValues = smoother.calculateEstimate();
  CHECK(assert_equal(expectedValues, actualValues, 1e-6));

  // Check the linearization point. The separator should remain identical to the filter provided values, but the others should be the optimal values
  // Isam2 is changing internally the linearization points, so the following check is done only on the separator variables
//  Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1);
  Values expectedLinearizationPoint = filterSeparatorValues;
  Values actualLinearizationPoint;
  for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
    actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key));
  }
  CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
}



/* ************************************************************************* */
TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
{
  // Create a set of optimizer parameters
  ISAM2Params parameters;
  parameters.optimizationParams = ISAM2GaussNewtonParams();
//  parameters.maxIterations = 1;
//  parameters.lambdaUpperBound = 1;
//  parameters.lambdaInitial = 1;
//  parameters.verbosity = NonlinearOptimizerParams::ERROR;
//  parameters.verbosityLM = ISAM2Params::DAMPED;

  // Create a Concurrent Batch Smoother
  ConcurrentIncrementalSmoother smoother(parameters);

  // Create a separator and cached smoother factors *from* the filter
  NonlinearFactorGraph smootherFactors, filterSumarization;
  Values smootherValues, filterSeparatorValues;

  filterSeparatorValues.insert(1, Pose3().compose(poseError));
  filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
  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));

  // Create expected values: the smoother output will be empty for this case
  NonlinearFactorGraph expectedSmootherSummarization;
  Values expectedSmootherSeparatorValues;

  NonlinearFactorGraph actualSmootherSummarization;
  Values actualSmootherSeparatorValues;
  smoother.presync();
  smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
  smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
  smoother.postsync();

  // Check
  CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
  CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));


  // Update the smoother
  smoother.update();

  smoother.presync();
  smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);

  // 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...

  GaussianFactorGraph::shared_ptr LinFactorGraph = allFactors.linearize(allValues);
//  GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph);
//  GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();

  KeySet allkeys = LinFactorGraph->keys();
  for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues)
    allkeys.erase(key_value.key);
  KeyVector variables(allkeys.begin(), allkeys.end());
  std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);

  expectedSmootherSummarization.resize(0);
  for(const GaussianFactor::shared_ptr& factor: *result.second) {
    expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues));
  }

  CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));

}

// =========================================================================================================
///* ************************************************************************* */
TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 )
{
  // Create a set of optimizer parameters
  ISAM2Params parameters;
  parameters.optimizationParams = ISAM2GaussNewtonParams();

  // Create a Concurrent Batch Smoother
  ConcurrentIncrementalSmoother smoother(parameters);

  // Add some factors to the smoother
  NonlinearFactorGraph newFactors;
  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));
  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));
  newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
  newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));

  // Update the smoother: add all factors
  smoother.update(newFactors, newValues);

  // factor we want to remove
  // NOTE: we can remove factors, paying attention that the remaining graph remains connected
  // we remove a single factor, the number 1, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
  FactorIndices removeFactorIndices(2,1);

  // Add no factors to the smoother (we only want to test the removal)
  NonlinearFactorGraph noFactors;
  Values noValues;
  smoother.update(noFactors, noValues, removeFactorIndices);

  NonlinearFactorGraph actualGraph = smoother.getFactors();

  actualGraph.print("actual graph:  \n");

  NonlinearFactorGraph expectedGraph;
  expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
  // we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
  // we should add an empty one, so that the ordering and labeling of the factors is preserved
  expectedGraph.push_back(NonlinearFactor::shared_ptr());
  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);

//  CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
}

/////* ************************************************************************* */
//TEST( ConcurrentIncrementalSmoother, removeFactors_topology_2 )
//{
//  // we try removing the last factor
//
//  // Create a set of optimizer parameters
//  LevenbergMarquardtParams parameters;
//
//  // Create a Concurrent Batch Smoother
//  ConcurrentBatchSmoother smoother(parameters);
//
//  // Add some factors to the smoother
//  NonlinearFactorGraph newFactors;
//  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));
//  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));
//  newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
//  newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
//
//  // Update the smoother: add all factors
//  smoother.update(newFactors, newValues);
//
//  // factor we want to remove
//  // NOTE: we can remove factors, paying attention that the remaining graph remains connected
//  // we remove a single factor, the number 1, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
//  std::vector<size_t> removeFactorIndices(1,4);
//
//  // Add no factors to the smoother (we only want to test the removal)
//  NonlinearFactorGraph noFactors;
//  Values noValues;
//  smoother.update(noFactors, noValues, removeFactorIndices);
//
//  NonlinearFactorGraph actualGraph = smoother.getFactors();
//
//  NonlinearFactorGraph expectedGraph;
//  expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
//  expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
//  expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
//  expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
//  // we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
//  // we should add an empty one, so that the ordering and labeling of the factors is preserved
//  expectedGraph.push_back(NonlinearFactor::shared_ptr());
//
//  CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
//}
//
//
/////* ************************************************************************* */
//TEST( ConcurrentBatchSmoother, removeFactors_topology_3 )
//{
//  // we try removing the first factor
//
//  // Create a set of optimizer parameters
//  LevenbergMarquardtParams parameters;
//  ConcurrentBatchSmoother Smoother(parameters);
//
//  // Add some factors to the Smoother
//  NonlinearFactorGraph newFactors;
//  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
//  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));
//  newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
//  newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
//
//  // Update the Smoother: add all factors
//  Smoother.update(newFactors, newValues);
//
//  // factor we want to remove
//  // NOTE: we can remove factors, paying attention that the remaining graph remains connected
//  // we remove a single factor, the number 0, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
//  std::vector<size_t> removeFactorIndices(1,0);
//
//  // Add no factors to the Smoother (we only want to test the removal)
//  NonlinearFactorGraph noFactors;
//  Values noValues;
//  Smoother.update(noFactors, noValues, removeFactorIndices);
//
//  NonlinearFactorGraph actualGraph = Smoother.getFactors();
//
//  NonlinearFactorGraph expectedGraph;
//  // we should add an empty one, so that the ordering and labeling of the factors is preserved
//  expectedGraph.push_back(NonlinearFactor::shared_ptr());
//  expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
//  expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
//  expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
//  expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
//
//  CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
//}
//
/////* ************************************************************************* */
//TEST( ConcurrentBatchSmoother, removeFactors_values )
//{
//  // we try removing the last factor
//
//  // Create a set of optimizer parameters
//  LevenbergMarquardtParams parameters;
//  ConcurrentBatchSmoother Smoother(parameters);
//
//  // Add some factors to the Smoother
//  NonlinearFactorGraph newFactors;
//  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));
//  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));
//  newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
//  newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
//
//  // Update the Smoother: add all factors
//  Smoother.update(newFactors, newValues);
//
//  // factor we want to remove
//  // NOTE: we can remove factors, paying attention that the remaining graph remains connected
//  // we remove a single factor, the number 4, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
//  std::vector<size_t> removeFactorIndices(1,4);
//
//  // Add no factors to the Smoother (we only want to test the removal)
//  NonlinearFactorGraph noFactors;
//  Values noValues;
//  Smoother.update(noFactors, noValues, removeFactorIndices);
//
//  NonlinearFactorGraph actualGraph = Smoother.getFactors();
//  Values actualValues = Smoother.calculateEstimate();
//
//  // note: factors are removed before the optimization
//  NonlinearFactorGraph expectedGraph;
//  expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
//  expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
//  expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
//  expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
//  // we removed this one:   expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
//  // we should add an empty one, so that the ordering and labeling of the factors is preserved
//  expectedGraph.push_back(NonlinearFactor::shared_ptr());
//
//  // Calculate expected factor graph and values
//  Values expectedValues = BatchOptimize(expectedGraph, newValues);
//
//  CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
//  CHECK(assert_equal(expectedValues, actualValues, 1e-6));
//}




/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */