diff --git a/gtsam/linear/HessianFactor-inl.h b/gtsam/linear/HessianFactor-inl.h new file mode 100644 index 000000000..31a327bb8 --- /dev/null +++ b/gtsam/linear/HessianFactor-inl.h @@ -0,0 +1,41 @@ +/* ---------------------------------------------------------------------------- + + * 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 HessianFactor-inl.h + * @brief Contains the HessianFactor class, a general quadratic factor + * @author Richard Roberts + * @date Dec 8, 2010 + */ + +#pragma once + +namespace gtsam { + + /* ************************************************************************* */ + template + HessianFactor::HessianFactor(const KEYS& keys, const SymmetricBlockMatrix& augmentedInformation) : + GaussianFactor(keys), info_(augmentedInformation) + { + // Check number of variables + if(Base::keys_.size() != augmentedInformation.nBlocks() - 1) + throw std::invalid_argument( + "Error in HessianFactor constructor input. Number of provided keys plus\n" + "one for the information vector must equal the number of provided matrix blocks."); + + // Check RHS dimension + if(augmentedInformation(0, augmentedInformation.nBlocks() - 1).cols() != 1) + throw std::invalid_argument( + "Error in HessianFactor constructor input. The last provided matrix block\n" + "must be the information vector, but the last provided block had more than one column."); + } + +} \ No newline at end of file diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 6b5f8581e..83e30f881 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -150,12 +150,12 @@ GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), } /* ************************************************************************* */ -namespace { DenseIndex _getColsHF(const Matrix& m) { return m.cols(); } } +namespace { DenseIndex _getSizeHF(const Vector& m) { return m.size(); } } /* ************************************************************************* */ HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, const std::vector& gs, double f) : -GaussianFactor(js), info_(br::join(Gs | br::transformed(&_getColsHF), cref_list_of<1,DenseIndex>(1))) +GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1))) { // Get the number of variables size_t variable_count = js.size(); @@ -205,10 +205,10 @@ namespace { { if(jf.get_model()->isConstrained()) throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); - info.full() = jf.matrixObject().full().transpose() * jfModel->invsigmas().asDiagonal() * + info.full().noalias() = jf.matrixObject().full().transpose() * jfModel->invsigmas().asDiagonal() * jfModel->invsigmas().asDiagonal() * jf.matrixObject().full(); } else { - info.full() = jf.matrixObject().full().transpose() * jf.matrixObject().full(); + info.full().noalias() = jf.matrixObject().full().transpose() * jf.matrixObject().full(); } } } @@ -241,26 +241,31 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : } /* ************************************************************************* */ -HessianFactor::HessianFactor(const GaussianFactorGraph& factors) -{ - Scatter scatter(factors); +namespace { + DenseIndex _dimFromScatterEntry(const Scatter::value_type& key_slotentry) { + return key_slotentry.second.dimension; } } - // Pull out keys and dimensions - gttic(keys); - vector dimensions(scatter.size() + 1); - BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { - dimensions[var_slot.second.slot] = var_slot.second.dimension; +/* ************************************************************************* */ +HessianFactor::HessianFactor(const GaussianFactorGraph& factors, + boost::optional ordering, + boost::optional scatter) +{ + boost::optional computedScatter; + if(!scatter) { + computedScatter = Scatter(factors); + scatter = computedScatter; } - // This is for the r.h.s. vector - dimensions.back() = 1; - gttoc(keys); // Allocate and copy keys gttic(allocate); - info_ = SymmetricBlockMatrix(dimensions); + // Allocate with dimensions for each variable plus 1 at the end for the information vector + vector dims(scatter->size() + 1); + br::copy(*scatter | br::transformed(&_dimFromScatterEntry), dims.begin()); + dims.back() = 1; + info_ = SymmetricBlockMatrix(dims); info_.full().setZero(); - keys_.resize(scatter.size()); - br::copy(scatter | br::map_keys, keys_.begin()); + keys_.resize(scatter->size()); + br::copy(*scatter | br::map_keys, keys_.begin()); gttoc(allocate); // Form A' * A @@ -269,9 +274,9 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors) { if(factor) { if(const HessianFactor* hessian = dynamic_cast(factor.get())) - updateATA(*hessian, scatter); + updateATA(*hessian, *scatter); else if(const JacobianFactor* jacobian = dynamic_cast(factor.get())) - updateATA(*jacobian, scatter); + updateATA(*jacobian, *scatter); else throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); } @@ -286,7 +291,7 @@ void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) c for(const_iterator key=this->begin(); key!=this->end(); ++key) cout << formatter(*key) << "(" << this->getDim(key) << ") "; cout << "\n"; - gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView()), "Ab^T * Ab: "); + gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView()), "Augmented information matrix: "); } /* ************************************************************************* */ @@ -294,6 +299,8 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { if(!dynamic_cast(&lf)) return false; else { + if(!Factor::equals(lf, tol)) + return false; Matrix thisMatrix = this->info_.full().selfadjointView(); thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; Matrix rhsMatrix = static_cast(lf).info_.full().selfadjointView(); @@ -348,9 +355,9 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte // First build an array of slots gttic(slots); //size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. - vector slots(update.size()); - size_t slot = 0; - BOOST_FOREACH(Index j, update) { + vector slots(update.size()); + DenseIndex slot = 0; + BOOST_FOREACH(Key j, update) { slots[slot] = scatter.find(j)->second.slot; ++ slot; } @@ -358,10 +365,10 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte // Apply updates to the upper triangle gttic(update); - for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; - for(size_t j1=0; j1<=j2; ++j1) { - size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; + for(DenseIndex j2=0; j2info_.nBlocks()-1 : slots[j2]; + for(DenseIndex j1=0; j1<=j2; ++j1) { + DenseIndex slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; if(slot2 > slot1) info_(slot1, slot2).noalias() += update.info_(j1, j2); else if(slot1 > slot2) @@ -379,24 +386,16 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt updateATA(HessianFactor(update), scatter); } -/* ************************************************************************* */ -void HessianFactor::partialCholesky(size_t nrFrontals) -{ - throw std::runtime_error("Not implemented"); - - //if(!choleskyPartial(matrix_, info_.offset(nrFrontals))) - // throw IndeterminantLinearSystemException(this->keys().front()); -} - /* ************************************************************************* */ GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFrontals) { - static const bool debug = false; + gttic(HessianFactor_splitEliminatedFactor); // Create one big conditionals with many frontal variables. gttic(Construct_conditional); - size_t varDim = info_.offset(nrFrontals); + const size_t varDim = info_.offset(nrFrontals); VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(info_, varDim); + Ab.full() = info_.range(0, nrFrontals, 0, info_.nBlocks()); GaussianConditional::shared_ptr conditional = boost::make_shared( keys_, nrFrontals, Ab); gttoc(Construct_conditional); @@ -414,24 +413,51 @@ GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFr /* ************************************************************************* */ GaussianFactor::shared_ptr HessianFactor::negate() const { - throw std::runtime_error("Not implemented"); + shared_ptr result = boost::make_shared(*this); + result->info_.full() = -result->info_.full(); // Negate the information matrix of the result + return result; +} - //// Copy Hessian Blocks from Hessian factor and invert - //std::vector js; - //std::vector Gs; - //std::vector gs; - //double f; - //js.insert(js.end(), begin(), end()); - //for(size_t i = 0; i < js.size(); ++i){ - // for(size_t j = i; j < js.size(); ++j){ - // Gs.push_back( -info(begin()+i, begin()+j) ); - // } - // gs.push_back( -linearTerm(begin()+i) ); - //} - //f = -constantTerm(); +/* ************************************************************************* */ +std::pair, boost::shared_ptr > + EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) +{ + gttic(EliminateCholesky); - //// Create the Anti-Hessian Factor from the negated blocks - //return HessianFactorOrdered::shared_ptr(new HessianFactorOrdered(js, Gs, gs, f)); + // Build joint factor + HessianFactor::shared_ptr jointFactor; + try { + jointFactor = boost::make_shared(factors, keys); + } catch(std::invalid_argument&) { + throw InvalidDenseElimination( + "EliminateCholesky was called with a request to eliminate variables that are not\n" + "involved in the provided factors."); + } + + // Do dense elimination + if(!choleskyPartial(jointFactor->info_.matrix(), jointFactor->info_.offset(keys.size()))) + throw IndeterminantLinearSystemException(keys.front()); + + // Split conditional + GaussianConditional::shared_ptr conditional = jointFactor->splitEliminatedFactor(keys.size()); + + return make_pair(conditional, jointFactor); +} + +/* ************************************************************************* */ +std::pair, boost::shared_ptr > + EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys) +{ + gttic(EliminatePreferCholesky); + + // If any JacobianFactors have constrained noise models, we have to convert + // all factors to JacobianFactors. Otherwise, we can convert all factors + // to HessianFactors. This is because QR can handle constrained noise + // models but Cholesky cannot. + if (hasConstraints(factors)) + return EliminateQR(factors, keys); + else + return EliminateCholesky(factors, keys); } } // gtsam diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 7e1da53f2..1442eb659 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -22,27 +22,21 @@ #include #include - -// Forward declarations for friend unit tests -class HessianFactorConversionConstructorTest; -class HessianFactorConstructor1Test; -class HessianFactorConstructor1bTest; -class HessianFactorcombineTest; - +#include namespace gtsam { // Forward declarations + class Ordering; class JacobianFactor; class GaussianConditional; class GaussianBayesNet; + class GaussianFactorGraph; - // Definition of Scatter, which is an intermediate data structure used when - // building a HessianFactor incrementally, to get the keys in the right - // order. - // The "scatter" is a map from global variable indices to slot indices in the - // union of involved variables. We also include the dimensionality of the - // variable. + // Definition of Scatter, which is an intermediate data structure used when building a + // HessianFactor incrementally, to get the keys in the right order. The "scatter" is a map from + // global variable indices to slot indices in the union of involved variables. We also include + // the dimensionality of the variable. struct GTSAM_EXPORT SlotEntry { size_t slot; size_t dimension; @@ -50,7 +44,7 @@ namespace gtsam { : slot(_slot), dimension(_dimension) {} std::string toString() const; }; - class Scatter : public FastMap { + class Scatter : public FastMap { public: Scatter() {} Scatter(const GaussianFactorGraph& gfg); @@ -182,6 +176,11 @@ namespace gtsam { HessianFactor(const std::vector& js, const std::vector& Gs, const std::vector& gs, double f); + /** Constructor with an arbitrary number of keys and with the augmented information matrix + * specified as a block matrix. */ + template + HessianFactor(const KEYS& keys, const SymmetricBlockMatrix& augmentedInformation); + /** Construct from a JacobianFactor (or from a GaussianConditional since it derives from it) */ explicit HessianFactor(const JacobianFactor& cg); @@ -190,7 +189,9 @@ namespace gtsam { explicit HessianFactor(const GaussianFactor& factor); /** Combine a set of factors into a single dense HessianFactor */ - HessianFactor(const GaussianFactorGraph& factors); + explicit HessianFactor(const GaussianFactorGraph& factors, + boost::optional ordering = boost::none, + boost::optional scatter = boost::none); /** Destructor */ virtual ~HessianFactor() {} @@ -331,20 +332,8 @@ namespace gtsam { */ virtual Matrix augmentedJacobian() const; - // Friend unit test classes - friend class ::HessianFactorConversionConstructorTest; - friend class ::HessianFactorConstructor1Test; - friend class ::HessianFactorConstructor1bTest; - friend class ::HessianFactorcombineTest; - - // used in eliminateCholesky: - - /** - * Do Cholesky. Note that after this, the lower triangle still contains - * some untouched non-zeros that should be zero. We zero them while - * extracting submatrices in splitEliminatedFactor. Frank says :-( - */ - void partialCholesky(size_t nrFrontals); + /** Return the full augmented Hessian matrix of this factor as a SymmetricBlockMatrix object. */ + const SymmetricBlockMatrix& matrixObject() const { return info_; } /** Update the factor by adding the information from the JacobianFactor * (used internally during elimination). @@ -360,29 +349,42 @@ namespace gtsam { */ void updateATA(const HessianFactor& update, const Scatter& scatter); - /** assert invariants */ - void assertInvariants() const; + /** + * Densely partially eliminate with Cholesky factorization. JacobianFactors are + * left-multiplied with their transpose to form the Hessian using the conversion constructor + * HessianFactor(const JacobianFactor&). + * + * If any factors contain constrained noise models, this function will fail because our current + * implementation cannot handle constrained noise models in Cholesky factorization. The + * function EliminatePreferCholesky() automatically does QR instead when this is the case. + * + * Variables are eliminated in the order specified in \c keys. + * + * @param factors Factors to combine and eliminate + * @param keys The variables to eliminate and their elimination ordering + * @return The conditional and remaining factor + * + * \addtogroup LinearSolving */ + friend GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys); /** - * Densely partially eliminate with Cholesky factorization. JacobianFactors - * are left-multiplied with their transpose to form the Hessian using the - * conversion constructor HessianFactor(const JacobianFactor&). - * - * If any factors contain constrained noise models, this function will fail - * because our current implementation cannot handle constrained noise models - * in Cholesky factorization. The function EliminatePreferCholesky() - * automatically does QR instead when this is the case. - * - * Variables are eliminated in the natural order of the variable indices of in - * the factors. - * @param factors Factors to combine and eliminate - * @param nrFrontals Number of frontal variables to eliminate. - * @return The conditional and remaining factor - - * \addtogroup LinearSolving - */ + * Densely partially eliminate with Cholesky factorization. JacobianFactors are + * left-multiplied with their transpose to form the Hessian using the conversion constructor + * HessianFactor(const JacobianFactor&). + * + * This function will fall back on QR factorization for any cliques containing JacobianFactor's + * with constrained noise models. + * + * Variables are eliminated in the order specified in \c keys. + * + * @param factors Factors to combine and eliminate + * @param keys The variables to eliminate and their elimination ordering + * @return The conditional and remaining factor + * + * \addtogroup LinearSolving */ friend GTSAM_EXPORT std::pair, boost::shared_ptr > - EliminateCholeskyOrdered(const GaussianFactorGraph& factors, const Ordering& keys); + EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys); private: @@ -400,3 +402,4 @@ namespace gtsam { } +#include diff --git a/gtsam/linear/tests/testHessianFactorUnordered.cpp b/gtsam/linear/tests/testHessianFactorUnordered.cpp index 46f842373..c2d99026a 100644 --- a/gtsam/linear/tests/testHessianFactorUnordered.cpp +++ b/gtsam/linear/tests/testHessianFactorUnordered.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -37,7 +38,8 @@ using namespace gtsam; const double tol = 1e-5; /* ************************************************************************* */ -TEST(HessianFactor, emptyConstructor) { +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 @@ -46,68 +48,39 @@ TEST(HessianFactor, emptyConstructor) { } /* ************************************************************************* */ -TEST(HessianFactor, ConversionConstructor) { +TEST(HessianFactor, ConversionConstructor) +{ + HessianFactor expected(list_of(0)(1), + SymmetricBlockMatrix(list_of(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))); - HessianFactor expected; - expected.keys_.push_back(0); - expected.keys_.push_back(1); - size_t dims[] = { 2, 4, 1 }; - expected.info_.resize(dims, dims+3, false); - expected.matrix_ = 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); + JacobianFactor jacobian( + 0, Matrix_(4,2, -1., 0., + +0.,-1., + 1., 0., + +0.,1.), + 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.), // f2 + Vector_(4, -0.2, 0.3, 0.2, -0.1), + noiseModel::Diagonal::Sigmas(Vector_(4, 0.2, 0.2, 0.1, 0.1))); - // sigmas - double sigma1 = 0.2; - double sigma2 = 0.1; - Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); + HessianFactor actual(jacobian); - // the combined linear factor - Matrix Ax2 = Matrix_(4,2, - // x2 - -1., 0., - +0.,-1., - 1., 0., - +0.,1. - ); - - 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 - ); - - // the RHS - Vector b2(4); - b2(0) = -0.2; - b2(1) = 0.3; - b2(2) = 0.2; - b2(3) = -0.1; - - vector > meas; - meas.push_back(make_pair(0, Ax2)); - meas.push_back(make_pair(1, Al1x1)); - JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas)); - - HessianFactor actual(combined); - - VectorValues values(std::vector(dims, dims+2)); - values[0] = Vector_(2, 1.0, 2.0); - values[1] = Vector_(4, 3.0, 4.0, 5.0, 6.0); - - EXPECT_LONGS_EQUAL(2, actual.size()); + VectorValues values = pair_list_of + (0, Vector_(2, 1.0, 2.0)) + (1, Vector_(4, 3.0, 4.0, 5.0, 6.0)); + EXPECT_LONGS_EQUAL(2, (long)actual.size()); EXPECT(assert_equal(expected, actual, 1e-9)); - - // error terms - EXPECT_DOUBLES_EQUAL(combined.error(values), actual.error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(jacobian.error(values), actual.error(values), 1e-9); } /* ************************************************************************* */ @@ -117,29 +90,23 @@ TEST(HessianFactor, Constructor1) Vector g = Vector_(2, -8.0, -9.0); double f = 10.0; - Vector dxv = Vector_(2, 1.5, 2.5); - - vector dims; - dims.push_back(2); - VectorValues dx(dims); - - dx[0] = dxv; + VectorValues dx = pair_list_of + (0, Vector_(2, 1.5, 2.5)); HessianFactor factor(0, G, g, f); // extract underlying parts - Matrix info_matrix = factor.info_.range(0, 1, 0, 1); - EXPECT(assert_equal(Matrix(G), info_matrix)); + EXPECT(assert_equal(G, Matrix(factor.info(factor.begin(), factor.begin())))); EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10); - EXPECT(assert_equal(g, Vector(factor.linearTerm()), 1e-10)); - EXPECT_LONGS_EQUAL(1, factor.size()); + EXPECT(assert_equal(g, Vector(factor.linearTerm()))); + EXPECT_LONGS_EQUAL(1, (long)factor.size()); // 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 * dxv.dot(g) + dxv.transpose() * G.selfadjointView() * dxv); + double expected_manual = 0.5 * (f - 2.0 * dx[0].dot(g) + dx[0].transpose() * G.selfadjointView() * dx[0]); EXPECT_DOUBLES_EQUAL(expected, expected_manual, 1e-10); - DOUBLES_EQUAL(expected, actual, 1e-10); + EXPECT_DOUBLES_EQUAL(expected, actual, 1e-10); } /* ************************************************************************* */ @@ -155,11 +122,10 @@ TEST(HessianFactor, Constructor1b) double f = dot(g,mu); // Check - Matrix info_matrix = factor.info_.range(0, 1, 0, 1); - EXPECT(assert_equal(Matrix(G), info_matrix)); + EXPECT(assert_equal(G, Matrix(factor.info(factor.begin(), factor.begin())))); EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10); - EXPECT(assert_equal(g, Vector(factor.linearTerm()), 1e-10)); - EXPECT_LONGS_EQUAL(1, factor.size()); + EXPECT(assert_equal(g, Vector(factor.linearTerm()))); + EXPECT_LONGS_EQUAL(1, (long)factor.size()); } /* ************************************************************************* */ @@ -175,13 +141,9 @@ TEST(HessianFactor, Constructor2) Vector dx0 = Vector_(1, 0.5); Vector dx1 = Vector_(2, 1.5, 2.5); - vector dims; - dims.push_back(1); - dims.push_back(2); - VectorValues dx(dims); - - dx[0] = dx0; - dx[1] = dx1; + VectorValues dx = pair_list_of + (0, dx0) + (1, dx1); HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); @@ -189,7 +151,7 @@ TEST(HessianFactor, Constructor2) double actual = factor.error(dx); DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(4, factor.rows()); + LONGS_EQUAL(4, (long)factor.rows()); DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10); Vector linearExpected(3); linearExpected << g1, g2; @@ -200,11 +162,10 @@ TEST(HessianFactor, Constructor2) EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1))); // Check case when vector values is larger than factor - dims.push_back(2); - VectorValues dxLarge(dims); - dxLarge[0] = dx0; - dxLarge[1] = dx1; - dxLarge[2] = Vector_(2, 0.1, 0.2); + VectorValues dxLarge = pair_list_of + (0, dx0) + (1, dx1) + (2, Vector_(2, 0.1, 0.2)); EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10); } @@ -230,15 +191,10 @@ TEST(HessianFactor, Constructor3) Vector dx1 = Vector_(2, 1.5, 2.5); Vector dx2 = Vector_(3, 1.5, 2.5, 3.5); - vector dims; - dims.push_back(1); - dims.push_back(2); - dims.push_back(3); - VectorValues dx(dims); - - dx[0] = dx0; - dx[1] = dx1; - dx[2] = dx2; + VectorValues dx = pair_list_of + (0, dx0) + (1, dx1) + (2, dx2); HessianFactor factor(0, 1, 2, G11, G12, G13, g1, G22, G23, g2, G33, g3, f); @@ -246,7 +202,7 @@ TEST(HessianFactor, Constructor3) double actual = factor.error(dx); DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(7, factor.rows()); + LONGS_EQUAL(7, (long)factor.rows()); DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10); Vector linearExpected(6); linearExpected << g1, g2, g3; @@ -282,16 +238,10 @@ TEST(HessianFactor, ConstructorNWay) Vector dx1 = Vector_(2, 1.5, 2.5); Vector dx2 = Vector_(3, 1.5, 2.5, 3.5); - vector dims; - dims.push_back(1); - dims.push_back(2); - dims.push_back(3); - VectorValues dx(dims); - - dx[0] = dx0; - dx[1] = dx1; - dx[2] = dx2; - + VectorValues dx = pair_list_of + (0, dx0) + (1, dx1) + (2, dx2); std::vector js; js.push_back(0); js.push_back(1); js.push_back(2); @@ -305,7 +255,7 @@ TEST(HessianFactor, ConstructorNWay) double actual = factor.error(dx); DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(7, factor.rows()); + LONGS_EQUAL(7, (long)factor.rows()); DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10); Vector linearExpected(6); linearExpected << g1, g2, g3; @@ -320,63 +270,7 @@ TEST(HessianFactor, ConstructorNWay) } /* ************************************************************************* */ -TEST_UNSAFE(HessianFactor, CopyConstructor_and_assignment) -{ - Matrix G11 = Matrix_(1,1, 1.0); - Matrix G12 = Matrix_(1,2, 2.0, 4.0); - Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Vector g1 = Vector_(1, -7.0); - Vector g2 = Vector_(2, -8.0, -9.0); - double f = 10.0; - - Vector dx0 = Vector_(1, 0.5); - Vector dx1 = Vector_(2, 1.5, 2.5); - - vector dims; - dims.push_back(1); - dims.push_back(2); - VectorValues dx(dims); - - dx[0] = dx0; - dx[1] = dx1; - - HessianFactor originalFactor(0, 1, G11, G12, g1, G22, g2, f); - - // Make a copy - HessianFactor copy1(originalFactor); - - double expected = 90.5; - double actual = copy1.error(dx); - - DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(4, copy1.rows()); - DOUBLES_EQUAL(10.0, copy1.constantTerm(), 1e-10); - - Vector linearExpected(3); linearExpected << g1, g2; - EXPECT(assert_equal(linearExpected, copy1.linearTerm())); - - EXPECT(assert_equal(G11, copy1.info(copy1.begin(), copy1.begin()))); - EXPECT(assert_equal(G12, copy1.info(copy1.begin(), copy1.begin()+1))); - EXPECT(assert_equal(G22, copy1.info(copy1.begin()+1, copy1.begin()+1))); - - // Make a copy using the assignment operator - HessianFactor copy2; - copy2 = HessianFactor(originalFactor); // Make a temporary to make sure copying does not shared references - - actual = copy2.error(dx); - DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(4, copy2.rows()); - DOUBLES_EQUAL(10.0, copy2.constantTerm(), 1e-10); - - EXPECT(assert_equal(linearExpected, copy2.linearTerm())); - - EXPECT(assert_equal(G11, copy2.info(copy2.begin(), copy2.begin()))); - EXPECT(assert_equal(G12, copy2.info(copy2.begin(), copy2.begin()+1))); - EXPECT(assert_equal(G22, copy2.info(copy2.begin()+1, copy2.begin()+1))); -} - -/* ************************************************************************* */ -TEST_UNSAFE(HessianFactor, CombineAndEliminate) +TEST(HessianFactor, CombineAndEliminate) { Matrix A01 = Matrix_(3,3, 1.0, 0.0, 0.0, @@ -417,29 +311,18 @@ TEST_UNSAFE(HessianFactor, CombineAndEliminate) // 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 hessians; - BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) { - if(boost::shared_ptr hf = boost::dynamic_pointer_cast(factor)) - hessians.push_back(hf); - else if(boost::shared_ptr jf = boost::dynamic_pointer_cast(factor)) - hessians.push_back(boost::make_shared(*jf)); - else - CHECK(false); - } + // perform elimination on jacobian + GaussianConditional::shared_ptr expectedConditional; + JacobianFactor::shared_ptr expectedRemainingFactor; + boost::tie(expectedConditional, expectedRemainingFactor) = expectedFactor.eliminate(Ordering(list_of(0))); // Eliminate - GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(gfg, 1); - HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast(actualCholesky.second); + GaussianConditional::shared_ptr actualConditional; + HessianFactor::shared_ptr actualCholeskyFactor; + boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0))); - EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6)); - EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6)); + EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); + EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6)); } /* ************************************************************************* */ @@ -481,13 +364,10 @@ TEST(HessianFactor, eliminate2 ) // eliminate the combined factor HessianFactor::shared_ptr combinedLF_Chol(new HessianFactor(combined)); - FactorGraph combinedLFG_Chol; - combinedLFG_Chol.push_back(combinedLF_Chol); + GaussianFactorGraph combinedLFG_Chol = list_of(combinedLF_Chol); - GaussianFactorGraph::EliminationResult actual_Chol = EliminateCholesky( - combinedLFG_Chol, 1); - HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast< - HessianFactor>(actual_Chol.second); + std::pair actual_Chol = + EliminateCholesky(combinedLFG_Chol, Ordering(list_of(0))); // create expected Conditional Gaussian double oldSigma = 0.0894427; // from when R was made unit @@ -500,8 +380,8 @@ TEST(HessianFactor, eliminate2 ) +0.00,-0.20,+0.00,-0.80 )/oldSigma; Vector d = Vector_(2,0.2,-0.14)/oldSigma; - GaussianConditional expectedCG(0,d,R11,1,S12,ones(2)); - EXPECT(assert_equal(expectedCG,*actual_Chol.first,1e-4)); + GaussianConditional expectedCG(0, d, R11, 1, S12); + EXPECT(assert_equal(expectedCG, *actual_Chol.first, 1e-4)); // the expected linear factor double sigma = 0.2236; @@ -512,89 +392,7 @@ TEST(HessianFactor, eliminate2 ) )/sigma; Vector b1 = Vector_(2,0.0,0.894427); JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0)); - EXPECT(assert_equal(HessianFactor(expectedLF), *actualFactor, 1.5e-3)); -} - -/* ************************************************************************* */ -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)); + EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3)); } /* ************************************************************************* */ @@ -613,8 +411,7 @@ TEST(HessianFactor, combine) { 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 factors; - factors.push_back(f); + GaussianFactorGraph factors = list_of(f); // Form Ab' * Ab HessianFactor actual(factors); @@ -628,7 +425,7 @@ TEST(HessianFactor, combine) { 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()), - Matrix(actual.matrix_.triangularView()), tol)); + Matrix(actual.matrixObject().full().triangularView()), tol)); }