12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Working on HessianFactor

release/4.3a0
Richard Roberts 2013-08-01 21:57:56 +00:00
parent af8f302402
commit 4ea9fda03b
3 changed files with 205 additions and 297 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

346
gtsam/linear/HessianFactor.cpp
View File

@ -40,14 +40,15 @@
#endif #endif
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/transformed.hpp> #include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/join.hpp> #include <boost/range/join.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <sstream> #include <sstream>
using namespace std; using namespace std;
using namespace boost::assign; using namespace boost::assign;
using boost::adaptors::transformed; namespace br { using namespace boost::range; using namespace boost::adaptors; }
namespace br = boost::range;
namespace gtsam { namespace gtsam {
@ -154,7 +155,7 @@ namespace { DenseIndex _getColsHF(const Matrix& m) { return m.cols(); } }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs, HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs, double f) : const std::vector<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 // Get the number of variables
size_t variable_count = js.size(); 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) namespace {
{ void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info)
throw std::runtime_error("Not implemented"); {
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"); _FromJacobianHelper(jf, info_);
//// Copy the variable indices }
//(GaussianFactorOrdered&)(*this) = gf;
//// Copy the matrix data depending on what type of factor we're copying from /* ************************************************************************* */
//if(dynamic_cast<const JacobianFactorOrdered*>(&gf)) { HessianFactor::HessianFactor(const GaussianFactor& gf) :
// const JacobianFactorOrdered& jf(static_cast<const JacobianFactorOrdered&>(gf)); GaussianFactor(gf)
// if(jf.model_->isConstrained()) {
// throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); // Copy the matrix data depending on what type of factor we're copying from
// else { if(const JacobianFactor* jf = dynamic_cast<const JacobianFactor*>(&gf))
// Vector invsigmas = jf.model_->invsigmas().cwiseProduct(jf.model_->invsigmas()); {
// info_.copyStructureFrom(jf.Ab_); info_ = SymmetricBlockMatrix::LikeActiveViewOf(jf->matrixObject());
// BlockInfo::constBlock A = jf.Ab_.full(); _FromJacobianHelper(*jf, info_);
// matrix_.noalias() = A.transpose() * invsigmas.asDiagonal() * A; }
// } else if(const HessianFactor* hf = dynamic_cast<const HessianFactor*>(&gf))
//} else if(dynamic_cast<const HessianFactorOrdered*>(&gf)) { {
// const HessianFactorOrdered& hf(static_cast<const HessianFactorOrdered&>(gf)); info_ = hf->info_;
// info_.assignNoalias(hf.info_); }
//} else else
// throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); {
//assertInvariants(); throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor");
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianFactorGraph& factors) 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<size_t> 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 // Allocate and copy keys
//gttic(keys); gttic(allocate);
//vector<size_t> dimensions(scatter.size() + 1); info_ = SymmetricBlockMatrix(dimensions);
//BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { info_.full().setZero();
// dimensions[var_slot.second.slot] = var_slot.second.dimension; keys_.resize(scatter.size());
//} br::copy(scatter | br::map_keys, keys_.begin());
//// This is for the r.h.s. vector gttoc(allocate);
//dimensions.back() = 1;
//gttoc(keys);
//const bool debug = ISDEBUG("EliminateCholesky"); // Form A' * A
//// Form Ab' * Ab gttic(update);
//gttic(allocate); BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
//info_.resize(dimensions.begin(), dimensions.end(), false); {
//// Fill in keys if(factor) {
//keys_.resize(scatter.size()); if(const HessianFactor* hessian = dynamic_cast<const HessianFactor*>(factor.get()))
//std::transform(scatter.begin(), scatter.end(), keys_.begin(), boost::bind(&Scatter::value_type::first, ::_1)); updateATA(*hessian, scatter);
//gttoc(allocate); else if(const JacobianFactor* jacobian = dynamic_cast<const JacobianFactor*>(factor.get()))
//gttic(zero); updateATA(*jacobian, scatter);
//matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols()); else
//gttoc(zero); throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
//gttic(update); }
//if (debug) cout << "Combining " << factors.size() << " factors" << endl; }
//BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factors) gttoc(update);
//{
// if(factor) {
// if(shared_ptr hessian = boost::dynamic_pointer_cast<HessianFactorOrdered>(factor))
// updateATA(*hessian, scatter);
// else if(JacobianFactorOrdered::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactorOrdered>(factor))
// updateATA(*jacobianFactor, scatter);
// else
// throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
// }
//}
//gttoc(update);
//if (debug) gtsam::print(matrix_, "Ab' * Ab: ");
//assertInvariants();
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -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<Eigen::Upper>(); return info_.full().selfadjointView<Eigen::Upper>();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix HessianFactor::information() const { Matrix HessianFactor::information() const
{
return info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>(); return info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix HessianFactor::augmentedJacobian() const Matrix HessianFactor::augmentedJacobian() const
{ {
throw std::runtime_error("Not implemented"); return JacobianFactor(*this).augmentedJacobian();
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<Matrix, Vector> HessianFactor::jacobian() const std::pair<Matrix, Vector> 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) 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"); // First build an array of slots
gttic(slots);
//// 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* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google.
//size_t slot = 0; vector<size_t> slots(update.size());
//BOOST_FOREACH(Index j, update) { size_t slot = 0;
// slots[slot] = scatter.find(j)->second.slot; BOOST_FOREACH(Index j, update) {
// ++ slot; slots[slot] = scatter.find(j)->second.slot;
//} ++ slot;
//gttoc(slots); }
gttoc(slots);
//if(debug) { // Apply updates to the upper triangle
// this->print("Updating this: "); gttic(update);
// update.print("with (Hessian): "); for(size_t j2=0; j2<update.info_.nBlocks(); ++j2) {
//} size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
for(size_t j1=0; j1<=j2; ++j1) {
//// Apply updates to the upper triangle size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
//gttic(update); if(slot2 > slot1)
//for(size_t j2=0; j2<update.info_.nBlocks(); ++j2) { info_(slot1, slot2).noalias() += update.info_(j1, j2);
// size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2]; else if(slot1 > slot2)
// for(size_t j1=0; j1<=j2; ++j1) { info_(slot2, slot1).noalias() += update.info_(j1, j2).transpose();
// size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; else
// if(slot2 > slot1) { info_(slot1, slot2).triangularView<Eigen::Upper>() += update.info_(j1, j2);
// 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() += gttoc(update);
// 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<Eigen::Upper>() +=
// 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);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter) void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter)
{ {
throw std::runtime_error("Not implemented"); updateATA(HessianFactor(update), scatter);
//// 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<const JacobianFactorOrdered::AbMatrix> 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<noiseModel::Unit>(update.model_)) {
// updateInform.noalias() = updateA.transpose() * updateA;
//} else {
// noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(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; j2<update.Ab_.nBlocks(); ++j2) {
// size_t slot2 = (j2 == update.size()) ? this->info_.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<Eigen::Upper>() +=
// 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);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -479,38 +391,36 @@ void HessianFactor::partialCholesky(size_t nrFrontals)
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(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<Matrix> BlockAb;
BlockAb Ab(matrix_, info_);
//// Extract conditionals size_t varDim = info_.offset(nrFrontals);
//gttic(extract_conditionals); Ab.rowEnd() = Ab.rowStart() + varDim;
//GaussianConditionalOrdered::shared_ptr conditional(new GaussianConditionalOrdered());
//typedef VerticalBlockView<Matrix> BlockAb;
//BlockAb Ab(matrix_, info_);
//size_t varDim = info_.offset(nrFrontals); // Create one big conditionals with many frontal variables.
//Ab.rowEnd() = Ab.rowStart() + varDim; gttic(construct_cond);
//// Create one big conditionals with many frontal variables. VerticalBlockMatrix Ab()
//gttic(construct_cond); GaussianConditional::shared_ptr conditional = boost::make_shared<GaussianConditional>(
//Vector sigmas = Vector::Ones(varDim); keys_, nrFrontals, Ab);
//conditional = boost::make_shared<ConditionalType>(keys_.begin(), keys_.end(), nrFrontals, Ab, sigmas); gttoc(construct_cond);
//gttoc(construct_cond);
//if(debug) conditional->print("Extracted conditional: ");
//gttoc(extract_conditionals); gttoc(extract_conditionals);
//// Take lower-right block of Ab_ to get the new factor // Take lower-right block of Ab_ to get the new factor
//gttic(remaining_factor); gttic(remaining_factor);
//info_.blockStart() = nrFrontals; info_.blockStart() = nrFrontals;
//// Assign the keys // Assign the keys
//vector<Index> remainingKeys(keys_.size() - nrFrontals); vector<Index> remainingKeys(keys_.size() - nrFrontals);
//remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end()); remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end());
//keys_.swap(remainingKeys); keys_.swap(remainingKeys);
//gttoc(remaining_factor); gttoc(remaining_factor);
//return conditional; return conditional;
} }
/* ************************************************************************* */ /* ************************************************************************* */

19
gtsam/linear/JacobianFactor.h
View File

@ -187,20 +187,19 @@ namespace gtsam {
*/ */
std::pair<Matrix, Vector> jacobianUnweighted() const; std::pair<Matrix, Vector> jacobianUnweighted() const;
/** /** Return (dense) matrix associated with factor. The returned system is an augmented matrix:
* Return (dense) matrix associated with factor * [A b]
* The returned system is an augmented matrix: [A b] * @param set weight to use whitening to bake in weights*/
* @param set weight to use whitening to bake in weights
*/
virtual Matrix augmentedJacobian() const; virtual Matrix augmentedJacobian() const;
/** /** Return (dense) matrix associated with factor. The returned system is an augmented matrix:
* Return (dense) matrix associated with factor * [A b]
* The returned system is an augmented matrix: [A b] * @param set weight to use whitening to bake in weights */
* @param set weight to use whitening to bake in weights
*/
Matrix augmentedJacobianUnweighted() const; 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 * Construct the corresponding anti-factor to negate information
* stored stored in this factor. * stored stored in this factor.

137
gtsam/linear/tests/testHessianFactorUnordered.cpp
View File

@ -25,6 +25,7 @@
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -35,20 +36,19 @@ using namespace gtsam;
const double tol = 1e-5; const double tol = 1e-5;
#if 0
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HessianFactor, emptyConstructor) { TEST(HessianFactor, emptyConstructor) {
HessianFactor f; HessianFactor f;
DOUBLES_EQUAL(0.0, f.constantTerm(), 1e-9); // Constant term should be zero 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(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 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) { TEST(HessianFactor, ConversionConstructor) {
HessianFactorOrdered expected; HessianFactor expected;
expected.keys_.push_back(0); expected.keys_.push_back(0);
expected.keys_.push_back(1); expected.keys_.push_back(1);
size_t dims[] = { 2, 4, 1 }; size_t dims[] = { 2, 4, 1 };
@ -94,11 +94,11 @@ TEST(HessianFactor, ConversionConstructor) {
vector<pair<Index, Matrix> > meas; vector<pair<Index, Matrix> > meas;
meas.push_back(make_pair(0, Ax2)); meas.push_back(make_pair(0, Ax2));
meas.push_back(make_pair(1, Al1x1)); 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<size_t>(dims, dims+2)); VectorValues values(std::vector<size_t>(dims, dims+2));
values[0] = Vector_(2, 1.0, 2.0); values[0] = Vector_(2, 1.0, 2.0);
values[1] = Vector_(4, 3.0, 4.0, 5.0, 6.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); Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
Vector g = Vector_(2, -8.0, -9.0); Vector g = Vector_(2, -8.0, -9.0);
@ -121,11 +121,11 @@ TEST(HessianFactorOrdered, Constructor1)
vector<size_t> dims; vector<size_t> dims;
dims.push_back(2); dims.push_back(2);
VectorValuesOrdered dx(dims); VectorValues dx(dims);
dx[0] = dxv; dx[0] = dxv;
HessianFactorOrdered factor(0, G, g, f); HessianFactor factor(0, G, g, f);
// extract underlying parts // extract underlying parts
Matrix info_matrix = factor.info_.range(0, 1, 0, 1); 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); Vector mu = Vector_(2,1.0,2.0);
Matrix Sigma = eye(2,2); Matrix Sigma = eye(2,2);
HessianFactorOrdered factor(0, mu, Sigma); HessianFactor factor(0, mu, Sigma);
Matrix G = eye(2,2); Matrix G = eye(2,2);
Vector g = G*mu; 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 G11 = Matrix_(1,1, 1.0);
Matrix G12 = Matrix_(1,2, 2.0, 4.0); Matrix G12 = Matrix_(1,2, 2.0, 4.0);
@ -178,12 +178,12 @@ TEST(HessianFactorOrdered, Constructor2)
vector<size_t> dims; vector<size_t> dims;
dims.push_back(1); dims.push_back(1);
dims.push_back(2); dims.push_back(2);
VectorValuesOrdered dx(dims); VectorValues dx(dims);
dx[0] = dx0; dx[0] = dx0;
dx[1] = dx1; 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 expected = 90.5;
double actual = factor.error(dx); double actual = factor.error(dx);
@ -201,7 +201,7 @@ TEST(HessianFactorOrdered, Constructor2)
// Check case when vector values is larger than factor // Check case when vector values is larger than factor
dims.push_back(2); dims.push_back(2);
VectorValuesOrdered dxLarge(dims); VectorValues dxLarge(dims);
dxLarge[0] = dx0; dxLarge[0] = dx0;
dxLarge[1] = dx1; dxLarge[1] = dx1;
dxLarge[2] = Vector_(2, 0.1, 0.2); 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 G11 = Matrix_(1,1, 1.0);
Matrix G12 = Matrix_(1,2, 2.0, 4.0); Matrix G12 = Matrix_(1,2, 2.0, 4.0);
@ -234,13 +234,13 @@ TEST(HessianFactorOrdered, Constructor3)
dims.push_back(1); dims.push_back(1);
dims.push_back(2); dims.push_back(2);
dims.push_back(3); dims.push_back(3);
VectorValuesOrdered dx(dims); VectorValues dx(dims);
dx[0] = dx0; dx[0] = dx0;
dx[1] = dx1; dx[1] = dx1;
dx[2] = dx2; 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 expected = 371.3750;
double actual = factor.error(dx); 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 G11 = Matrix_(1,1, 1.0);
Matrix G12 = Matrix_(1,2, 2.0, 4.0); Matrix G12 = Matrix_(1,2, 2.0, 4.0);
@ -286,7 +286,7 @@ TEST(HessianFactorOrdered, ConstructorNWay)
dims.push_back(1); dims.push_back(1);
dims.push_back(2); dims.push_back(2);
dims.push_back(3); dims.push_back(3);
VectorValuesOrdered dx(dims); VectorValues dx(dims);
dx[0] = dx0; dx[0] = dx0;
dx[1] = dx1; 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); 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; std::vector<Vector> gs;
gs.push_back(g1); gs.push_back(g2); gs.push_back(g3); 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 expected = 371.3750;
double actual = factor.error(dx); 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 G11 = Matrix_(1,1, 1.0);
Matrix G12 = Matrix_(1,2, 2.0, 4.0); Matrix G12 = Matrix_(1,2, 2.0, 4.0);
@ -335,15 +335,15 @@ TEST_UNSAFE(HessianFactorOrdered, CopyConstructor_and_assignment)
vector<size_t> dims; vector<size_t> dims;
dims.push_back(1); dims.push_back(1);
dims.push_back(2); dims.push_back(2);
VectorValuesOrdered dx(dims); VectorValues dx(dims);
dx[0] = dx0; dx[0] = dx0;
dx[1] = dx1; 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 // Make a copy
HessianFactorOrdered copy1(originalFactor); HessianFactor copy1(originalFactor);
double expected = 90.5; double expected = 90.5;
double actual = copy1.error(dx); 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))); EXPECT(assert_equal(G22, copy1.info(copy1.begin()+1, copy1.begin()+1)));
// Make a copy using the assignment operator // Make a copy using the assignment operator
HessianFactorOrdered copy2; HessianFactor copy2;
copy2 = HessianFactorOrdered(originalFactor); // Make a temporary to make sure copying does not shared references copy2 = HessianFactor(originalFactor); // Make a temporary to make sure copying does not shared references
actual = copy2.error(dx); actual = copy2.error(dx);
DOUBLES_EQUAL(expected, actual, 1e-10); 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, Matrix A01 = Matrix_(3,3,
1.0, 0.0, 0.0, 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 b2 = Vector_(3, 3.5, 3.5, 3.5);
Vector s2 = Vector_(3, 3.6, 3.6, 3.6); 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(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, 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); Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
// create a full, uneliminated version of the factor // 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 // perform elimination
GaussianConditionalOrdered::shared_ptr expectedBN = expectedFactor.eliminate(1); GaussianConditional::shared_ptr expectedBN = expectedFactor.eliminate(1);
// create expected Hessian after elimination // create expected Hessian after elimination
HessianFactorOrdered expectedCholeskyFactor(expectedFactor); HessianFactor expectedCholeskyFactor(expectedFactor);
// Convert all factors to hessians // Convert all factors to hessians
FactorGraphOrdered<HessianFactorOrdered> hessians; FactorGraph<HessianFactor> hessians;
BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, gfg) { BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) {
if(boost::shared_ptr<HessianFactorOrdered> hf = boost::dynamic_pointer_cast<HessianFactorOrdered>(factor)) if(boost::shared_ptr<HessianFactor> hf = boost::dynamic_pointer_cast<HessianFactor>(factor))
hessians.push_back(hf); hessians.push_back(hf);
else if(boost::shared_ptr<JacobianFactorOrdered> jf = boost::dynamic_pointer_cast<JacobianFactorOrdered>(factor)) else if(boost::shared_ptr<JacobianFactor> jf = boost::dynamic_pointer_cast<JacobianFactor>(factor))
hessians.push_back(boost::make_shared<HessianFactorOrdered>(*jf)); hessians.push_back(boost::make_shared<HessianFactor>(*jf));
else else
CHECK(false); CHECK(false);
} }
// Eliminate // Eliminate
GaussianFactorGraphOrdered::EliminationResult actualCholesky = EliminateCholeskyOrdered(gfg, 1); GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(gfg, 1);
HessianFactorOrdered::shared_ptr actualFactor = boost::dynamic_pointer_cast<HessianFactorOrdered>(actualCholesky.second); HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<HessianFactor>(actualCholesky.second);
EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6)); EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6));
EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6)); EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HessianFactorOrdered, eliminate2 ) TEST(HessianFactor, eliminate2 )
{ {
// sigmas // sigmas
double sigma1 = 0.2; double sigma1 = 0.2;
@ -477,17 +477,17 @@ TEST(HessianFactorOrdered, eliminate2 )
vector<pair<Index, Matrix> > meas; vector<pair<Index, Matrix> > meas;
meas.push_back(make_pair(0, Ax2)); meas.push_back(make_pair(0, Ax2));
meas.push_back(make_pair(1, Al1x1)); 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 // eliminate the combined factor
HessianFactorOrdered::shared_ptr combinedLF_Chol(new HessianFactorOrdered(combined)); HessianFactor::shared_ptr combinedLF_Chol(new HessianFactor(combined));
FactorGraphOrdered<HessianFactorOrdered> combinedLFG_Chol; FactorGraph<HessianFactor> combinedLFG_Chol;
combinedLFG_Chol.push_back(combinedLF_Chol); combinedLFG_Chol.push_back(combinedLF_Chol);
GaussianFactorGraphOrdered::EliminationResult actual_Chol = EliminateCholeskyOrdered( GaussianFactorGraph::EliminationResult actual_Chol = EliminateCholesky(
combinedLFG_Chol, 1); combinedLFG_Chol, 1);
HessianFactorOrdered::shared_ptr actualFactor = boost::dynamic_pointer_cast< HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<
HessianFactorOrdered>(actual_Chol.second); HessianFactor>(actual_Chol.second);
// create expected Conditional Gaussian // create expected Conditional Gaussian
double oldSigma = 0.0894427; // from when R was made unit 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 +0.00,-0.20,+0.00,-0.80
)/oldSigma; )/oldSigma;
Vector d = Vector_(2,0.2,-0.14)/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)); EXPECT(assert_equal(expectedCG,*actual_Chol.first,1e-4));
// the expected linear factor // the expected linear factor
@ -511,15 +511,15 @@ TEST(HessianFactorOrdered, eliminate2 )
0.00, 1.00, +0.00, -1.00 0.00, 1.00, +0.00, -1.00
)/sigma; )/sigma;
Vector b1 = Vector_(2,0.0,0.894427); Vector b1 = Vector_(2,0.0,0.894427);
JacobianFactorOrdered expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0)); JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0));
EXPECT(assert_equal(HessianFactorOrdered(expectedLF), *actualFactor, 1.5e-3)); EXPECT(assert_equal(HessianFactor(expectedLF), *actualFactor, 1.5e-3));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HessianFactorOrdered, eliminateUnsorted) { TEST(HessianFactor, eliminateUnsorted) {
JacobianFactorOrdered::shared_ptr factor1( JacobianFactor::shared_ptr factor1(
new JacobianFactorOrdered(0, new JacobianFactor(0,
Matrix_(3,3, Matrix_(3,3,
44.7214, 0.0, 0.0, 44.7214, 0.0, 0.0,
0.0, 44.7214, 0.0, 0.0, 44.7214, 0.0,
@ -531,8 +531,8 @@ TEST(HessianFactorOrdered, eliminateUnsorted) {
0.0, 0.0, -44.7214), 0.0, 0.0, -44.7214),
Vector_(3, 1.98916e-17, -4.96503e-15, -7.75792e-17), Vector_(3, 1.98916e-17, -4.96503e-15, -7.75792e-17),
noiseModel::Unit::Create(3))); noiseModel::Unit::Create(3)));
HessianFactorOrdered::shared_ptr unsorted_factor2( HessianFactor::shared_ptr unsorted_factor2(
new HessianFactorOrdered(JacobianFactorOrdered(0, new HessianFactor(JacobianFactor(0,
Matrix_(6,3, Matrix_(6,3,
25.8367, 0.1211, 0.0593, 25.8367, 0.1211, 0.0593,
0.0, 23.4099, 30.8733, 0.0, 23.4099, 30.8733,
@ -555,8 +555,8 @@ TEST(HessianFactorOrdered, eliminateUnsorted) {
permutation[1] = 0; permutation[1] = 0;
unsorted_factor2->permuteWithInverse(permutation); unsorted_factor2->permuteWithInverse(permutation);
HessianFactorOrdered::shared_ptr sorted_factor2( HessianFactor::shared_ptr sorted_factor2(
new HessianFactorOrdered(JacobianFactorOrdered(0, new HessianFactor(JacobianFactor(0,
Matrix_(6,3, Matrix_(6,3,
25.7429, -1.6897, 0.4587, 25.7429, -1.6897, 0.4587,
1.6400, 23.3095, -8.4816, 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), Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
noiseModel::Unit::Create(6)))); noiseModel::Unit::Create(6))));
GaussianFactorGraphOrdered sortedGraph; GaussianFactorGraph sortedGraph;
// sortedGraph.push_back(factor1); // sortedGraph.push_back(factor1);
sortedGraph.push_back(sorted_factor2); sortedGraph.push_back(sorted_factor2);
GaussianFactorGraphOrdered unsortedGraph; GaussianFactorGraph unsortedGraph;
// unsortedGraph.push_back(factor1); // unsortedGraph.push_back(factor1);
unsortedGraph.push_back(unsorted_factor2); unsortedGraph.push_back(unsorted_factor2);
GaussianConditionalOrdered::shared_ptr expected_bn; GaussianConditional::shared_ptr expected_bn;
GaussianFactorOrdered::shared_ptr expected_factor; GaussianFactor::shared_ptr expected_factor;
boost::tie(expected_bn, expected_factor) = boost::tie(expected_bn, expected_factor) =
EliminatePreferCholeskyOrdered(sortedGraph, 1); EliminatePreferCholesky(sortedGraph, 1);
GaussianConditionalOrdered::shared_ptr actual_bn; GaussianConditional::shared_ptr actual_bn;
GaussianFactorOrdered::shared_ptr actual_factor; GaussianFactor::shared_ptr actual_factor;
boost::tie(actual_bn, 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_bn, *actual_bn, 1e-10));
EXPECT(assert_equal(*expected_factor, *actual_factor, 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 // update the information matrix with a single jacobian factor
Matrix A0 = Matrix_(2, 2, Matrix A0 = Matrix_(2, 2,
@ -612,12 +612,12 @@ TEST(HessianFactorOrdered, combine) {
0.0, -8.94427191); 0.0, -8.94427191);
Vector b = Vector_(2, 2.23606798,-1.56524758); Vector b = Vector_(2, 2.23606798,-1.56524758);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2)); SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2));
GaussianFactorOrdered::shared_ptr f(new JacobianFactorOrdered(0, A0, 1, A1, 2, A2, b, model)); GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model));
FactorGraphOrdered<GaussianFactorOrdered> factors; FactorGraph<GaussianFactor> factors;
factors.push_back(f); factors.push_back(f);
// Form Ab' * Ab // Form Ab' * Ab
HessianFactorOrdered actual(factors); HessianFactor actual(factors);
Matrix expected = Matrix_(7, 7, Matrix expected = Matrix_(7, 7,
125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000, 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<Eigen::Upper>()), tol)); Matrix(actual.matrix_.triangularView<Eigen::Upper>()), tol));
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}