Reverted to LieScalar until Prior and Between factors fixed
dellaert
parent
9b0298d148
commit
d06de2f044
|
|
@ -6,23 +6,24 @@
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/treeTraversal-inst.h>
|
||||
#include <tests/smallExample.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/BearingRangeFactor.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/BearingRangeFactor.h>
|
||||
#include <tests/smallExample.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/treeTraversal-inst.h>
|
||||
#include <gtsam/base/LieScalar.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/list_of.hpp>
|
||||
|
|
@ -34,6 +35,9 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
using boost::shared_ptr;
|
||||
|
||||
static const SharedNoiseModel model;
|
||||
static const LieScalar Zero(0);
|
||||
|
||||
// SETDEBUG("ISAM2 update", true);
|
||||
// SETDEBUG("ISAM2 update verbose", true);
|
||||
// SETDEBUG("ISAM2 recalculate", true);
|
||||
|
|
@ -202,11 +206,11 @@ struct ConsistencyVisitor
|
|||
consistent(true), isam(isam) {}
|
||||
int operator()(const ISAM2::sharedClique& node, int& parentData)
|
||||
{
|
||||
if(std::find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end())
|
||||
if(find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end())
|
||||
{
|
||||
if(node->parent_.expired())
|
||||
consistent = false;
|
||||
if(std::find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end())
|
||||
if(find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end())
|
||||
consistent = false;
|
||||
}
|
||||
BOOST_FOREACH(Key j, node->conditional()->frontals())
|
||||
|
|
@ -222,7 +226,7 @@ struct ConsistencyVisitor
|
|||
bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) {
|
||||
|
||||
TestResult& result_ = result;
|
||||
const std::string name_ = test.getName();
|
||||
const string name_ = test.getName();
|
||||
|
||||
Values actual = isam.calculateEstimate();
|
||||
Values expected = fullinit.retract(fullgraph.linearize(fullinit)->optimize());
|
||||
|
|
@ -284,19 +288,19 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
|
|||
TEST(ISAM2, AddFactorsStep1)
|
||||
{
|
||||
NonlinearFactorGraph nonlinearFactors;
|
||||
nonlinearFactors += PriorFactor<double>(10, 0.0, gtsam::SharedNoiseModel());
|
||||
nonlinearFactors += PriorFactor<LieScalar>(10, Zero, model);
|
||||
nonlinearFactors += NonlinearFactor::shared_ptr();
|
||||
nonlinearFactors += PriorFactor<double>(11, 0.0, gtsam::SharedNoiseModel());
|
||||
nonlinearFactors += PriorFactor<LieScalar>(11, Zero, model);
|
||||
|
||||
NonlinearFactorGraph newFactors;
|
||||
newFactors += PriorFactor<double>(1, 0.0, gtsam::SharedNoiseModel());
|
||||
newFactors += PriorFactor<double>(2, 0.0, gtsam::SharedNoiseModel());
|
||||
newFactors += PriorFactor<LieScalar>(1, Zero, model);
|
||||
newFactors += PriorFactor<LieScalar>(2, Zero, model);
|
||||
|
||||
NonlinearFactorGraph expectedNonlinearFactors;
|
||||
expectedNonlinearFactors += PriorFactor<double>(10, 0.0, gtsam::SharedNoiseModel());
|
||||
expectedNonlinearFactors += PriorFactor<double>(1, 0.0, gtsam::SharedNoiseModel());
|
||||
expectedNonlinearFactors += PriorFactor<double>(11, 0.0, gtsam::SharedNoiseModel());
|
||||
expectedNonlinearFactors += PriorFactor<double>(2, 0.0, gtsam::SharedNoiseModel());
|
||||
expectedNonlinearFactors += PriorFactor<LieScalar>(10, Zero, model);
|
||||
expectedNonlinearFactors += PriorFactor<LieScalar>(1, Zero, model);
|
||||
expectedNonlinearFactors += PriorFactor<LieScalar>(11, Zero, model);
|
||||
expectedNonlinearFactors += PriorFactor<LieScalar>(2, Zero, model);
|
||||
|
||||
const FastVector<size_t> expectedNewFactorIndices = list_of(1)(3);
|
||||
|
||||
|
|
@ -693,18 +697,17 @@ namespace {
|
|||
TEST(ISAM2, marginalizeLeaves1)
|
||||
{
|
||||
ISAM2 isam;
|
||||
|
||||
NonlinearFactorGraph factors;
|
||||
factors += PriorFactor<double>(0, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += PriorFactor<LieScalar>(0, Zero, model);
|
||||
|
||||
factors += BetweenFactor<double>(0, 1, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<double>(1, 2, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<double>(0, 2, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<LieScalar>(0, 1, Zero, model);
|
||||
factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
|
||||
factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
|
||||
|
||||
Values values;
|
||||
values.insert(0, 0.0);
|
||||
values.insert(1, 0.0);
|
||||
values.insert(2, 0.0);
|
||||
values.insert(0, Zero);
|
||||
values.insert(1, Zero);
|
||||
values.insert(2, Zero);
|
||||
|
||||
FastMap<Key,int> constrainedKeys;
|
||||
constrainedKeys.insert(make_pair(0,0));
|
||||
|
|
@ -723,18 +726,18 @@ TEST(ISAM2, marginalizeLeaves2)
|
|||
ISAM2 isam;
|
||||
|
||||
NonlinearFactorGraph factors;
|
||||
factors += PriorFactor<double>(0, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += PriorFactor<LieScalar>(0, Zero, model);
|
||||
|
||||
factors += BetweenFactor<double>(0, 1, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<double>(1, 2, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<double>(0, 2, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<double>(2, 3, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<LieScalar>(0, 1, Zero, model);
|
||||
factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
|
||||
factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
|
||||
factors += BetweenFactor<LieScalar>(2, 3, Zero, model);
|
||||
|
||||
Values values;
|
||||
values.insert(0, 0.0);
|
||||
values.insert(1, 0.0);
|
||||
values.insert(2, 0.0);
|
||||
values.insert(3, 0.0);
|
||||
values.insert(0, Zero);
|
||||
values.insert(1, Zero);
|
||||
values.insert(2, Zero);
|
||||
values.insert(3, Zero);
|
||||
|
||||
FastMap<Key,int> constrainedKeys;
|
||||
constrainedKeys.insert(make_pair(0,0));
|
||||
|
|
@ -754,25 +757,25 @@ TEST(ISAM2, marginalizeLeaves3)
|
|||
ISAM2 isam;
|
||||
|
||||
NonlinearFactorGraph factors;
|
||||
factors += PriorFactor<double>(0, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += PriorFactor<LieScalar>(0, Zero, model);
|
||||
|
||||
factors += BetweenFactor<double>(0, 1, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<double>(1, 2, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<double>(0, 2, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<LieScalar>(0, 1, Zero, model);
|
||||
factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
|
||||
factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
|
||||
|
||||
factors += BetweenFactor<double>(2, 3, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<LieScalar>(2, 3, Zero, model);
|
||||
|
||||
factors += BetweenFactor<double>(3, 4, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<double>(4, 5, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<double>(3, 5, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<LieScalar>(3, 4, Zero, model);
|
||||
factors += BetweenFactor<LieScalar>(4, 5, Zero, model);
|
||||
factors += BetweenFactor<LieScalar>(3, 5, Zero, model);
|
||||
|
||||
Values values;
|
||||
values.insert(0, 0.0);
|
||||
values.insert(1, 0.0);
|
||||
values.insert(2, 0.0);
|
||||
values.insert(3, 0.0);
|
||||
values.insert(4, 0.0);
|
||||
values.insert(5, 0.0);
|
||||
values.insert(0, Zero);
|
||||
values.insert(1, Zero);
|
||||
values.insert(2, Zero);
|
||||
values.insert(3, Zero);
|
||||
values.insert(4, Zero);
|
||||
values.insert(5, Zero);
|
||||
|
||||
FastMap<Key,int> constrainedKeys;
|
||||
constrainedKeys.insert(make_pair(0,0));
|
||||
|
|
@ -794,14 +797,14 @@ TEST(ISAM2, marginalizeLeaves4)
|
|||
ISAM2 isam;
|
||||
|
||||
NonlinearFactorGraph factors;
|
||||
factors += PriorFactor<double>(0, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<double>(0, 2, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += BetweenFactor<double>(1, 2, 0.0, noiseModel::Unit::Create(1));
|
||||
factors += PriorFactor<LieScalar>(0, Zero, model);
|
||||
factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
|
||||
factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
|
||||
|
||||
Values values;
|
||||
values.insert(0, 0.0);
|
||||
values.insert(1, 0.0);
|
||||
values.insert(2, 0.0);
|
||||
values.insert(0, Zero);
|
||||
values.insert(1, Zero);
|
||||
values.insert(2, Zero);
|
||||
|
||||
FastMap<Key,int> constrainedKeys;
|
||||
constrainedKeys.insert(make_pair(0,0));
|
||||
|
|
|
|||
Loading…
Reference in New Issue