/* ************************************************************************* */
TEST (Serialization, gaussian_factor_graph) {
GaussianFactorGraph graph;
{
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.);
Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4);
Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34);
Vector d(2); d << 0.2, 0.5;
GaussianConditional cg(0, d, R, 1, A1, 2, A2);
graph.push_back(cg);
}
{
Key i1 = 4, i2 = 7;
Matrix A1 = eye(3), A2 = -1.0 * eye(3);
Vector b = ones(3);
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1.0, 2.0, 3.0));
JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
HessianFactor hessianfactor(jacobianfactor);
graph.push_back(jacobianfactor);
graph.push_back(hessianfactor);
}
EXPECT(equalsObj(graph));
EXPECT(equalsXML(graph));
EXPECT(equalsBinary(graph));
}
/* ************************************************************************* */
GaussianFactorGraph GaussianFactorGraph::clone() const {
GaussianFactorGraph result;
BOOST_FOREACH(const sharedFactor& f, *this) {
if (f)
result.push_back(f->clone());
else
result.push_back(sharedFactor()); // Passes on null factors so indices remain valid
}
return result;
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, negate) {
GaussianFactorGraph init_graph = createGaussianFactorGraphWithHessianFactor();
init_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor
GaussianFactorGraph actNegation = init_graph.negate();
GaussianFactorGraph expNegation;
expNegation.push_back(init_graph.at(0)->negate());
expNegation.push_back(init_graph.at(1)->negate());
expNegation.push_back(init_graph.at(2)->negate());
expNegation.push_back(init_graph.at(3)->negate());
expNegation.push_back(init_graph.at(4)->negate());
expNegation.push_back(GaussianFactor::shared_ptr());
EXPECT(assert_equal(expNegation, actNegation));
}
/* ************************************************************************* */
KalmanFilter::State fuse(const KalmanFilter::State& p,
GaussianFactor* newFactor, bool useQR) {
// Create a factor graph
GaussianFactorGraph factorGraph;
// push back previous solution and new factor
factorGraph.push_back(p->toFactor());
factorGraph.push_back(GaussianFactor::shared_ptr(newFactor));
// Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
return solve(factorGraph, useQR);
}
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( ISAM, iSAM_smoother )
{
Ordering ordering;
for (int t = 1; t <= 7; t++) ordering += X(t);
// Create smoother with 7 nodes
GaussianFactorGraph smoother = createSmoother(7);
// run iSAM for every factor
GaussianISAM actual;
for(boost::shared_ptr<GaussianFactor> factor: smoother) {
GaussianFactorGraph factorGraph;
factorGraph.push_back(factor);
actual.update(factorGraph);
}
// Create expected Bayes Tree by solving smoother with "natural" ordering
GaussianBayesTree expected = *smoother.eliminateMultifrontal(ordering);
// Verify sigmas in the bayes tree
for(const GaussianBayesTree::sharedClique& clique: expected.nodes() | br::map_values) {
GaussianConditional::shared_ptr conditional = clique->conditional();
EXPECT(!conditional->get_model());
}
// Check whether BayesTree is correct
EXPECT(assert_equal(GaussianFactorGraph(expected).augmentedHessian(), GaussianFactorGraph(actual).augmentedHessian()));
// obtain solution
VectorValues e; // expected solution
for (int t = 1; t <= 7; t++) e.insert(X(t), Vector::Zero(2));
VectorValues optimized = actual.optimize(); // actual solution
EXPECT(assert_equal(e, optimized));
}
/* ************************************************************************* */
int main() {
gtsam::Key PoseKey1(11);
gtsam::Key PoseKey2(12);
gtsam::Key VelKey1(21);
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
double measurement_dt(0.1);
Vector world_g(Vector3(0.0, 0.0, 9.81));
Vector world_rho(Vector3(0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector3(0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
// Second test: zero angular motion, some acceleration - generated in matlab
Vector measurement_acc(Vector3(6.501390843381716, -6.763926150509185, -2.300389940090343));
Vector measurement_gyro(Vector3(0.1, 0.2, 0.3));
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
Rot3 R1(0.487316618, 0.125253866, 0.86419557,
0.580273724, 0.693095498, -0.427669306,
-0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1);
Vector3 Vel1 = Vector(Vector3(0.5,-0.5,0.4));
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388);
Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
Pose3 Pose2(R2, t2);
Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
Vector3 Vel2 = Vel1 + dv;
imuBias::ConstantBias Bias1;
Values values;
values.insert(PoseKey1, Pose1);
values.insert(PoseKey2, Pose2);
values.insert(VelKey1, Vel1);
values.insert(VelKey2, Vel2);
values.insert(BiasKey1, Bias1);
Ordering ordering;
ordering.push_back(PoseKey1);
ordering.push_back(VelKey1);
ordering.push_back(BiasKey1);
ordering.push_back(PoseKey2);
ordering.push_back(VelKey2);
GaussianFactorGraph graph;
gttic_(LinearizeTiming);
for(size_t i = 0; i < 100000; ++i) {
GaussianFactor::shared_ptr g = f.linearize(values);
graph.push_back(g);
}
gttoc_(LinearizeTiming);
tictoc_finishedIteration_();
tictoc_print_();
}
/* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) {
// Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph;
// 0.5*(x-x0)'*inv(Sigma)*(x-x0)
HessianFactor::shared_ptr factor(new HessianFactor(0, x, P0));
factorGraph.push_back(factor);
return solve(factorGraph, useQR());
}
/**
* 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(HessianFactor, eliminateUnsorted) {
JacobianFactor::shared_ptr factor1(
new JacobianFactor(0,
Matrix_(3,3,
44.7214, 0.0, 0.0,
0.0, 44.7214, 0.0,
0.0, 0.0, 44.7214),
1,
Matrix_(3,3,
-0.179168, -44.721, 0.717294,
44.721, -0.179168, -44.9138,
0.0, 0.0, -44.7214),
Vector_(3, 1.98916e-17, -4.96503e-15, -7.75792e-17),
noiseModel::Unit::Create(3)));
HessianFactor::shared_ptr unsorted_factor2(
new HessianFactor(JacobianFactor(0,
Matrix_(6,3,
25.8367, 0.1211, 0.0593,
0.0, 23.4099, 30.8733,
0.0, 0.0, 25.8729,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0),
1,
Matrix_(6,3,
25.7429, -1.6897, 0.4587,
1.6400, 23.3095, -8.4816,
0.0034, 0.0509, -25.7855,
0.9997, -0.0002, 0.0824,
0.0, 0.9973, 0.9517,
0.0, 0.0, 0.9973),
Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
noiseModel::Unit::Create(6))));
Permutation permutation(2);
permutation[0] = 1;
permutation[1] = 0;
unsorted_factor2->permuteWithInverse(permutation);
HessianFactor::shared_ptr sorted_factor2(
new HessianFactor(JacobianFactor(0,
Matrix_(6,3,
25.7429, -1.6897, 0.4587,
1.6400, 23.3095, -8.4816,
0.0034, 0.0509, -25.7855,
0.9997, -0.0002, 0.0824,
0.0, 0.9973, 0.9517,
0.0, 0.0, 0.9973),
1,
Matrix_(6,3,
25.8367, 0.1211, 0.0593,
0.0, 23.4099, 30.8733,
0.0, 0.0, 25.8729,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0),
Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
noiseModel::Unit::Create(6))));
GaussianFactorGraph sortedGraph;
// sortedGraph.push_back(factor1);
sortedGraph.push_back(sorted_factor2);
GaussianFactorGraph unsortedGraph;
// unsortedGraph.push_back(factor1);
unsortedGraph.push_back(unsorted_factor2);
GaussianConditional::shared_ptr expected_bn;
GaussianFactor::shared_ptr expected_factor;
boost::tie(expected_bn, expected_factor) =
EliminatePreferCholesky(sortedGraph, 1);
GaussianConditional::shared_ptr actual_bn;
GaussianFactor::shared_ptr actual_factor;
boost::tie(actual_bn, actual_factor) =
EliminatePreferCholesky(unsortedGraph, 1);
EXPECT(assert_equal(*expected_bn, *actual_bn, 1e-10));
EXPECT(assert_equal(*expected_factor, *actual_factor, 1e-10));
}
