Ejemplo n.º 1
0
/* ************************************************************************* */
TEST(GaussianFactorGraph, multiplyHessianAdd2) {
  GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();

  // brute force
  Matrix AtA;
  Vector eta;
  boost::tie(AtA, eta) = gfg.hessian();
  Vector X(6);
  X << 1, 2, 3, 4, 5, 6;
  Vector Y(6);
  Y << -450, -450, 300, 400, 2950, 3450;
  EXPECT(assert_equal(Y, AtA * X));

  VectorValues x = map_list_of<Key, Vector>(0, Vector2(1, 2))(1, Vector2(3, 4))(2, Vector2(5, 6));

  VectorValues expected;
  expected.insert(0, Vector2(-450, -450));
  expected.insert(1, Vector2(300, 400));
  expected.insert(2, Vector2(2950, 3450));

  VectorValues actual;
  gfg.multiplyHessianAdd(1.0, x, actual);
  EXPECT(assert_equal(expected, actual));

  // now, do it with non-zero y
  gfg.multiplyHessianAdd(1.0, x, actual);
  EXPECT(assert_equal(2 * expected, actual));
}
Ejemplo n.º 2
0
TEST(LPInitSolver, infinite_loop_multi_var) {
  LP initchecker;
  Key X = symbol('X', 1);
  Key Y = symbol('Y', 1);
  Key Z = symbol('Z', 1);
  initchecker.cost = LinearCost(Z, kOne);  // min alpha
  initchecker.inequalities.push_back(
      LinearInequality(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2,
                       1));  //-2x-y-alpha <= -2
  initchecker.inequalities.push_back(
      LinearInequality(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6,
                       2));  // -x+2y-alpha <= 6
  initchecker.inequalities.push_back(LinearInequality(
      X, -1.0 * kOne, Z, -1.0 * kOne, 0, 3));  // -x - alpha <= 0
  initchecker.inequalities.push_back(LinearInequality(
      X, 1.0 * kOne, Z, -1.0 * kOne, 20, 4));  // x - alpha <= 20
  initchecker.inequalities.push_back(LinearInequality(
      Y, -1.0 * kOne, Z, -1.0 * kOne, 0, 5));  // -y - alpha <= 0
  LPSolver solver(initchecker);
  VectorValues starter;
  starter.insert(X, kZero);
  starter.insert(Y, kZero);
  starter.insert(Z, Vector::Constant(1, 2.0));
  VectorValues results, duals;
  boost::tie(results, duals) = solver.optimize(starter);
  VectorValues expected;
  expected.insert(X, Vector::Constant(1, 13.5));
  expected.insert(Y, Vector::Constant(1, 6.5));
  expected.insert(Z, Vector::Constant(1, -6.5));
  CHECK(assert_equal(results, expected, 1e-7));
}
Ejemplo n.º 3
0
/* ************************************************************************* */
TEST(VectorValues, resizeLike) {
  // insert, with out-of-order indices
  VectorValues original;
  original.insert(0, Vector_(1, 1.0));
  original.insert(1, Vector_(2, 2.0, 3.0));
  original.insert(5, Vector_(2, 6.0, 7.0));
  original.insert(2, Vector_(2, 4.0, 5.0));

  VectorValues actual(10, 3);
  actual.resizeLike(original);

  // Check dimensions
  LONGS_EQUAL(6, actual.size());
  LONGS_EQUAL(7, actual.dim());
  LONGS_EQUAL(1, actual.dim(0));
  LONGS_EQUAL(2, actual.dim(1));
  LONGS_EQUAL(2, actual.dim(2));
  LONGS_EQUAL(2, actual.dim(5));

  // Logic
  EXPECT(actual.exists(0));
  EXPECT(actual.exists(1));
  EXPECT(actual.exists(2));
  EXPECT(!actual.exists(3));
  EXPECT(!actual.exists(4));
  EXPECT(actual.exists(5));
  EXPECT(!actual.exists(6));

  // Check exceptions
  CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
}
Ejemplo n.º 4
0
/* ************************************************************************* */
TEST(GaussianFactorGraph, gradientAtZero) {
  GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
  VectorValues expected;
  VectorValues actual = gfg.gradientAtZero();
  expected.insert(0, Vector2(-25, 17.5));
  expected.insert(1, Vector2(5, -13.5));
  expected.insert(2, Vector2(29, 4));
  EXPECT(assert_equal(expected, actual));
}
Ejemplo n.º 5
0
/* ************************************************************************* */
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]));
}
Ejemplo n.º 6
0
/* ************************************************************************* */
TEST(GaussianFactorGraph, hessianDiagonal) {
  GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
  VectorValues expected;
  Matrix infoMatrix = gfg.hessian().first;
  Vector d = infoMatrix.diagonal();

  VectorValues actual = gfg.hessianDiagonal();
  expected.insert(0, d.segment<2>(0));
  expected.insert(1, d.segment<2>(2));
  expected.insert(2, d.segment<2>(4));
  EXPECT(assert_equal(expected, actual));
}
Ejemplo n.º 7
0
/* ************************************************************************* */
TEST(GaussianFactorGraph, transposeMultiplication) {
  GaussianFactorGraph A = createSimpleGaussianFactorGraph();

  Errors e;
  e += Vector2(0.0, 0.0), Vector2(15.0, 0.0), Vector2(0.0, -5.0), Vector2(-7.5, -5.0);

  VectorValues expected;
  expected.insert(1, Vector2(-37.5, -50.0));
  expected.insert(2, Vector2(-150.0, 25.0));
  expected.insert(0, Vector2(187.5, 25.0));

  VectorValues actual = A.transposeMultiply(e);
  EXPECT(assert_equal(expected, actual));
}
Ejemplo n.º 8
0
  /* ************************************************************************* */
  VectorValues GaussianConditional::solveOtherRHS(
    const VectorValues& parents, const VectorValues& rhs) const
  {
    // Concatenate all vector values that correspond to parent variables
    Vector xS = parents.vector(FastVector<Key>(beginParents(), endParents()));

    // Instead of updating getb(), update the right-hand-side from the given rhs
    const Vector rhsR = rhs.vector(FastVector<Key>(beginFrontals(), endFrontals()));
    xS = rhsR - get_S() * xS;

    // Solve Matrix
    Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);

    // Scale by sigmas
    if(model_)
      soln.array() *= model_->sigmas().array();

    // Insert solution into a VectorValues
    VectorValues result;
    DenseIndex vectorPosition = 0;
    for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
      result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal)));
      vectorPosition += getDim(frontal);
    }

    return result;
  }
