C++ GaussianFactorGraph::add方法代码示例

/* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate2) {
  Matrix A01 = I_3x3;
  Vector3 b0(1.5, 1.5, 1.5);
  Vector3 s0(1.6, 1.6, 1.6);

  Matrix A10 = 2.0 * I_3x3;
  Matrix A11 = -2.0 * I_3x3;
  Vector3 b1(2.5, 2.5, 2.5);
  Vector3 s1(2.6, 2.6, 2.6);

  Matrix A21 = 3.0 * I_3x3;
  Vector3 b2(3.5, 3.5, 3.5);
  Vector3 s2(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));

  Matrix93 A0, A1;
  A0 << A10, Z_3x3, Z_3x3;
  A1 << A11, A01, A21;
  Vector9 b, sigmas;
  b << b1, b0, b2;
  sigmas << s1, s0, s2;

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

  // Make sure combining works
  HessianFactor hessian(gfg);
  EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6));
  EXPECT(
      assert_equal(jacobian.augmentedInformation(),
          hessian.augmentedInformation(), 1e-9));

  // perform elimination on jacobian
  Ordering ordering = list_of(0);
  GaussianConditional::shared_ptr expectedConditional;
  JacobianFactor::shared_ptr expectedFactor;
  boost::tie(expectedConditional, expectedFactor) = //
      jacobian.eliminate(ordering);

  // Eliminate
  GaussianConditional::shared_ptr actualConditional;
  HessianFactor::shared_ptr actualHessian;
  boost::tie(actualConditional, actualHessian) = //
      EliminateCholesky(gfg, ordering);

  EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
  VectorValues v;
  v.insert(1, Vector3(1, 2, 3));
  EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
  EXPECT(
      assert_equal(expectedFactor->augmentedInformation(),
          actualHessian->augmentedInformation(), 1e-9));
  EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
}

/* ************************************************************************* */
TEST(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 on jacobian
  GaussianConditional::shared_ptr expectedConditional;
  JacobianFactor::shared_ptr expectedRemainingFactor;
  boost::tie(expectedConditional, expectedRemainingFactor) = expectedFactor.eliminate(Ordering(list_of(0)));

  // Eliminate
  GaussianConditional::shared_ptr actualConditional;
  HessianFactor::shared_ptr actualCholeskyFactor;
  boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0)));

  EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
  EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, matrices) {
  // Create factor graph:
  // x1 x2 x3 x4 x5  b
  //  1  2  3  0  0  4
  //  5  6  7  0  0  8
  //  9 10  0 11 12 13
  //  0  0  0 14 15 16

  Matrix A00 = (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished();
  Matrix A10 = (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished();
  Matrix A11 = (Matrix(2, 2) << 11, 12, 14, 15).finished();

  GaussianFactorGraph gfg;
  SharedDiagonal model = noiseModel::Unit::Create(2);
  gfg.add(0, A00, Vector2(4., 8.), model);
  gfg.add(0, A10, 1, A11, Vector2(13., 16.), model);

  Matrix Ab(4, 6);
  Ab << 1, 2, 3, 0, 0, 4, 5, 6, 7, 0, 0, 8, 9, 10, 0, 11, 12, 13, 0, 0, 0, 14, 15, 16;

  // augmented versions
  EXPECT(assert_equal(Ab, gfg.augmentedJacobian()));
  EXPECT(assert_equal(Ab.transpose() * Ab, gfg.augmentedHessian()));

  // jacobian
  Matrix A = Ab.leftCols(Ab.cols() - 1);
  Vector b = Ab.col(Ab.cols() - 1);
  Matrix actualA;
  Vector actualb;
  boost::tie(actualA, actualb) = gfg.jacobian();
  EXPECT(assert_equal(A, actualA));
  EXPECT(assert_equal(b, actualb));

  // hessian
  Matrix L = A.transpose() * A;
  Vector eta = A.transpose() * b;
  Matrix actualL;
  Vector actualeta;
  boost::tie(actualL, actualeta) = gfg.hessian();
  EXPECT(assert_equal(L, actualL));
  EXPECT(assert_equal(eta, actualeta));

  // hessianBlockDiagonal
  VectorValues expectLdiagonal;  // Make explicit that diagonal is sum-squares of columns
  expectLdiagonal.insert(0, Vector3(1 + 25 + 81, 4 + 36 + 100, 9 + 49));
  expectLdiagonal.insert(1, Vector2(121 + 196, 144 + 225));
  EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal()));

  // hessianBlockDiagonal
  map<Key, Matrix> actualBD = gfg.hessianBlockDiagonal();
  LONGS_EQUAL(2, actualBD.size());
  EXPECT(assert_equal(A00.transpose() * A00 + A10.transpose() * A10, actualBD[0]));
  EXPECT(assert_equal(A11.transpose() * A11, actualBD[1]));
}

	/* ************************************************************************* */
	KalmanFilter::State KalmanFilter::init(const Vector& x0,
			const SharedDiagonal& P0) {

		// Create a factor graph f(x0), eliminate it into P(x0)
		GaussianFactorGraph factorGraph;
		factorGraph.add(0, I_, x0, P0); // |x-x0|^2_diagSigma
		return solve(factorGraph, useQR());
	}

