/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testHessianFactor.cpp
 * @author  Richard Roberts
 * @date    Dec 15, 2010
 */

#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/TestableAssertions.h>

#include <CppUnitLite/TestHarness.h>

#include <vector>
#include <utility>

using namespace std;
using namespace gtsam;

const double tol = 1e-5;

using Dims = std::vector<Eigen::Index>;  // For constructing block matrices

/* ************************************************************************* */
TEST(HessianFactor, Slot)
{
  KeyVector keys {2, 4, 1};
  EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2));
  EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4));
  EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1));
  EXPECT_LONGS_EQUAL(3, GaussianFactor::Slot(keys,5)); // does not exist
}

/* ************************************************************************* */
TEST(HessianFactor, emptyConstructor)
{
  HessianFactor f;
  DOUBLES_EQUAL(0.0, f.constantTerm(), 1e-9);        // Constant term should be zero
  //EXPECT(assert_equal(Vector(), f.linearTerm()));    // Linear term should be empty
  EXPECT(assert_equal((Matrix) Z_1x1, f.info().selfadjointView())); // Full matrix should be 1-by-1 zero matrix
  DOUBLES_EQUAL(0.0, f.error(VectorValues()), 1e-9); // Should have zero error
}

/* ************************************************************************* */
TEST(HessianFactor, ConversionConstructor)
{
  HessianFactor expected(KeyVector{0, 1},
    SymmetricBlockMatrix(Dims{2, 4, 1}, (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,   25.0000,       0.0,       0.0,       0.0,   -5.0000,
           0.0,  -25.0000,       0.0,   25.0000,       0.0,       0.0,    7.5000,
     -100.0000,       0.0,       0.0,       0.0,  100.0000,       0.0,  -20.0000,
           0.0, -100.0000,       0.0,       0.0,       0.0,  100.0000,   10.0000,
       25.0000,  -17.5000,   -5.0000,    7.5000,  -20.0000,   10.0000,    8.2500).finished()));

  JacobianFactor jacobian(
    0, (Matrix(4,2) << -1., 0.,
                    +0.,-1.,
                     1., 0.,
                    +0.,1.).finished(),
    1, (Matrix(4,4) << 1., 0., 0.00,  0., // f4
                    0., 1., 0.00,  0., // f4
                    0., 0.,  -1.,  0., // f2
                    0., 0., 0.00, -1.).finished(), // f2
    (Vector(4) << -0.2, 0.3, 0.2, -0.1).finished(),
    noiseModel::Diagonal::Sigmas((Vector(4) << 0.2, 0.2, 0.1, 0.1).finished()));

  HessianFactor actual(jacobian);

  VectorValues values{{0, Vector2(1.0, 2.0)}, {1, Vector4(3.0, 4.0, 5.0, 6.0)}};

  EXPECT_LONGS_EQUAL(2, (long)actual.size());
  EXPECT(assert_equal(expected, actual, 1e-9));
  EXPECT_DOUBLES_EQUAL(jacobian.error(values), actual.error(values), 1e-9);
}

/* ************************************************************************* */
TEST(HessianFactor, Constructor1)
{
  Matrix G = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished();
  Vector g = Vector2(-8.0, -9.0);
  double f = 10.0;
  HessianFactor factor(0, G, g, f);

  // extract underlying parts
  EXPECT(assert_equal(G, Matrix(factor.info().diagonalBlock(0))));
  EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10);
  EXPECT(assert_equal(g, Vector(factor.linearTerm())));
  EXPECT_LONGS_EQUAL(1, (long)factor.size());

  VectorValues dx{{0, Vector2(1.5, 2.5)}};

  // error 0.5*(f - 2*x'*g + x'*G*x)
  double expected = 80.375;
  double actual = factor.error(dx);
  double expected_manual = 0.5 * (f - 2.0 * dx[0].dot(g) + dx[0].transpose() * G.selfadjointView<Eigen::Upper>() * dx[0]);
  EXPECT_DOUBLES_EQUAL(expected, expected_manual, 1e-10);
  EXPECT_DOUBLES_EQUAL(expected, actual, 1e-10);
}

