/* ************************************************************************* */
TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
Matrix A1 = (Matrix(2, 2) <<
2.74222, -0.0067457,
0.0, 2.63624);
Matrix A2 = (Matrix(2, 2) <<
-0.0455167, -0.0443573,
-0.0222154, -0.102489);
Vector b = (Vector(2) << 0.0277052,
-0.0533393);
JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2);
Values values;
values.insert(l1, landmark1);
values.insert(l2, landmark2);
values.insert(x1, poseA1);
values.insert(x2, poseA2);
LinearContainerFactor actFactor(expLinFactor, values);
LinearContainerFactor actFactorNolin(expLinFactor);
EXPECT(assert_equal(actFactor, actFactor, tol));
EXPECT(assert_inequal(actFactor, actFactorNolin, tol));
EXPECT(assert_inequal(actFactorNolin, actFactor, tol));
// Check contents
Values expLinPoint;
expLinPoint.insert(l1, landmark1);
expLinPoint.insert(l2, landmark2);
CHECK(actFactor.linearizationPoint());
EXPECT(actFactor.hasLinearizationPoint());
EXPECT(assert_equal(expLinPoint, *actFactor.linearizationPoint()));
// Check error evaluation
Vector delta_l1 = (Vector(2) << 1.0, 2.0);
Vector delta_l2 = (Vector(2) << 3.0, 4.0);
VectorValues delta = values.zeroVectors();
delta.at(l1) = delta_l1;
delta.at(l2) = delta_l2;
Values noisyValues = values.retract(delta);
double expError = expLinFactor.error(delta);
EXPECT_DOUBLES_EQUAL(expError, actFactor.error(noisyValues), tol);
EXPECT_DOUBLES_EQUAL(expLinFactor.error(values.zeroVectors()), actFactor.error(values), tol);
// Check linearization with corrections for updated linearization point
GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(noisyValues);
Vector bprime = b - A1 * delta_l1 - A2 * delta_l2;
JacobianFactor expLinFactor2(l1, A1, l2, A2, bprime, diag_model2);
EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol));
}
/* ************************************************************************* */
TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
// 2 variable example, one pose, one landmark (planar)
// Initial ordering: x1, l1
Matrix G11 = (Matrix(3, 3) <<
1.0, 2.0, 3.0,
0.0, 5.0, 6.0,
0.0, 0.0, 9.0);
Matrix G12 = (Matrix(3, 2) <<
1.0, 2.0,
3.0, 5.0,
4.0, 6.0);
Vector g1 = (Vector(3) << 1.0, 2.0, 3.0);
Matrix G22 = (Matrix(2, 2) <<
0.5, 0.2,
0.0, 0.6);
Vector g2 = (Vector(2) << -8.0, -9.0);
double f = 10.0;
// Construct full matrices
Matrix G(5,5);
G << G11, G12, Matrix::Zero(2,3), G22;
HessianFactor initFactor(x1, l1, G11, G12, g1, G22, g2, f);
Values linearizationPoint, expLinPoints;
linearizationPoint.insert(l1, landmark1);
linearizationPoint.insert(x1, poseA1);
expLinPoints = linearizationPoint;
linearizationPoint.insert(x2, poseA2);
LinearContainerFactor actFactor(initFactor, linearizationPoint);
EXPECT(!actFactor.isJacobian());
EXPECT(actFactor.isHessian());
EXPECT(actFactor.hasLinearizationPoint());
Values actLinPoint = *actFactor.linearizationPoint();
EXPECT(assert_equal(expLinPoints, actLinPoint));
// Create delta
Vector delta_l1 = (Vector(2) << 1.0, 2.0);
Vector delta_x1 = (Vector(3) << 3.0, 4.0, 0.5);
Vector delta_x2 = (Vector(3) << 6.0, 7.0, 0.3);
// Check error calculation
VectorValues delta = linearizationPoint.zeroVectors();
delta.at(l1) = delta_l1;
delta.at(x1) = delta_x1;
delta.at(x2) = delta_x2;
EXPECT(assert_equal((Vector(5) << 3.0, 4.0, 0.5, 1.0, 2.0), delta.vector(initFactor.keys())));
Values noisyValues = linearizationPoint.retract(delta);
double expError = initFactor.error(delta);
EXPECT_DOUBLES_EQUAL(expError, actFactor.error(noisyValues), tol);
EXPECT_DOUBLES_EQUAL(initFactor.error(linearizationPoint.zeroVectors()), actFactor.error(linearizationPoint), tol);
// Compute updated versions
Vector dv = (Vector(5) << 3.0, 4.0, 0.5, 1.0, 2.0);
Vector g(5); g << g1, g2;
Vector g_prime = g - G.selfadjointView<Eigen::Upper>() * dv;
// Check linearization with corrections for updated linearization point
Vector g1_prime = g_prime.head(3);
Vector g2_prime = g_prime.tail(2);
double f_prime = f + dv.transpose() * G.selfadjointView<Eigen::Upper>() * dv - 2.0 * dv.transpose() * g;
HessianFactor expNewFactor(x1, l1, G11, G12, g1_prime, G22, g2_prime, f_prime);
EXPECT(assert_equal(*expNewFactor.clone(), *actFactor.linearize(noisyValues), tol));
}