Ejemplo n.º 9
0
VectorValues KeyInfo::x0() const {
  VectorValues result;
  BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) {
    result.insert(item.first, Vector::Zero(item.second.dim()));
  }
  return result;
}
Ejemplo n.º 10
0
  /* ************************************************************************* */
  VectorValues GaussianConditional::solve(const VectorValues& x) const
  {
    // Concatenate all vector values that correspond to parent variables
    const Vector xS = x.vector(FastVector<Key>(beginParents(), endParents()));

    // Update right-hand-side
    const Vector rhs = get_d() - get_S() * xS;

    // Solve matrix
    const Vector solution = get_R().triangularView<Eigen::Upper>().solve(rhs);

    // Check for indeterminant solution
    if (solution.hasNaN()) {
      throw IndeterminantLinearSystemException(keys().front());
    }

    // Insert solution into a VectorValues
    VectorValues result;
    DenseIndex vectorPosition = 0;
    for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
      result.insert(*frontal, solution.segment(vectorPosition, getDim(frontal)));
      vectorPosition += getDim(frontal);
    }

    return result;
  }
Ejemplo n.º 11
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));
}
Ejemplo n.º 12
0
/* ************************************************************************* */
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));
}
Ejemplo n.º 13
0
/* ************************************************************************* */
TEST(LPSolver, LinearCost) {
  LinearCost cost(1, Vector3(2., 4., 6.));
  VectorValues x;
  x.insert(1, Vector3(1., 3., 5.));
  double error = cost.error(x);
  double expectedError = 44.0;
  DOUBLES_EQUAL(expectedError, error, 1e-100);
}
Ejemplo n.º 14
0
/* ************************************************************************* */
TEST(GaussianFactorGraph, multiplyHessianAdd) {
  GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();

  VectorValues x = map_list_of<Key, Vector>(0, Vector2(1, 2))(1, Vector2(3, 4))(2, Vector2(5, 6));

  VectorValues expected;
  expected.insert(0, Vector2(-450, -450));
  expected.insert(1, Vector2(0, 0));
  expected.insert(2, Vector2(950, 1050));

  VectorValues actual;
  gfg.multiplyHessianAdd(1.0, x, actual);
  EXPECT(assert_equal(expected, actual));

  // now, do it with non-zero y
  gfg.multiplyHessianAdd(1.0, x, actual);
  EXPECT(assert_equal(2 * expected, actual));
}
Ejemplo n.º 15
0
/* ************************************************************************* */
TEST(LPSolver, simpleTest1) {
  LP lp = simpleLP1();
  LPSolver lpSolver(lp);
  VectorValues init;
  init.insert(1, Vector::Zero(2));

  VectorValues x1 =
      lpSolver.buildWorkingGraph(InequalityFactorGraph(), init).optimize();
  VectorValues expected_x1;
  expected_x1.insert(1, Vector::Ones(2));
  CHECK(assert_equal(expected_x1, x1, 1e-10));

  VectorValues result, duals;
  boost::tie(result, duals) = lpSolver.optimize(init);
  VectorValues expectedResult;
  expectedResult.insert(1, Vector2(8. / 3., 2. / 3.));
  CHECK(assert_equal(expectedResult, result, 1e-10));
}
Ejemplo n.º 16
0
/* ************************************************************************* */
TEST(VectorValues, assignment) {

  VectorValues actual;

  {
    // insert, with out-of-order indices
    VectorValues original;
    original.insert(0, Vector_(1, 1.0));
    original.insert(1, Vector_(2, 2.0, 3.0));
    original.insert(5, Vector_(2, 6.0, 7.0));
    original.insert(2, Vector_(2, 4.0, 5.0));
    actual = original;
  }

  // Check dimensions
  LONGS_EQUAL(6, actual.size());
  LONGS_EQUAL(7, actual.dim());
  LONGS_EQUAL(1, actual.dim(0));
  LONGS_EQUAL(2, actual.dim(1));
  LONGS_EQUAL(2, actual.dim(2));
  LONGS_EQUAL(2, actual.dim(5));

  // Logic
  EXPECT(actual.exists(0));
  EXPECT(actual.exists(1));
  EXPECT(actual.exists(2));
  EXPECT(!actual.exists(3));
  EXPECT(!actual.exists(4));
  EXPECT(actual.exists(5));
  EXPECT(!actual.exists(6));

  // Check values
  EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
  EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
  EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
  EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5]));
  EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector()));

  // Check exceptions
  CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
}
Ejemplo n.º 17
0
 /* ************************************************************************* */
 VectorValues Values::localCoordinates(const Values& cp) const {
   if(this->size() != cp.size())
     throw DynamicValuesMismatched();
   VectorValues result;
   for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) {
     if(it1->key != it2->key)
       throw DynamicValuesMismatched(); // If keys do not match
     // Will throw a dynamic_cast exception if types do not match
     // NOTE: this is separate from localCoordinates(cp, ordering, result) due to at() vs. insert
     result.insert(it1->key, it1->value.localCoordinates_(it2->value));
   }
   return result;
 }