/* ************************************************************************* */
TEST(HessianFactor, Constructor1b)
{
  Vector mu = Vector2(1.0,2.0);
  Matrix Sigma = I_2x2;

  HessianFactor factor(0, mu, Sigma);

  Matrix G = I_2x2;
  Vector g = G*mu;
  double f = dot(g,mu);

  // Check
  EXPECT(assert_equal(G, Matrix(factor.info().diagonalBlock(0))));
  EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10);
  EXPECT(assert_equal(g, Vector(factor.linearTerm())));
  EXPECT_LONGS_EQUAL(1, (long)factor.size());
}

/* ************************************************************************* */
TEST(HessianFactor, Constructor2)
{
  Matrix G11 = (Matrix(1,1) << 1.0).finished();
  Matrix G12 = (Matrix(1,2) << 2.0, 4.0).finished();
  Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished();
  Vector g1 = (Vector(1) << -7.0).finished();
  Vector g2 = Vector2(-8.0, -9.0);
  double f = 10.0;

  Vector dx0 = (Vector(1) << 0.5).finished();
  Vector dx1 = Vector2(1.5, 2.5);

  VectorValues dx{{0, dx0}, {1, dx1}};

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

  double expected = 90.5;
  double actual = factor.error(dx);

  DOUBLES_EQUAL(expected, actual, 1e-10);
  LONGS_EQUAL(4, (long)factor.rows());
  DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10);

  Vector linearExpected(3);  linearExpected << g1, g2;
  EXPECT(assert_equal(linearExpected, factor.linearTerm()));

  EXPECT(assert_equal(G11, factor.info().diagonalBlock(0)));
  EXPECT(assert_equal(G12, factor.info().aboveDiagonalBlock(0, 1)));
  EXPECT(assert_equal(G22, factor.info().diagonalBlock(1)));

  // Check case when vector values is larger than factor
  VectorValues dxLarge {{0, dx0}, {1, dx1}, {2, Vector2(0.1, 0.2)}};
  EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10);
}

/* ************************************************************************* */
TEST(HessianFactor, Constructor3)
{
  Matrix G11 = (Matrix(1,1) << 1.0).finished();
  Matrix G12 = (Matrix(1,2) << 2.0, 4.0).finished();
  Matrix G13 = (Matrix(1,3) << 3.0, 6.0, 9.0).finished();

  Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished();
  Matrix G23 = (Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0).finished();

  Matrix G33 = (Matrix(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0).finished();

  Vector g1 = (Vector(1) << -7.0).finished();
  Vector g2 = Vector2(-8.0, -9.0);
  Vector g3 = Vector3(1.0,  2.0,  3.0);

  double f = 10.0;

  Vector dx0 = (Vector(1) << 0.5).finished();
  Vector dx1 = Vector2(1.5, 2.5);
  Vector dx2 = Vector3(1.5, 2.5, 3.5);

  VectorValues dx {{0, dx0}, {1, dx1}, {2, dx2}};
  HessianFactor factor(0, 1, 2, G11, G12, G13, g1, G22, G23, g2, G33, g3, f);

  double expected = 371.3750;
  double actual = factor.error(dx);

  DOUBLES_EQUAL(expected, actual, 1e-10);
  LONGS_EQUAL(7, (long)factor.rows());
  DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10);

  Vector linearExpected(6);  linearExpected << g1, g2, g3;
  EXPECT(assert_equal(linearExpected, factor.linearTerm()));

  EXPECT(assert_equal(G11, factor.info().diagonalBlock(0)));
  EXPECT(assert_equal(G12, factor.info().aboveDiagonalBlock(0, 1)));
  EXPECT(assert_equal(G13, factor.info().aboveDiagonalBlock(0, 2)));
  EXPECT(assert_equal(G22, factor.info().diagonalBlock(1)));
  EXPECT(assert_equal(G23, factor.info().aboveDiagonalBlock(1, 2)));
  EXPECT(assert_equal(G33, factor.info().diagonalBlock(2)));
}

