コード例 #1
0
/* ************************************************************************* */
TEST(HessianFactor, combine) {

  // update the information matrix with a single jacobian factor
  Matrix A0 = Matrix_(2, 2,
  11.1803399,     0.0,
      0.0, 11.1803399);
  Matrix A1 = Matrix_(2, 2,
  -2.23606798,        0.0,
         0.0, -2.23606798);
  Matrix A2 = Matrix_(2, 2,
  -8.94427191,      0.0,
         0.0, -8.94427191);
  Vector b = Vector_(2, 2.23606798,-1.56524758);
  SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2));
  GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model));
  FactorGraph<GaussianFactor> factors;
  factors.push_back(f);

  // Form Ab' * Ab
  HessianFactor actual(factors);

  Matrix expected = Matrix_(7, 7,
  125.0000,       0.0,  -25.0000,       0.0, -100.0000,       0.0,   25.0000,
       0.0,  125.0000,       0.0,  -25.0000,       0.0, -100.0000,  -17.5000,
  -25.0000,       0.0,    5.0000,       0.0,   20.0000,       0.0,   -5.0000,
       0.0,  -25.0000,       0.0,    5.0000,       0.0,   20.0000,    3.5000,
 -100.0000,       0.0,   20.0000,       0.0,   80.0000,       0.0,  -20.0000,
       0.0, -100.0000,       0.0,   20.0000,       0.0,   80.0000,   14.0000,
   25.0000,  -17.5000,   -5.0000,    3.5000,  -20.0000,   14.0000,    7.4500);
  EXPECT(assert_equal(Matrix(expected.triangularView<Eigen::Upper>()),
      Matrix(actual.matrix_.triangularView<Eigen::Upper>()), tol));

}
コード例 #2
0
/* ************************************************************************* */
TEST_UNSAFE(HessianFactor, CombineAndEliminate)
{
  Matrix A01 = Matrix_(3,3,
      1.0, 0.0, 0.0,
      0.0, 1.0, 0.0,
      0.0, 0.0, 1.0);
  Vector b0 = Vector_(3, 1.5, 1.5, 1.5);
  Vector s0 = Vector_(3, 1.6, 1.6, 1.6);

  Matrix A10 = Matrix_(3,3,
      2.0, 0.0, 0.0,
      0.0, 2.0, 0.0,
      0.0, 0.0, 2.0);
  Matrix A11 = Matrix_(3,3,
      -2.0, 0.0, 0.0,
      0.0, -2.0, 0.0,
      0.0, 0.0, -2.0);
  Vector b1 = Vector_(3, 2.5, 2.5, 2.5);
  Vector s1 = Vector_(3, 2.6, 2.6, 2.6);

  Matrix A21 = Matrix_(3,3,
      3.0, 0.0, 0.0,
      0.0, 3.0, 0.0,
      0.0, 0.0, 3.0);
  Vector b2 = Vector_(3, 3.5, 3.5, 3.5);
  Vector s2 = Vector_(3, 3.6, 3.6, 3.6);

  GaussianFactorGraph gfg;
  gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
  gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
  gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));

  Matrix zero3x3 = zeros(3,3);
  Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
  Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
  Vector b = gtsam::concatVectors(3, &b1, &b0, &b2);
  Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);

  // create a full, uneliminated version of the factor
  JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));

  // perform elimination
  GaussianConditional::shared_ptr expectedBN = expectedFactor.eliminate(1);

  // create expected Hessian after elimination
  HessianFactor expectedCholeskyFactor(expectedFactor);

  // Convert all factors to hessians
  FactorGraph<HessianFactor> hessians;
  BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) {
    if(boost::shared_ptr<HessianFactor> hf = boost::dynamic_pointer_cast<HessianFactor>(factor))
      hessians.push_back(hf);
    else if(boost::shared_ptr<JacobianFactor> jf = boost::dynamic_pointer_cast<JacobianFactor>(factor))
      hessians.push_back(boost::make_shared<HessianFactor>(*jf));
    else
      CHECK(false);
  }

  // Eliminate
  GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(gfg, 1);
  HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<HessianFactor>(actualCholesky.second);

  EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6));
  EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6));
}