From 4ea9fda03bacc1ac1b43c1dad19e0234a17bdac9 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 1 Aug 2013 21:57:56 +0000 Subject: [PATCH] Working on HessianFactor --- gtsam/linear/HessianFactor.cpp | 346 +++++++----------- gtsam/linear/JacobianFactor.h | 19 +- .../tests/testHessianFactorUnordered.cpp | 137 ++++--- 3 files changed, 205 insertions(+), 297 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index cc6ded573..bd1b943bb 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -40,14 +40,15 @@ #endif #include #include +#include #include +#include #include using namespace std; using namespace boost::assign; -using boost::adaptors::transformed; -namespace br = boost::range; +namespace br { using namespace boost::range; using namespace boost::adaptors; } namespace gtsam { @@ -154,7 +155,7 @@ namespace { DenseIndex _getColsHF(const Matrix& m) { return m.cols(); } } /* ************************************************************************* */ HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, const std::vector& gs, double f) : -GaussianFactor(js), info_(br::join(Gs | transformed(&_getColsHF), cref_list_of<1,DenseIndex>(1))) +GaussianFactor(js), info_(br::join(Gs | br::transformed(&_getColsHF), cref_list_of<1,DenseIndex>(1))) { // Get the number of variables size_t variable_count = js.size(); @@ -196,82 +197,86 @@ GaussianFactor(js), info_(br::join(Gs | transformed(&_getColsHF), cref_list_of<1 } /* ************************************************************************* */ -HessianFactor::HessianFactor(const JacobianFactor& jf) -{ - throw std::runtime_error("Not implemented"); +namespace { + void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) + { + const SharedDiagonal& jfModel = jf.get_model(); + if(jfModel) + { + 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() * + jfModel->invsigmas().asDiagonal() * jf.matrixObject().full(); + } else { + info.full() = jf.matrixObject().full().transpose() * jf.matrixObject().full(); + } + } } /* ************************************************************************* */ -HessianFactor::HessianFactor(const GaussianFactor& gf) +HessianFactor::HessianFactor(const JacobianFactor& jf) : + GaussianFactor(jf), info_(SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) { - throw std::runtime_error("Not implemented"); - //// Copy the variable indices - //(GaussianFactorOrdered&)(*this) = gf; - //// Copy the matrix data depending on what type of factor we're copying from - //if(dynamic_cast(&gf)) { - // const JacobianFactorOrdered& jf(static_cast(gf)); - // if(jf.model_->isConstrained()) - // throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); - // else { - // Vector invsigmas = jf.model_->invsigmas().cwiseProduct(jf.model_->invsigmas()); - // info_.copyStructureFrom(jf.Ab_); - // BlockInfo::constBlock A = jf.Ab_.full(); - // matrix_.noalias() = A.transpose() * invsigmas.asDiagonal() * A; - // } - //} else if(dynamic_cast(&gf)) { - // const HessianFactorOrdered& hf(static_cast(gf)); - // info_.assignNoalias(hf.info_); - //} else - // throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); - //assertInvariants(); + _FromJacobianHelper(jf, info_); +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(const GaussianFactor& gf) : + GaussianFactor(gf) +{ + // Copy the matrix data depending on what type of factor we're copying from + if(const JacobianFactor* jf = dynamic_cast(&gf)) + { + info_ = SymmetricBlockMatrix::LikeActiveViewOf(jf->matrixObject()); + _FromJacobianHelper(*jf, info_); + } + else if(const HessianFactor* hf = dynamic_cast(&gf)) + { + info_ = hf->info_; + } + else + { + throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); + } } /* ************************************************************************* */ HessianFactor::HessianFactor(const GaussianFactorGraph& factors) { - throw std::runtime_error("Not implemented"); + Scatter scatter(factors); - //Scatter scatter(factors); + // 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; + } + // This is for the r.h.s. vector + dimensions.back() = 1; + gttoc(keys); - //// 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; - //} - //// This is for the r.h.s. vector - //dimensions.back() = 1; - //gttoc(keys); + // Allocate and copy keys + gttic(allocate); + info_ = SymmetricBlockMatrix(dimensions); + info_.full().setZero(); + keys_.resize(scatter.size()); + br::copy(scatter | br::map_keys, keys_.begin()); + gttoc(allocate); - //const bool debug = ISDEBUG("EliminateCholesky"); - //// Form Ab' * Ab - //gttic(allocate); - //info_.resize(dimensions.begin(), dimensions.end(), false); - //// Fill in keys - //keys_.resize(scatter.size()); - //std::transform(scatter.begin(), scatter.end(), keys_.begin(), boost::bind(&Scatter::value_type::first, ::_1)); - //gttoc(allocate); - //gttic(zero); - //matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols()); - //gttoc(zero); - //gttic(update); - //if (debug) cout << "Combining " << factors.size() << " factors" << endl; - //BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factors) - //{ - // if(factor) { - // if(shared_ptr hessian = boost::dynamic_pointer_cast(factor)) - // updateATA(*hessian, scatter); - // else if(JacobianFactorOrdered::shared_ptr jacobianFactor = boost::dynamic_pointer_cast(factor)) - // updateATA(*jacobianFactor, scatter); - // else - // throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); - // } - //} - //gttoc(update); - - //if (debug) gtsam::print(matrix_, "Ab' * Ab: "); - - //assertInvariants(); + // Form A' * A + gttic(update); + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) + { + if(factor) { + if(const HessianFactor* hessian = dynamic_cast(factor.get())) + updateATA(*hessian, scatter); + else if(const JacobianFactor* jacobian = dynamic_cast(factor.get())) + updateATA(*jacobian, scatter); + else + throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); + } + } + gttoc(update); } /* ************************************************************************* */ @@ -298,25 +303,27 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { } /* ************************************************************************* */ -Matrix HessianFactor::augmentedInformation() const { +Matrix HessianFactor::augmentedInformation() const +{ return info_.full().selfadjointView(); } /* ************************************************************************* */ -Matrix HessianFactor::information() const { +Matrix HessianFactor::information() const +{ return info_.range(0, this->size(), 0, this->size()).selfadjointView(); } /* ************************************************************************* */ Matrix HessianFactor::augmentedJacobian() const { - throw std::runtime_error("Not implemented"); + return JacobianFactor(*this).augmentedJacobian(); } /* ************************************************************************* */ std::pair HessianFactor::jacobian() const { - throw std::runtime_error("Not implemented"); + return JacobianFactor(*this).jacobian(); } /* ************************************************************************* */ @@ -335,136 +342,41 @@ double HessianFactor::error(const VectorValues& c) const { /* ************************************************************************* */ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) { + // This function updates 'combined' with the information in 'update'. 'scatter' maps variables in + // the update factor to slots in the combined factor. - throw std::runtime_error("Not implemented"); - - //// This function updates 'combined' with the information in 'update'. - //// 'scatter' maps variables in the update factor to slots in the combined - //// factor. - - //const bool debug = ISDEBUG("updateATA"); - - //// First build an array of slots - //gttic(slots); + // 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. - //size_t slot = 0; - //BOOST_FOREACH(Index j, update) { - // slots[slot] = scatter.find(j)->second.slot; - // ++ slot; - //} - //gttoc(slots); + vector slots(update.size()); + size_t slot = 0; + BOOST_FOREACH(Index j, update) { + slots[slot] = scatter.find(j)->second.slot; + ++ slot; + } + gttoc(slots); - //if(debug) { - // this->print("Updating this: "); - // update.print("with (Hessian): "); - //} - - //// 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]; - // if(slot2 > slot1) { - // if(debug) - // cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; - // matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() += - // update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()); - // } else if(slot1 > slot2) { - // if(debug) - // cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl; - // matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() += - // update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()).transpose(); - // } else { - // if(debug) - // cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; - // matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView() += - // update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()); - // } - // if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n"; - // if(debug) this->print(); - // } - //} - //gttoc(update); + // 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]; + if(slot2 > slot1) + info_(slot1, slot2).noalias() += update.info_(j1, j2); + else if(slot1 > slot2) + info_(slot2, slot1).noalias() += update.info_(j1, j2).transpose(); + else + info_(slot1, slot2).triangularView() += update.info_(j1, j2); + } + } + gttoc(update); } /* ************************************************************************* */ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter) { - throw std::runtime_error("Not implemented"); - - //// This function updates 'combined' with the information in 'update'. - //// 'scatter' maps variables in the update factor to slots in the combined - //// factor. - - //const bool debug = ISDEBUG("updateATA"); - - //// 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. - //size_t slot = 0; - //BOOST_FOREACH(Index j, update) { - // slots[slot] = scatter.find(j)->second.slot; - // ++ slot; - //} - //gttoc(slots); - - //gttic(form_ATA); - //if(update.model_->isConstrained()) - // throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model"); - - //if(debug) { - // this->print("Updating this: "); - // update.print("with (Jacobian): "); - //} - - //typedef Eigen::Block BlockUpdateMatrix; - //BlockUpdateMatrix updateA(update.matrix_.block( - // update.Ab_.rowStart(),update.Ab_.offset(0), update.Ab_.full().rows(), update.Ab_.full().cols())); - //if (debug) cout << "updateA: \n" << updateA << endl; - - //Matrix updateInform; - //if(boost::dynamic_pointer_cast(update.model_)) { - // updateInform.noalias() = updateA.transpose() * updateA; - //} else { - // noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast(update.model_)); - // if(diagonal) { - // Vector invsigmas2 = update.model_->invsigmas().cwiseProduct(update.model_->invsigmas()); - // updateInform.noalias() = updateA.transpose() * invsigmas2.asDiagonal() * updateA; - // } else - // throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal"); - //} - //if (debug) cout << "updateInform: \n" << updateInform << endl; - // gttoc(form_ATA); - - //// 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]; - // size_t off0 = update.Ab_.offset(0); - // if(slot2 > slot1) { - // if(debug) - // cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; - // matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() += - // updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()); - // } else if(slot1 > slot2) { - // if(debug) - // cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl; - // matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() += - // updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()).transpose(); - // } else { - // if(debug) - // cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; - // matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView() += - // updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()); - // } - // if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n"; - // if(debug) this->print(); - // } - //} - //gttoc(update); + updateATA(HessianFactor(update), scatter); } /* ************************************************************************* */ @@ -479,38 +391,36 @@ void HessianFactor::partialCholesky(size_t nrFrontals) /* ************************************************************************* */ GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFrontals) { - throw std::runtime_error("Not implemented"); + static const bool debug = false; - //static const bool debug = false; + // Extract conditionals + gttic(extract_conditionals); + typedef VerticalBlockView BlockAb; + BlockAb Ab(matrix_, info_); - //// Extract conditionals - //gttic(extract_conditionals); - //GaussianConditionalOrdered::shared_ptr conditional(new GaussianConditionalOrdered()); - //typedef VerticalBlockView BlockAb; - //BlockAb Ab(matrix_, info_); + size_t varDim = info_.offset(nrFrontals); + Ab.rowEnd() = Ab.rowStart() + varDim; - //size_t varDim = info_.offset(nrFrontals); - //Ab.rowEnd() = Ab.rowStart() + varDim; + // Create one big conditionals with many frontal variables. + gttic(construct_cond); - //// Create one big conditionals with many frontal variables. - //gttic(construct_cond); - //Vector sigmas = Vector::Ones(varDim); - //conditional = boost::make_shared(keys_.begin(), keys_.end(), nrFrontals, Ab, sigmas); - //gttoc(construct_cond); - //if(debug) conditional->print("Extracted conditional: "); + VerticalBlockMatrix Ab() + GaussianConditional::shared_ptr conditional = boost::make_shared( + keys_, nrFrontals, Ab); + gttoc(construct_cond); - //gttoc(extract_conditionals); + gttoc(extract_conditionals); - //// Take lower-right block of Ab_ to get the new factor - //gttic(remaining_factor); - //info_.blockStart() = nrFrontals; - //// Assign the keys - //vector remainingKeys(keys_.size() - nrFrontals); - //remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end()); - //keys_.swap(remainingKeys); - //gttoc(remaining_factor); + // Take lower-right block of Ab_ to get the new factor + gttic(remaining_factor); + info_.blockStart() = nrFrontals; + // Assign the keys + vector remainingKeys(keys_.size() - nrFrontals); + remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end()); + keys_.swap(remainingKeys); + gttoc(remaining_factor); - //return conditional; + return conditional; } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index cadcfe093..e0c387e97 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -187,20 +187,19 @@ namespace gtsam { */ std::pair jacobianUnweighted() const; - /** - * Return (dense) matrix associated with factor - * The returned system is an augmented matrix: [A b] - * @param set weight to use whitening to bake in weights - */ + /** Return (dense) matrix associated with factor. The returned system is an augmented matrix: + * [A b] + * @param set weight to use whitening to bake in weights*/ virtual Matrix augmentedJacobian() const; - /** - * Return (dense) matrix associated with factor - * The returned system is an augmented matrix: [A b] - * @param set weight to use whitening to bake in weights - */ + /** Return (dense) matrix associated with factor. The returned system is an augmented matrix: + * [A b] + * @param set weight to use whitening to bake in weights */ Matrix augmentedJacobianUnweighted() const; + /** Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ + const VerticalBlockMatrix& matrixObject() const { return Ab_; } + /** * Construct the corresponding anti-factor to negate information * stored stored in this factor. diff --git a/gtsam/linear/tests/testHessianFactorUnordered.cpp b/gtsam/linear/tests/testHessianFactorUnordered.cpp index f038624c6..46f842373 100644 --- a/gtsam/linear/tests/testHessianFactorUnordered.cpp +++ b/gtsam/linear/tests/testHessianFactorUnordered.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -35,20 +36,19 @@ using namespace gtsam; const double tol = 1e-5; -#if 0 /* ************************************************************************* */ 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(zeros(1,1), f.info())); // Full matrix should be 1-by-1 zero matrix - DOUBLES_EQUAL(0.0, f.error(VectorValuesOrdered()), 1e-9); // Should have zero error + DOUBLES_EQUAL(0.0, f.error(VectorValues()), 1e-9); // Should have zero error } /* ************************************************************************* */ TEST(HessianFactor, ConversionConstructor) { - HessianFactorOrdered expected; + HessianFactor expected; expected.keys_.push_back(0); expected.keys_.push_back(1); size_t dims[] = { 2, 4, 1 }; @@ -94,11 +94,11 @@ TEST(HessianFactor, ConversionConstructor) { vector > meas; meas.push_back(make_pair(0, Ax2)); meas.push_back(make_pair(1, Al1x1)); - JacobianFactorOrdered combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas)); + JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas)); - HessianFactorOrdered actual(combined); + HessianFactor actual(combined); - VectorValuesOrdered values(std::vector(dims, dims+2)); + 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); @@ -111,7 +111,7 @@ TEST(HessianFactor, ConversionConstructor) { } /* ************************************************************************* */ -TEST(HessianFactorOrdered, Constructor1) +TEST(HessianFactor, Constructor1) { Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); Vector g = Vector_(2, -8.0, -9.0); @@ -121,11 +121,11 @@ TEST(HessianFactorOrdered, Constructor1) vector dims; dims.push_back(2); - VectorValuesOrdered dx(dims); + VectorValues dx(dims); dx[0] = dxv; - HessianFactorOrdered factor(0, G, g, f); + HessianFactor factor(0, G, g, f); // extract underlying parts Matrix info_matrix = factor.info_.range(0, 1, 0, 1); @@ -143,12 +143,12 @@ TEST(HessianFactorOrdered, Constructor1) } /* ************************************************************************* */ -TEST(HessianFactorOrdered, Constructor1b) +TEST(HessianFactor, Constructor1b) { Vector mu = Vector_(2,1.0,2.0); Matrix Sigma = eye(2,2); - HessianFactorOrdered factor(0, mu, Sigma); + HessianFactor factor(0, mu, Sigma); Matrix G = eye(2,2); Vector g = G*mu; @@ -163,7 +163,7 @@ TEST(HessianFactorOrdered, Constructor1b) } /* ************************************************************************* */ -TEST(HessianFactorOrdered, Constructor2) +TEST(HessianFactor, Constructor2) { Matrix G11 = Matrix_(1,1, 1.0); Matrix G12 = Matrix_(1,2, 2.0, 4.0); @@ -178,12 +178,12 @@ TEST(HessianFactorOrdered, Constructor2) vector dims; dims.push_back(1); dims.push_back(2); - VectorValuesOrdered dx(dims); + VectorValues dx(dims); dx[0] = dx0; dx[1] = dx1; - HessianFactorOrdered factor(0, 1, G11, G12, g1, G22, g2, f); + HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); double expected = 90.5; double actual = factor.error(dx); @@ -201,7 +201,7 @@ TEST(HessianFactorOrdered, Constructor2) // Check case when vector values is larger than factor dims.push_back(2); - VectorValuesOrdered dxLarge(dims); + VectorValues dxLarge(dims); dxLarge[0] = dx0; dxLarge[1] = dx1; dxLarge[2] = Vector_(2, 0.1, 0.2); @@ -209,7 +209,7 @@ TEST(HessianFactorOrdered, Constructor2) } /* ************************************************************************* */ -TEST(HessianFactorOrdered, Constructor3) +TEST(HessianFactor, Constructor3) { Matrix G11 = Matrix_(1,1, 1.0); Matrix G12 = Matrix_(1,2, 2.0, 4.0); @@ -234,13 +234,13 @@ TEST(HessianFactorOrdered, Constructor3) dims.push_back(1); dims.push_back(2); dims.push_back(3); - VectorValuesOrdered dx(dims); + VectorValues dx(dims); dx[0] = dx0; dx[1] = dx1; dx[2] = dx2; - HessianFactorOrdered factor(0, 1, 2, G11, G12, G13, g1, G22, G23, g2, G33, g3, f); + HessianFactor factor(0, 1, 2, G11, G12, G13, g1, G22, G23, g2, G33, g3, f); double expected = 371.3750; double actual = factor.error(dx); @@ -261,7 +261,7 @@ TEST(HessianFactorOrdered, Constructor3) } /* ************************************************************************* */ -TEST(HessianFactorOrdered, ConstructorNWay) +TEST(HessianFactor, ConstructorNWay) { Matrix G11 = Matrix_(1,1, 1.0); Matrix G12 = Matrix_(1,2, 2.0, 4.0); @@ -286,7 +286,7 @@ TEST(HessianFactorOrdered, ConstructorNWay) dims.push_back(1); dims.push_back(2); dims.push_back(3); - VectorValuesOrdered dx(dims); + VectorValues dx(dims); dx[0] = dx0; dx[1] = dx1; @@ -299,7 +299,7 @@ TEST(HessianFactorOrdered, ConstructorNWay) 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 gs; gs.push_back(g1); gs.push_back(g2); gs.push_back(g3); - HessianFactorOrdered factor(js, Gs, gs, f); + HessianFactor factor(js, Gs, gs, f); double expected = 371.3750; double actual = factor.error(dx); @@ -320,7 +320,7 @@ TEST(HessianFactorOrdered, ConstructorNWay) } /* ************************************************************************* */ -TEST_UNSAFE(HessianFactorOrdered, CopyConstructor_and_assignment) +TEST_UNSAFE(HessianFactor, CopyConstructor_and_assignment) { Matrix G11 = Matrix_(1,1, 1.0); Matrix G12 = Matrix_(1,2, 2.0, 4.0); @@ -335,15 +335,15 @@ TEST_UNSAFE(HessianFactorOrdered, CopyConstructor_and_assignment) vector dims; dims.push_back(1); dims.push_back(2); - VectorValuesOrdered dx(dims); + VectorValues dx(dims); dx[0] = dx0; dx[1] = dx1; - HessianFactorOrdered originalFactor(0, 1, G11, G12, g1, G22, g2, f); + HessianFactor originalFactor(0, 1, G11, G12, g1, G22, g2, f); // Make a copy - HessianFactorOrdered copy1(originalFactor); + HessianFactor copy1(originalFactor); double expected = 90.5; double actual = copy1.error(dx); @@ -360,8 +360,8 @@ TEST_UNSAFE(HessianFactorOrdered, CopyConstructor_and_assignment) EXPECT(assert_equal(G22, copy1.info(copy1.begin()+1, copy1.begin()+1))); // Make a copy using the assignment operator - HessianFactorOrdered copy2; - copy2 = HessianFactorOrdered(originalFactor); // Make a temporary to make sure copying does not shared references + 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); @@ -376,7 +376,7 @@ TEST_UNSAFE(HessianFactorOrdered, CopyConstructor_and_assignment) } /* ************************************************************************* */ -TEST_UNSAFE(HessianFactorOrdered, CombineAndEliminate) +TEST_UNSAFE(HessianFactor, CombineAndEliminate) { Matrix A01 = Matrix_(3,3, 1.0, 0.0, 0.0, @@ -403,7 +403,7 @@ TEST_UNSAFE(HessianFactorOrdered, CombineAndEliminate) Vector b2 = Vector_(3, 3.5, 3.5, 3.5); Vector s2 = Vector_(3, 3.6, 3.6, 3.6); - GaussianFactorGraphOrdered gfg; + 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)); @@ -415,35 +415,35 @@ TEST_UNSAFE(HessianFactorOrdered, CombineAndEliminate) Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); // create a full, uneliminated version of the factor - JacobianFactorOrdered expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); + JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); // perform elimination - GaussianConditionalOrdered::shared_ptr expectedBN = expectedFactor.eliminate(1); + GaussianConditional::shared_ptr expectedBN = expectedFactor.eliminate(1); // create expected Hessian after elimination - HessianFactorOrdered expectedCholeskyFactor(expectedFactor); + HessianFactor expectedCholeskyFactor(expectedFactor); // Convert all factors to hessians - FactorGraphOrdered hessians; - BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, gfg) { - if(boost::shared_ptr hf = boost::dynamic_pointer_cast(factor)) + 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 if(boost::shared_ptr jf = boost::dynamic_pointer_cast(factor)) + hessians.push_back(boost::make_shared(*jf)); else CHECK(false); } // Eliminate - GaussianFactorGraphOrdered::EliminationResult actualCholesky = EliminateCholeskyOrdered(gfg, 1); - HessianFactorOrdered::shared_ptr actualFactor = boost::dynamic_pointer_cast(actualCholesky.second); + GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(gfg, 1); + HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast(actualCholesky.second); EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6)); EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6)); } /* ************************************************************************* */ -TEST(HessianFactorOrdered, eliminate2 ) +TEST(HessianFactor, eliminate2 ) { // sigmas double sigma1 = 0.2; @@ -477,17 +477,17 @@ TEST(HessianFactorOrdered, eliminate2 ) vector > meas; meas.push_back(make_pair(0, Ax2)); meas.push_back(make_pair(1, Al1x1)); - JacobianFactorOrdered combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas)); + JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas)); // eliminate the combined factor - HessianFactorOrdered::shared_ptr combinedLF_Chol(new HessianFactorOrdered(combined)); - FactorGraphOrdered combinedLFG_Chol; + HessianFactor::shared_ptr combinedLF_Chol(new HessianFactor(combined)); + FactorGraph combinedLFG_Chol; combinedLFG_Chol.push_back(combinedLF_Chol); - GaussianFactorGraphOrdered::EliminationResult actual_Chol = EliminateCholeskyOrdered( + GaussianFactorGraph::EliminationResult actual_Chol = EliminateCholesky( combinedLFG_Chol, 1); - HessianFactorOrdered::shared_ptr actualFactor = boost::dynamic_pointer_cast< - HessianFactorOrdered>(actual_Chol.second); + HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast< + HessianFactor>(actual_Chol.second); // create expected Conditional Gaussian double oldSigma = 0.0894427; // from when R was made unit @@ -500,7 +500,7 @@ TEST(HessianFactorOrdered, eliminate2 ) +0.00,-0.20,+0.00,-0.80 )/oldSigma; Vector d = Vector_(2,0.2,-0.14)/oldSigma; - GaussianConditionalOrdered expectedCG(0,d,R11,1,S12,ones(2)); + GaussianConditional expectedCG(0,d,R11,1,S12,ones(2)); EXPECT(assert_equal(expectedCG,*actual_Chol.first,1e-4)); // the expected linear factor @@ -511,15 +511,15 @@ TEST(HessianFactorOrdered, eliminate2 ) 0.00, 1.00, +0.00, -1.00 )/sigma; Vector b1 = Vector_(2,0.0,0.894427); - JacobianFactorOrdered expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0)); - EXPECT(assert_equal(HessianFactorOrdered(expectedLF), *actualFactor, 1.5e-3)); + JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0)); + EXPECT(assert_equal(HessianFactor(expectedLF), *actualFactor, 1.5e-3)); } /* ************************************************************************* */ -TEST(HessianFactorOrdered, eliminateUnsorted) { +TEST(HessianFactor, eliminateUnsorted) { - JacobianFactorOrdered::shared_ptr factor1( - new JacobianFactorOrdered(0, + JacobianFactor::shared_ptr factor1( + new JacobianFactor(0, Matrix_(3,3, 44.7214, 0.0, 0.0, 0.0, 44.7214, 0.0, @@ -531,8 +531,8 @@ TEST(HessianFactorOrdered, eliminateUnsorted) { 0.0, 0.0, -44.7214), Vector_(3, 1.98916e-17, -4.96503e-15, -7.75792e-17), noiseModel::Unit::Create(3))); - HessianFactorOrdered::shared_ptr unsorted_factor2( - new HessianFactorOrdered(JacobianFactorOrdered(0, + HessianFactor::shared_ptr unsorted_factor2( + new HessianFactor(JacobianFactor(0, Matrix_(6,3, 25.8367, 0.1211, 0.0593, 0.0, 23.4099, 30.8733, @@ -555,8 +555,8 @@ TEST(HessianFactorOrdered, eliminateUnsorted) { permutation[1] = 0; unsorted_factor2->permuteWithInverse(permutation); - HessianFactorOrdered::shared_ptr sorted_factor2( - new HessianFactorOrdered(JacobianFactorOrdered(0, + HessianFactor::shared_ptr sorted_factor2( + new HessianFactor(JacobianFactor(0, Matrix_(6,3, 25.7429, -1.6897, 0.4587, 1.6400, 23.3095, -8.4816, @@ -575,30 +575,30 @@ TEST(HessianFactorOrdered, eliminateUnsorted) { Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), noiseModel::Unit::Create(6)))); - GaussianFactorGraphOrdered sortedGraph; + GaussianFactorGraph sortedGraph; // sortedGraph.push_back(factor1); sortedGraph.push_back(sorted_factor2); - GaussianFactorGraphOrdered unsortedGraph; + GaussianFactorGraph unsortedGraph; // unsortedGraph.push_back(factor1); unsortedGraph.push_back(unsorted_factor2); - GaussianConditionalOrdered::shared_ptr expected_bn; - GaussianFactorOrdered::shared_ptr expected_factor; + GaussianConditional::shared_ptr expected_bn; + GaussianFactor::shared_ptr expected_factor; boost::tie(expected_bn, expected_factor) = - EliminatePreferCholeskyOrdered(sortedGraph, 1); + EliminatePreferCholesky(sortedGraph, 1); - GaussianConditionalOrdered::shared_ptr actual_bn; - GaussianFactorOrdered::shared_ptr actual_factor; + GaussianConditional::shared_ptr actual_bn; + GaussianFactor::shared_ptr actual_factor; boost::tie(actual_bn, actual_factor) = - EliminatePreferCholeskyOrdered(unsortedGraph, 1); + EliminatePreferCholesky(unsortedGraph, 1); EXPECT(assert_equal(*expected_bn, *actual_bn, 1e-10)); EXPECT(assert_equal(*expected_factor, *actual_factor, 1e-10)); } /* ************************************************************************* */ -TEST(HessianFactorOrdered, combine) { +TEST(HessianFactor, combine) { // update the information matrix with a single jacobian factor Matrix A0 = Matrix_(2, 2, @@ -612,12 +612,12 @@ TEST(HessianFactorOrdered, combine) { 0.0, -8.94427191); Vector b = Vector_(2, 2.23606798,-1.56524758); SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2)); - GaussianFactorOrdered::shared_ptr f(new JacobianFactorOrdered(0, A0, 1, A1, 2, A2, b, model)); - FactorGraphOrdered factors; + GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model)); + FactorGraph factors; factors.push_back(f); // Form Ab' * Ab - HessianFactorOrdered actual(factors); + HessianFactor actual(factors); Matrix expected = Matrix_(7, 7, 125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000, @@ -631,7 +631,6 @@ TEST(HessianFactorOrdered, combine) { Matrix(actual.matrix_.triangularView()), tol)); } -#endif /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);}