// Create a planar factor graph and optimize
double timePlanarSmoother(int N, bool old = true) {
GaussianFactorGraph fg = planarGraph(N).get<0>();
clock_t start = clock();
fg.optimize();
clock_t end = clock ();
double dif = (double)(end - start) / CLOCKS_PER_SEC;
return dif;
}
// Create a Kalman smoother for t=1:T and optimize
double timeKalmanSmoother(int T) {
GaussianFactorGraph smoother = createSmoother(T);
clock_t start = clock();
// Keys will come out sorted since keys() returns a set
smoother.optimize(Ordering(smoother.keys()));
clock_t end = clock ();
double dif = (double)(end - start) / CLOCKS_PER_SEC;
return dif;
}
/* ************************************************************************* */
bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const IncrementalFixedLagSmoother& smoother, const Key& key) {
GaussianFactorGraph linearized = *fullgraph.linearize(fullinit);
VectorValues delta = linearized.optimize();
Values fullfinal = fullinit.retract(delta);
Point2 expected = fullfinal.at<Point2>(key);
Point2 actual = smoother.calculateEstimate<Point2>(key);
return assert_equal(expected, actual);
}
TEST(LPSolver, overConstrainedLinearSystem2) {
GaussianFactorGraph graph;
graph.push_back(JacobianFactor(1, I_1x1, 2, I_1x1, kOne,
noiseModel::Constrained::All(1)));
graph.push_back(JacobianFactor(1, I_1x1, 2, -I_1x1, 5 * kOne,
noiseModel::Constrained::All(1)));
graph.push_back(JacobianFactor(1, I_1x1, 2, 2 * I_1x1, 6 * kOne,
noiseModel::Constrained::All(1)));
VectorValues x = graph.optimize();
// This check confirms that gtsam linear constraint solver can't handle
// over-constrained system
CHECK(graph.error(x) != 0.0);
}
/**
* TEST gtsam solver with an over-constrained system
* x + y = 1
* x - y = 5
* x + 2y = 6
*/
TEST(LPSolver, overConstrainedLinearSystem) {
GaussianFactorGraph graph;
Matrix A1 = Vector3(1, 1, 1);
Matrix A2 = Vector3(1, -1, 2);
Vector b = Vector3(1, 5, 6);
JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3));
graph.push_back(factor);
VectorValues x = graph.optimize();
// This check confirms that gtsam linear constraint solver can't handle
// over-constrained system
CHECK(factor.error(x) != 0.0);
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, gradient) {
GaussianFactorGraph fg = createSimpleGaussianFactorGraph();
// Construct expected gradient
// 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]
VectorValues expected = map_list_of<Key, Vector>(1, Vector2(5.0, -12.5))(2, Vector2(30.0, 5.0))(
0, Vector2(-25.0, 17.5));
// Check the gradient at delta=0
VectorValues zero = VectorValues::Zero(expected);
VectorValues actual = fg.gradient(zero);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(expected, fg.gradientAtZero()));
// Check the gradient at the solution (should be zero)
VectorValues solution = fg.optimize();
VectorValues actual2 = fg.gradient(solution);
EXPECT(assert_equal(VectorValues::Zero(solution), actual2));
}
TEST(GaussianFactorGraph, DenseSolve) {
GaussianFactorGraph fg = createSimpleGaussianFactorGraph();
VectorValues expected = fg.optimize();
VectorValues actual = fg.optimizeDensely();
EXPECT(assert_equal(expected, actual));
}
