Renamed unit tests already converted to 'Obsolete'
parent
323d618c3e
commit
b857dab6a9
|
@ -37,18 +37,18 @@ static SharedDiagonal
|
|||
constraintModel = noiseModel::Constrained::All(2);
|
||||
|
||||
static GaussianFactorGraph createSimpleGaussianFactorGraph() {
|
||||
GaussianFactorGraph fg;
|
||||
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
|
||||
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
|
||||
fg.add(2, 10*eye(2), -1.0*ones(2), unit2);
|
||||
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
|
||||
fg.add(2, -10*eye(2), 0, 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
|
||||
// measurement between x1 and l1: l1-x1=[0.0;0.2]
|
||||
fg.add(2, -5*eye(2), 1, 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
|
||||
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
|
||||
fg.add(0, -5*eye(2), 1, 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
|
||||
return fg;
|
||||
}
|
||||
GaussianFactorGraph fg;
|
||||
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
|
||||
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
|
||||
fg.add(2, 10*eye(2), -1.0*ones(2), unit2);
|
||||
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
|
||||
fg.add(2, -10*eye(2), 0, 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
|
||||
// measurement between x1 and l1: l1-x1=[0.0;0.2]
|
||||
fg.add(2, -5*eye(2), 1, 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
|
||||
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
|
||||
fg.add(0, -5*eye(2), 1, 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
|
||||
return fg;
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
@ -464,73 +464,73 @@ TEST(GaussianFactorGraph, matrices) {
|
|||
EXPECT(assert_equal(expectedL, actualL));
|
||||
EXPECT(assert_equal(expectedeta, actualeta));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, gradient )
|
||||
{
|
||||
GaussianFactorGraph fg = createSimpleGaussianFactorGraph();
|
||||
|
||||
// Construct expected gradient
|
||||
VectorValues expected;
|
||||
|
||||
// 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2
|
||||
// worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
|
||||
expected.insert(1,Vector_(2, 5.0,-12.5));
|
||||
expected.insert(2,Vector_(2, 30.0, 5.0));
|
||||
expected.insert(0,Vector_(2,-25.0, 17.5));
|
||||
|
||||
// Check the gradient at delta=0
|
||||
VectorValues zero = VectorValues::Zero(expected);
|
||||
VectorValues actual = gradient(fg, zero);
|
||||
EXPECT(assert_equal(expected,actual));
|
||||
|
||||
// Check the gradient at the solution (should be zero)
|
||||
VectorValues solution = *GaussianSequentialSolver(fg).optimize();
|
||||
VectorValues actual2 = gradient(fg, solution);
|
||||
EXPECT(assert_equal(VectorValues::Zero(solution), actual2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, transposeMultiplication )
|
||||
{
|
||||
GaussianFactorGraph A = createSimpleGaussianFactorGraph();
|
||||
|
||||
VectorValues e;
|
||||
e.insert(0, Vector_(2, 0.0, 0.0));
|
||||
e.insert(1, Vector_(2,15.0, 0.0));
|
||||
e.insert(2, Vector_(2, 0.0,-5.0));
|
||||
e.insert(3, Vector_(2,-7.5,-5.0));
|
||||
|
||||
VectorValues expected;
|
||||
expected.insert(1, Vector_(2, -37.5,-50.0));
|
||||
expected.insert(2, Vector_(2,-150.0, 25.0));
|
||||
expected.insert(0, Vector_(2, 187.5, 25.0));
|
||||
|
||||
VectorValues actual = VectorValues::SameStructure(expected);
|
||||
transposeMultiply(A, e, actual);
|
||||
EXPECT(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianFactorGraph, eliminate_empty )
|
||||
{
|
||||
// eliminate an empty factor
|
||||
GaussianFactorGraph gfg;
|
||||
gfg.push_back(boost::make_shared<JacobianFactor>());
|
||||
GaussianConditional::shared_ptr actualCG;
|
||||
GaussianFactorGraph remainingGFG;
|
||||
boost::tie(actualCG, remainingGFG) = gfg.eliminateOne(0);
|
||||
|
||||
// expected Conditional Gaussian is just a parent-less node with P(x)=1
|
||||
GaussianConditional expectedCG(0, Vector(), Matrix(), Vector());
|
||||
|
||||
// expected remaining graph should be the same as the original, still empty :-)
|
||||
GaussianFactorGraph expectedLF = gfg;
|
||||
|
||||
// check if the result matches
|
||||
EXPECT(actualCG->equals(expectedCG));
|
||||
EXPECT(remainingGFG.equals(expectedLF));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, gradient )
|
||||
{
|
||||
GaussianFactorGraph fg = createSimpleGaussianFactorGraph();
|
||||
|
||||
// Construct expected gradient
|
||||
VectorValues expected;
|
||||
|
||||
// 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2
|
||||
// worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
|
||||
expected.insert(1,Vector_(2, 5.0,-12.5));
|
||||
expected.insert(2,Vector_(2, 30.0, 5.0));
|
||||
expected.insert(0,Vector_(2,-25.0, 17.5));
|
||||
|
||||
// Check the gradient at delta=0
|
||||
VectorValues zero = VectorValues::Zero(expected);
|
||||
VectorValues actual = gradient(fg, zero);
|
||||
EXPECT(assert_equal(expected,actual));
|
||||
|
||||
// Check the gradient at the solution (should be zero)
|
||||
VectorValues solution = *GaussianSequentialSolver(fg).optimize();
|
||||
VectorValues actual2 = gradient(fg, solution);
|
||||
EXPECT(assert_equal(VectorValues::Zero(solution), actual2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, transposeMultiplication )
|
||||
{
|
||||
GaussianFactorGraph A = createSimpleGaussianFactorGraph();
|
||||
|
||||
VectorValues e;
|
||||
e.insert(0, Vector_(2, 0.0, 0.0));
|
||||
e.insert(1, Vector_(2,15.0, 0.0));
|
||||
e.insert(2, Vector_(2, 0.0,-5.0));
|
||||
e.insert(3, Vector_(2,-7.5,-5.0));
|
||||
|
||||
VectorValues expected;
|
||||
expected.insert(1, Vector_(2, -37.5,-50.0));
|
||||
expected.insert(2, Vector_(2,-150.0, 25.0));
|
||||
expected.insert(0, Vector_(2, 187.5, 25.0));
|
||||
|
||||
VectorValues actual = VectorValues::SameStructure(expected);
|
||||
transposeMultiply(A, e, actual);
|
||||
EXPECT(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianFactorGraph, eliminate_empty )
|
||||
{
|
||||
// eliminate an empty factor
|
||||
GaussianFactorGraph gfg;
|
||||
gfg.push_back(boost::make_shared<JacobianFactor>());
|
||||
GaussianConditional::shared_ptr actualCG;
|
||||
GaussianFactorGraph remainingGFG;
|
||||
boost::tie(actualCG, remainingGFG) = gfg.eliminateOne(0);
|
||||
|
||||
// expected Conditional Gaussian is just a parent-less node with P(x)=1
|
||||
GaussianConditional expectedCG(0, Vector(), Matrix(), Vector());
|
||||
|
||||
// expected remaining graph should be the same as the original, still empty :-)
|
||||
GaussianFactorGraph expectedLF = gfg;
|
||||
|
||||
// check if the result matches
|
||||
EXPECT(actualCG->equals(expectedCG));
|
||||
EXPECT(remainingGFG.equals(expectedLF));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
Loading…
Reference in New Issue