/* ************************************************************************* */
TEST(GaussianFactorGraph, sparseJacobian) {
  // Create factor graph:
  // x1 x2 x3 x4 x5  b
  //  1  2  3  0  0  4
  //  5  6  7  0  0  8
  //  9 10  0 11 12 13
  //  0  0  0 14 15 16

  // Expected - NOTE that we transpose this!
  Matrix expectedT = (Matrix(16, 3) <<
      1., 1., 2.,
      1., 2., 4.,
      1., 3., 6.,
      2., 1.,10.,
      2., 2.,12.,
      2., 3.,14.,
      1., 6., 8.,
      2., 6.,16.,
      3., 1.,18.,
      3., 2.,20.,
      3., 4.,22.,
      3., 5.,24.,
      4., 4.,28.,
      4., 5.,30.,
      3., 6.,26.,
      4., 6.,32.).finished();

  Matrix expected = expectedT.transpose();

  GaussianFactorGraph gfg;
  SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
  gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.).finished(), Vector2(4., 8.), model);
  gfg.add(0, (Matrix(2, 3) << 9., 10., 0., 0., 0., 0.).finished(), 1,
          (Matrix(2, 2) << 11., 12., 14., 15.).finished(), Vector2(13., 16.), model);

  Matrix actual = gfg.sparseJacobian_();

  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(GaussianBayesTree, shortcut_overlapping_separator)
{
  // Test computing shortcuts when the separator overlaps.  This previously
  // would have highlighted a problem where information was duplicated.

  // Create factor graph:
  // f(1,2,5)
  // f(3,4,5)
  // f(5,6)
  // f(6,7)
  GaussianFactorGraph fg;
  noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
  fg.add(1, (Matrix(1, 1) <<  1.0).finished(), 3, (Matrix(1, 1) <<  2.0).finished(), 5, (Matrix(1, 1) <<  3.0).finished(), (Vector(1) << 4.0).finished(), model);
  fg.add(1, (Matrix(1, 1) <<  5.0).finished(), (Vector(1) << 6.0).finished(), model);
  fg.add(2, (Matrix(1, 1) <<  7.0).finished(), 4, (Matrix(1, 1) <<  8.0).finished(), 5, (Matrix(1, 1) <<  9.0).finished(), (Vector(1) << 10.0).finished(), model);
  fg.add(2, (Matrix(1, 1) <<  11.0).finished(), (Vector(1) << 12.0).finished(), model);
  fg.add(5, (Matrix(1, 1) <<  13.0).finished(), 6, (Matrix(1, 1) <<  14.0).finished(), (Vector(1) << 15.0).finished(), model);
  fg.add(6, (Matrix(1, 1) <<  17.0).finished(), 7, (Matrix(1, 1) <<  18.0).finished(), (Vector(1) << 19.0).finished(), model);
  fg.add(7, (Matrix(1, 1) <<  20.0).finished(), (Vector(1) << 21.0).finished(), model);

  // Eliminate into BayesTree
  // c(6,7)
  // c(5|6)
  //   c(1,2|5)
  //   c(3,4|5)
  Ordering ordering(fg.keys());
  GaussianBayesTree bt = *fg.eliminateMultifrontal(ordering); // eliminate in increasing key order, fg.keys() is sorted.

  GaussianFactorGraph joint = *bt.joint(1,2, EliminateQR);

  Matrix expectedJointJ = (Matrix(2,3) <<
    5, 0, 6,
    0, -11, -12
    ).finished();
  Matrix actualJointJ = joint.augmentedJacobian();

  EXPECT(assert_equal(expectedJointJ, actualJointJ));
}

/* ************************************************************************* */
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));
}