Ejemplo n.º 18
0
TEST(LPInitSolver, infinite_loop_single_var) {
  LP initchecker;
  initchecker.cost = LinearCost(1, Vector3(0, 0, 1));  // min alpha
  initchecker.inequalities.push_back(
      LinearInequality(1, Vector3(-2, -1, -1), -2, 1));  //-2x-y-alpha <= -2
  initchecker.inequalities.push_back(
      LinearInequality(1, Vector3(-1, 2, -1), 6, 2));  // -x+2y-alpha <= 6
  initchecker.inequalities.push_back(
      LinearInequality(1, Vector3(-1, 0, -1), 0, 3));  // -x - alpha <= 0
  initchecker.inequalities.push_back(
      LinearInequality(1, Vector3(1, 0, -1), 20, 4));  // x - alpha <= 20
  initchecker.inequalities.push_back(
      LinearInequality(1, Vector3(0, -1, -1), 0, 5));  // -y - alpha <= 0
  LPSolver solver(initchecker);
  VectorValues starter;
  starter.insert(1, Vector3(0, 0, 2));
  VectorValues results, duals;
  boost::tie(results, duals) = solver.optimize(starter);
  VectorValues expected;
  expected.insert(1, Vector3(13.5, 6.5, -6.5));
  CHECK(assert_equal(results, expected, 1e-7));
}
Ejemplo n.º 19
0
/* ************************************************************************* */
TEST(VectorValues, append) {
  // insert
  VectorValues actual;
  actual.insert(0, Vector_(1, 1.0));
  actual.insert(1, Vector_(2, 2.0, 3.0));
  actual.insert(2, Vector_(2, 4.0, 5.0));

  // append
  vector<size_t> dims(2);
  dims[0] = 3;
  dims[1] = 5;
  actual.append(dims);

  // Check dimensions
  LONGS_EQUAL(5, actual.size());
  LONGS_EQUAL(13, actual.dim());
  LONGS_EQUAL(1, actual.dim(0));
  LONGS_EQUAL(2, actual.dim(1));
  LONGS_EQUAL(2, actual.dim(2));
  LONGS_EQUAL(3, actual.dim(3));
  LONGS_EQUAL(5, actual.dim(4));

  // Logic
  EXPECT(actual.exists(0));
  EXPECT(actual.exists(1));
  EXPECT(actual.exists(2));
  EXPECT(actual.exists(3));
  EXPECT(actual.exists(4));
  EXPECT(!actual.exists(5));

  // Check values
  EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
  EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
  EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));

  // Check exceptions
  CHECK_EXCEPTION(actual.insert(3, Vector()), invalid_argument);
}
Ejemplo n.º 20
0
/* ************************************************************************* */
TEST(HessianFactor, hessianDiagonal)
{
  Matrix G11 = (Matrix(1, 1) << 1);
  Matrix G12 = (Matrix(1, 2) << 0, 0);
  Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1);
  Vector g1 = (Vector(1) << -7);
  Vector g2 = (Vector(2) << -8, -9);
  double f = 194;

  HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f);

  // hessianDiagonal
  VectorValues expected;
  expected.insert(0, (Vector(1) << 1));
  expected.insert(1, (Vector(2) << 1,1));
  EXPECT(assert_equal(expected, factor.hessianDiagonal()));

  // hessianBlockDiagonal
  map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
  LONGS_EQUAL(2,actualBD.size());
  EXPECT(assert_equal(G11,actualBD[0]));
  EXPECT(assert_equal(G22,actualBD[1]));
}
Ejemplo n.º 21
0
/* ************************************************************************* */
TEST (Serialization, linear_factors) {
  VectorValues values;
  values.insert(0, (Vector(1) << 1.0));
  values.insert(1, (Vector(2) << 2.0,3.0));
  values.insert(2, (Vector(2) << 4.0,5.0));
  EXPECT(equalsObj<VectorValues>(values));
  EXPECT(equalsXML<VectorValues>(values));
  EXPECT(equalsBinary<VectorValues>(values));

  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);
  EXPECT(equalsObj(jacobianfactor));
  EXPECT(equalsXML(jacobianfactor));
  EXPECT(equalsBinary(jacobianfactor));

  HessianFactor hessianfactor(jacobianfactor);
  EXPECT(equalsObj(hessianfactor));
  EXPECT(equalsXML(hessianfactor));
  EXPECT(equalsBinary(hessianfactor));
}
Ejemplo n.º 22
0
/* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate1) {
  Matrix3 A01 = 3.0 * I_3x3;
  Vector3 b0(1, 0, 0);

  Matrix3 A21 = 4.0 * I_3x3;
  Vector3 b2 = Vector3::Zero();

  GaussianFactorGraph gfg;
  gfg.add(1, A01, b0);
  gfg.add(1, A21, b2);

  Matrix63 A1;
  A1 << A01, A21;
  Vector6 b;
  b << b0, b2;

  // create a full, uneliminated version of the factor
  JacobianFactor jacobian(1, A1, b);

  // Make sure combining works
  HessianFactor hessian(gfg);
  VectorValues v;
  v.insert(1, Vector3(1, 0, 0));
  EXPECT_DOUBLES_EQUAL(jacobian.error(v), hessian.error(v), 1e-9);
  EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6));
  EXPECT(assert_equal(25.0 * I_3x3, hessian.information(), 1e-9));
  EXPECT(
      assert_equal(jacobian.augmentedInformation(),
          hessian.augmentedInformation(), 1e-9));

  // perform elimination on jacobian
  Ordering ordering = list_of(1);
  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));
  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));
}
Ejemplo n.º 23
0
/* ************************************************************************* */
TEST(VectorValues, permute) {

	VectorValues original;
	original.insert(0, Vector_(1, 1.0));
	original.insert(1, Vector_(2, 2.0, 3.0));
	original.insert(2, Vector_(2, 4.0, 5.0));
	original.insert(3, Vector_(2, 6.0, 7.0));

	VectorValues expected;
	expected.insert(0, Vector_(2, 4.0, 5.0)); // from 2
	expected.insert(1, Vector_(1, 1.0)); // from 0
	expected.insert(2, Vector_(2, 6.0, 7.0)); // from 3
	expected.insert(3, Vector_(2, 2.0, 3.0)); // from 1

	Permutation permutation(4);
	permutation[0] = 2;
	permutation[1] = 0;
	permutation[2] = 3;
	permutation[3] = 1;

	VectorValues actual = original.permute(permutation);

	EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(RegularHessianFactor, Constructors)
{
  // First construct a regular JacobianFactor
  // 0.5*|x0 + x1 + x3 - [1;2]|^2 = 0.5*|A*x-b|^2, with A=[I I I]
  Matrix A1 = I_2x2, A2 = I_2x2, A3 = I_2x2;
  Vector2 b(1,2);
  vector<pair<Key, Matrix> > terms;
  terms += make_pair(0, A1), make_pair(1, A2), make_pair(3, A3);
  RegularJacobianFactor<2> jf(terms, b);

  // Test conversion from JacobianFactor
  RegularHessianFactor<2> factor(jf);

  // 0.5*|A*x-b|^2 = 0.5*(Ax-b)'*(Ax-b) = 0.5*x'*A'A*x - x'*A'b + 0.5*b'*b
  // Compare with comment in HessianFactor: E(x) = 0.5 x^T G x - x^T g + 0.5 f
  // Hence G = I6, g A'*b = [b;b;b], and f = b'*b = 1+4 = 5
  Matrix G11 = I_2x2;
  Matrix G12 = I_2x2;
  Matrix G13 = I_2x2;

  Matrix G22 = I_2x2;
  Matrix G23 = I_2x2;

  Matrix G33 = I_2x2;

  Vector2 g1 = b, g2 = b, g3 = b;

  double f = 5;

  // Test ternary constructor
  RegularHessianFactor<2> factor2(0, 1, 3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
  EXPECT(assert_equal(factor,factor2));

  // Test n-way constructor
  vector<Key> keys; keys += 0, 1, 3;
  vector<Matrix> Gs; Gs += G11, G12, G13, G22, G23, G33;
  vector<Vector> gs; gs += g1, g2, g3;
  RegularHessianFactor<2> factor3(keys, Gs, gs, f);
  EXPECT(assert_equal(factor, factor3));

  // Test constructor from Gaussian Factor Graph
  GaussianFactorGraph gfg;
  gfg += jf;
  RegularHessianFactor<2> factor4(gfg);
  EXPECT(assert_equal(factor, factor4));
  GaussianFactorGraph gfg2;
  gfg2 += factor;
  RegularHessianFactor<2> factor5(gfg);
  EXPECT(assert_equal(factor, factor5));

  // Test constructor from Information matrix
  Matrix info = factor.augmentedInformation();
  vector<size_t> dims; dims += 2, 2, 2;
  SymmetricBlockMatrix sym(dims, info, true);
  RegularHessianFactor<2> factor6(keys, sym);
  EXPECT(assert_equal(factor, factor6));

  // multiplyHessianAdd:
  {
  // brute force
  Matrix AtA = factor.information();
  HessianFactor::const_iterator i1 = factor.begin();
  HessianFactor::const_iterator i2 = i1 + 1;
  Vector X(6); X << 1,2,3,4,5,6;
  Vector Y(6); Y << 9, 12, 9, 12, 9, 12;
  EXPECT(assert_equal(Y,AtA*X));

  VectorValues x = map_list_of<Key, Vector>
    (0, Vector2(1,2))
    (1, Vector2(3,4))
    (3, Vector2(5,6));

  VectorValues expected;
  expected.insert(0, Y.segment<2>(0));
  expected.insert(1, Y.segment<2>(2));
  expected.insert(3, Y.segment<2>(4));

  // VectorValues version
  double alpha = 1.0;
  VectorValues actualVV;
  actualVV.insert(0, Vector2::Zero());
  actualVV.insert(1, Vector2::Zero());
  actualVV.insert(3, Vector2::Zero());
  factor.multiplyHessianAdd(alpha, x, actualVV);
  EXPECT(assert_equal(expected, actualVV));

  // RAW ACCESS
  Vector expected_y(8); expected_y << 9, 12, 9, 12, 0, 0, 9, 12;
  Vector fast_y = Vector8::Zero();
  double xvalues[8] = {1,2,3,4,0,0,5,6};
  factor.multiplyHessianAdd(alpha, xvalues, fast_y.data());
  EXPECT(assert_equal(expected_y, fast_y));

  // now, do it with non-zero y
  factor.multiplyHessianAdd(alpha, xvalues, fast_y.data());
  EXPECT(assert_equal(2*expected_y, fast_y));

  // check some expressions
  EXPECT(assert_equal(G12,factor.info(i1,i2).knownOffDiagonal()));
  EXPECT(assert_equal(G22,factor.info(i2,i2).selfadjointView()));
  EXPECT(assert_equal((Matrix)G12.transpose(),factor.info(i2,i1).knownOffDiagonal()));
  }
}
Ejemplo n.º 25
0
 /* ************************************************************************* */
 VectorValues Values::zeroVectors() const {
   VectorValues result;
   BOOST_FOREACH(const ConstKeyValuePair& key_value, *this)
     result.insert(key_value.key, Vector::Zero(key_value.value.dim()));
   return result;
 }