/* ************************************************************************* */
TEST(HessianFactor, ConstructorNWay)
{
  Matrix G11 = (Matrix(1,1) << 1.0).finished();
  Matrix G12 = (Matrix(1,2) << 2.0, 4.0).finished();
  Matrix G13 = (Matrix(1,3) << 3.0, 6.0, 9.0).finished();

  Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished();
  Matrix G23 = (Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0).finished();

  Matrix G33 = (Matrix(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0).finished();

  Vector g1 = (Vector(1) << -7.0).finished();
  Vector g2 = Vector2(-8.0, -9.0);
  Vector g3 = Vector3(1.0,  2.0,  3.0);

  double f = 10.0;

  Vector dx0 = (Vector(1) << 0.5).finished();
  Vector dx1 = Vector2(1.5, 2.5);
  Vector dx2 = Vector3(1.5, 2.5, 3.5);

  VectorValues dx {{0, dx0}, {1, dx1}, {2, dx2}};

  KeyVector js {0, 1, 2};
  std::vector<Matrix> Gs;
  Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
  std::vector<Vector> gs;
  gs.push_back(g1); gs.push_back(g2); gs.push_back(g3);
  HessianFactor factor(js, Gs, gs, f);

  double expected = 371.3750;
  double actual = factor.error(dx);

  DOUBLES_EQUAL(expected, actual, 1e-10);
  LONGS_EQUAL(7, (long)factor.rows());
  DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10);

  Vector linearExpected(6);  linearExpected << g1, g2, g3;
  EXPECT(assert_equal(linearExpected, factor.linearTerm()));

  EXPECT(assert_equal(G11, factor.info().diagonalBlock(0)));
  EXPECT(assert_equal(G12, factor.info().aboveDiagonalBlock(0, 1)));
  EXPECT(assert_equal(G13, factor.info().aboveDiagonalBlock(0, 2)));
  EXPECT(assert_equal(G22, factor.info().diagonalBlock(1)));
  EXPECT(assert_equal(G23, factor.info().aboveDiagonalBlock(1, 2)));
  EXPECT(assert_equal(G33, factor.info().diagonalBlock(2)));
}

/* ************************************************************************* */
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 {1};
  GaussianConditional::shared_ptr expectedConditional;
  JacobianFactor::shared_ptr expectedFactor;
  std::tie(expectedConditional, expectedFactor) = jacobian.eliminate(ordering);
  CHECK(expectedFactor);

  // Eliminate
  GaussianConditional::shared_ptr actualConditional;
  HessianFactor::shared_ptr actualHessian;
  std::tie(actualConditional, actualHessian) = //
      EliminateCholesky(gfg, ordering);
  actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison

  EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
#ifdef TEST_ERROR_EQUIVALENCE
  // these tests disabled because QR cuts off the all zeros + error row
  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));
#endif
  }

/* ************************************************************************* */
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 {0};
  GaussianConditional::shared_ptr expectedConditional;
  JacobianFactor::shared_ptr expectedFactor;
  std::tie(expectedConditional, expectedFactor) = //
      jacobian.eliminate(ordering);

  // Eliminate
  GaussianConditional::shared_ptr actualConditional;
  HessianFactor::shared_ptr actualHessian;
  std::tie(actualConditional, actualHessian) = //
      EliminateCholesky(gfg, ordering);
  actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison

  EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
#ifdef TEST_ERROR_EQUIVALENCE
  // these tests disabled because QR cuts off the all zeros + error row
  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));
#endif
}

/* ************************************************************************* */
TEST(HessianFactor, eliminate2 )
{
  // sigmas
  double sigma1 = 0.2;
  double sigma2 = 0.1;
  Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2).finished();

  // the combined linear factor
  Matrix Ax2 = (Matrix(4,2) <<
      // x2
      -1., 0.,
      +0.,-1.,
      1., 0.,
      +0.,1.
  ).finished();

  Matrix Al1x1 = (Matrix(4,4) <<
      // l1   x1
      1., 0., 0.00,  0., // f4
      0., 1., 0.00,  0., // f4
      0., 0., -1.,  0., // f2
      0., 0., 0.00,-1.  // f2
  ).finished();

  // the RHS
  Vector b2(4);
  b2(0) = -0.2;
  b2(1) =  0.3;
  b2(2) =  0.2;
  b2(3) = -0.1;

  vector<pair<Key, Matrix> > meas;
  meas.push_back(make_pair(0, Ax2));
  meas.push_back(make_pair(1, Al1x1));
  JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));

  // eliminate the combined factor
  HessianFactor::shared_ptr combinedLF_Chol(new HessianFactor(combined));
  GaussianFactorGraph combinedLFG_Chol {combinedLF_Chol};

  std::pair<GaussianConditional::shared_ptr, HessianFactor::shared_ptr> actual_Chol =
    EliminateCholesky(combinedLFG_Chol, Ordering{0});

  // create expected Conditional Gaussian
  double oldSigma = 0.0894427; // from when R was made unit
  Matrix R11 = (Matrix(2,2) <<
      1.00,  0.00,
      0.00,  1.00
  ).finished()/oldSigma;
  Matrix S12 = (Matrix(2,4) <<
      -0.20, 0.00,-0.80, 0.00,
      +0.00,-0.20,+0.00,-0.80
  ).finished()/oldSigma;
  Vector d = Vector2(0.2,-0.14)/oldSigma;
  GaussianConditional expectedCG(0, d, R11, 1, S12);
  EXPECT(assert_equal(expectedCG, *actual_Chol.first, 1e-4));

  // the expected linear factor
  double sigma = 0.2236;
  Matrix Bl1x1 = (Matrix(2,4) <<
      // l1          x1
      1.00, 0.00, -1.00,  0.00,
      0.00, 1.00, +0.00, -1.00
  ).finished()/sigma;
  Vector b1 = Vector2(0.0,0.894427);
  JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0));
  EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3));
}

/* ************************************************************************* */
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).finished();
  Matrix A1 = (Matrix(2, 2) <<
  -2.23606798,        0.0,
         0.0, -2.23606798).finished();
  Matrix A2 = (Matrix(2, 2) <<
  -8.94427191,      0.0,
         0.0, -8.94427191).finished();
  Vector b = Vector2(2.23606798,-1.56524758);
  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector::Ones(2));
  GaussianFactorGraph factors{
      std::make_shared<JacobianFactor>(0, A0, 1, A1, 2, A2, b, model)};

  // 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).finished();
  EXPECT(assert_equal(expected, Matrix(actual.info().selfadjointView()), tol));

}

/* ************************************************************************* */
TEST(HessianFactor, gradientAtZero)
{
  Matrix G11 = (Matrix(1, 1) << 1).finished();
  Matrix G12 = (Matrix(1, 2) << 0, 0).finished();
  Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1).finished();
  Vector g1 = (Vector(1) << -7).finished();
  Vector g2 = Vector2(-8, -9);
  double f = 194;

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

  // test gradient at zero
  VectorValues expectedG{{0, -g1}, {1, -g2}};
  Matrix A; Vector b; std::tie(A,b) = factor.jacobian();
  KeyVector keys {0, 1};
  EXPECT(assert_equal(-A.transpose()*b, expectedG.vector(keys)));
  VectorValues actualG = factor.gradientAtZero();
  EXPECT(assert_equal(expectedG, actualG));
}

/* ************************************************************************* */
TEST(HessianFactor, gradient)
{
  Matrix G11 = (Matrix(1, 1) << 1).finished();
  Matrix G12 = (Matrix(1, 2) << 0, 0).finished();
  Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1).finished();
  Vector g1 = (Vector(1) << -7).finished();
  Vector g2 = (Vector(2) << -8, -9).finished();
  double f = 194;

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

  // test gradient
  Vector x0 = (Vector(1) << 3.0).finished();
  Vector x1 = (Vector(2) << -3.5, 7.1).finished();
  VectorValues x {{0, x0}, {1, x1}};

  Vector expectedGrad0 = (Vector(1) << 10.0).finished();
  Vector expectedGrad1 = (Vector(2) << 4.5, 16.1).finished();
  Vector grad0 = factor.gradient(0, x);
  Vector grad1 = factor.gradient(1, x);

  EXPECT(assert_equal(expectedGrad0, grad0));
  EXPECT(assert_equal(expectedGrad1, grad1));
}

/* ************************************************************************* */
TEST(HessianFactor, hessianDiagonal)
{
  Matrix G11 = (Matrix(1, 1) << 1).finished();
  Matrix G12 = (Matrix(1, 2) << 0, 0).finished();
  Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1).finished();
  Vector g1 = (Vector(1) << -7).finished();
  Vector g2 = Vector2(-8, -9);
  double f = 194;

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

  // hessianDiagonal
  VectorValues expected;
  expected.insert(0, (Vector(1) << 1).finished());
  expected.insert(1, Vector2(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]));
}

/* ************************************************************************* */
TEST(HessianFactor, Solve)
{
  Matrix2 A;
  A << 1, 2, 3, 4;
  Matrix2 G = A.transpose() * A;
  Vector2 b(5, 6);
  Vector2 g = A.transpose() * b;
  double f = 0;
  Key key(55);
  HessianFactor factor(key, G, g, f);

  VectorValues expected;
  expected.insert(key, A.inverse() * b);
  EXPECT(assert_equal(expected, factor.solve()));
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */