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

added raw memory access version of hessianDiagonal

release/4.3a0
Luca 2014-03-19 17:43:20 -04:00
parent d2b6b12bba
commit 6edd3f10fc
1 changed files with 604 additions and 573 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

521
gtsam/linear/JacobianFactor.cpp
View File

@ -57,63 +57,56 @@ using namespace boost::assign;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor() : JacobianFactor::JacobianFactor() :
Ab_(cref_list_of<1>(1), 0) Ab_(cref_list_of<1>(1), 0) {
{
getb().setZero(); getb().setZero();
} }
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor(const GaussianFactor& gf) { JacobianFactor::JacobianFactor(const GaussianFactor& gf) {
// Copy the matrix data depending on what type of factor we're copying from // Copy the matrix data depending on what type of factor we're copying from
if(const JacobianFactor* rhs = dynamic_cast<const JacobianFactor*>(&gf)) if (const JacobianFactor* rhs = dynamic_cast<const JacobianFactor*>(&gf))
*this = JacobianFactor(*rhs); *this = JacobianFactor(*rhs);
else if(const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&gf)) else if (const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&gf))
*this = JacobianFactor(*rhs); *this = JacobianFactor(*rhs);
else else
throw std::invalid_argument("In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor"); throw std::invalid_argument(
} "In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
}
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor(const Vector& b_in) : JacobianFactor::JacobianFactor(const Vector& b_in) :
Ab_(cref_list_of<1>(1), b_in.size()) Ab_(cref_list_of<1>(1), b_in.size()) {
{
getb() = b_in; getb() = b_in;
} }
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor(Key i1, const Matrix& A1, JacobianFactor::JacobianFactor(Key i1, const Matrix& A1, const Vector& b,
const Vector& b, const SharedDiagonal& model) const SharedDiagonal& model) {
{
fillTerms(cref_list_of<1>(make_pair(i1, A1)), b, model); fillTerms(cref_list_of<1>(make_pair(i1, A1)), b, model);
} }
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor( JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2,
const Key i1, const Matrix& A1, Key i2, const Matrix& A2, const Matrix& A2, const Vector& b, const SharedDiagonal& model) {
const Vector& b, const SharedDiagonal& model) fillTerms(cref_list_of<2>(make_pair(i1, A1))(make_pair(i2, A2)), b, model);
{ }
fillTerms(cref_list_of<2>
(make_pair(i1,A1))
(make_pair(i2,A2)), b, model);
}
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor( JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2,
const Key i1, const Matrix& A1, Key i2, const Matrix& A2, const Matrix& A2, Key i3, const Matrix& A3, const Vector& b,
Key i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) const SharedDiagonal& model) {
{ fillTerms(
fillTerms(cref_list_of<3> cref_list_of<3>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)),
(make_pair(i1,A1)) b, model);
(make_pair(i2,A2)) }
(make_pair(i3,A3)), b, model);
}
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor(const HessianFactor& factor) : JacobianFactor::JacobianFactor(const HessianFactor& factor) :
Base(factor), Ab_(VerticalBlockMatrix::LikeActiveViewOf(factor.matrixObject(), factor.rows())) Base(factor), Ab_(
{ VerticalBlockMatrix::LikeActiveViewOf(factor.matrixObject(),
factor.rows())) {
// Copy Hessian into our matrix and then do in-place Cholesky // Copy Hessian into our matrix and then do in-place Cholesky
Ab_.full() = factor.matrixObject().full(); Ab_.full() = factor.matrixObject().full();
@ -123,7 +116,7 @@ namespace gtsam {
boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix()); boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix());
// Check for indefinite system // Check for indefinite system
if(!success) if (!success)
throw IndeterminantLinearSystemException(factor.keys().front()); throw IndeterminantLinearSystemException(factor.keys().front());
// Zero out lower triangle // Zero out lower triangle
@ -132,37 +125,39 @@ namespace gtsam {
// FIXME: replace with triangular system // FIXME: replace with triangular system
Ab_.rowEnd() = maxrank; Ab_.rowEnd() = maxrank;
model_ = SharedDiagonal(); // should be same as Unit::Create(maxrank); model_ = SharedDiagonal(); // should be same as Unit::Create(maxrank);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Helper functions for combine constructor // Helper functions for combine constructor
namespace { namespace {
boost::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims( boost::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
const FastVector<JacobianFactor::shared_ptr>& factors, const FastVector<VariableSlots::const_iterator>& variableSlots) const FastVector<JacobianFactor::shared_ptr>& factors,
{ const FastVector<VariableSlots::const_iterator>& variableSlots) {
gttic(countDims); gttic(countDims);
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
FastVector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max()); FastVector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max());
#else #else
FastVector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max()); FastVector<DenseIndex> varDims(variableSlots.size(),
numeric_limits<DenseIndex>::max());
#endif #endif
DenseIndex m = 0; DenseIndex m = 0;
DenseIndex n = 0; DenseIndex n = 0;
for(size_t jointVarpos = 0; jointVarpos < variableSlots.size(); ++jointVarpos) for (size_t jointVarpos = 0; jointVarpos < variableSlots.size();
{ ++jointVarpos) {
const VariableSlots::const_iterator& slots = variableSlots[jointVarpos]; const VariableSlots::const_iterator& slots = variableSlots[jointVarpos];
assert(slots->second.size() == factors.size()); assert(slots->second.size() == factors.size());
bool foundVariable = false; bool foundVariable = false;
for(size_t sourceFactorI = 0; sourceFactorI < slots->second.size(); ++sourceFactorI) for (size_t sourceFactorI = 0; sourceFactorI < slots->second.size();
{ ++sourceFactorI) {
const size_t sourceVarpos = slots->second[sourceFactorI]; const size_t sourceVarpos = slots->second[sourceFactorI];
if(sourceVarpos != VariableSlots::Empty) { if (sourceVarpos != VariableSlots::Empty) {
const JacobianFactor& sourceFactor = *factors[sourceFactorI]; const JacobianFactor& sourceFactor = *factors[sourceFactorI];
if(sourceFactor.cols() > 1) { if (sourceFactor.cols() > 1) {
foundVariable = true; foundVariable = true;
DenseIndex vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos); DenseIndex vardim = sourceFactor.getDim(
sourceFactor.begin() + sourceVarpos);
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
if(varDims[jointVarpos] == numeric_limits<DenseIndex>::max()) { if(varDims[jointVarpos] == numeric_limits<DenseIndex>::max()) {
@ -185,8 +180,9 @@ namespace gtsam {
} }
} }
if(!foundVariable) if (!foundVariable)
throw std::invalid_argument("Unable to determine dimensionality for all variables"); throw std::invalid_argument(
"Unable to determine dimensionality for all variables");
} }
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) { BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) {
@ -200,44 +196,43 @@ namespace gtsam {
#endif #endif
return boost::make_tuple(varDims, m, n); return boost::make_tuple(varDims, m, n);
} }
/* ************************************************************************* */ /* ************************************************************************* */
FastVector<JacobianFactor::shared_ptr> FastVector<JacobianFactor::shared_ptr> _convertOrCastToJacobians(
_convertOrCastToJacobians(const GaussianFactorGraph& factors) const GaussianFactorGraph& factors) {
{
gttic(Convert_to_Jacobians); gttic(Convert_to_Jacobians);
FastVector<JacobianFactor::shared_ptr> jacobians; FastVector<JacobianFactor::shared_ptr> jacobians;
jacobians.reserve(factors.size()); jacobians.reserve(factors.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
if(factor) { if (factor) {
if(JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(factor)) if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<
JacobianFactor>(factor))
jacobians.push_back(jf); jacobians.push_back(jf);
else else
jacobians.push_back(boost::make_shared<JacobianFactor>(*factor)); jacobians.push_back(boost::make_shared<JacobianFactor>(*factor));
} }
} }
return jacobians; return jacobians;
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor( JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
const GaussianFactorGraph& graph,
boost::optional<const Ordering&> ordering, boost::optional<const Ordering&> ordering,
boost::optional<const VariableSlots&> variableSlots) boost::optional<const VariableSlots&> variableSlots) {
{
gttic(JacobianFactor_combine_constructor); gttic(JacobianFactor_combine_constructor);
// Compute VariableSlots if one was not provided // Compute VariableSlots if one was not provided
boost::optional<VariableSlots> computedVariableSlots; boost::optional<VariableSlots> computedVariableSlots;
if(!variableSlots) { if (!variableSlots) {
computedVariableSlots = VariableSlots(graph); computedVariableSlots = VariableSlots(graph);
variableSlots = computedVariableSlots; // Binds reference, does not copy VariableSlots variableSlots = computedVariableSlots; // Binds reference, does not copy VariableSlots
} }
// Cast or convert to Jacobians // Cast or convert to Jacobians
FastVector<JacobianFactor::shared_ptr> jacobians = _convertOrCastToJacobians(graph); FastVector<JacobianFactor::shared_ptr> jacobians = _convertOrCastToJacobians(
graph);
gttic(Order_slots); gttic(Order_slots);
// Order variable slots - we maintain the vector of ordered slots, as well as keep a list // Order variable slots - we maintain the vector of ordered slots, as well as keep a list
@ -245,22 +240,24 @@ namespace gtsam {
// be added after all of the ordered variables. // be added after all of the ordered variables.
FastVector<VariableSlots::const_iterator> orderedSlots; FastVector<VariableSlots::const_iterator> orderedSlots;
orderedSlots.reserve(variableSlots->size()); orderedSlots.reserve(variableSlots->size());
if(ordering) { if (ordering) {
// If an ordering is provided, arrange the slots first that ordering // If an ordering is provided, arrange the slots first that ordering
FastList<VariableSlots::const_iterator> unorderedSlots; FastList<VariableSlots::const_iterator> unorderedSlots;
size_t nOrderingSlotsUsed = 0; size_t nOrderingSlotsUsed = 0;
orderedSlots.resize(ordering->size()); orderedSlots.resize(ordering->size());
FastMap<Key, size_t> inverseOrdering = ordering->invert(); FastMap<Key, size_t> inverseOrdering = ordering->invert();
for(VariableSlots::const_iterator item = variableSlots->begin(); item != variableSlots->end(); ++item) { for (VariableSlots::const_iterator item = variableSlots->begin();
FastMap<Key, size_t>::const_iterator orderingPosition = inverseOrdering.find(item->first); item != variableSlots->end(); ++item) {
if(orderingPosition == inverseOrdering.end()) { FastMap<Key, size_t>::const_iterator orderingPosition =
inverseOrdering.find(item->first);
if (orderingPosition == inverseOrdering.end()) {
unorderedSlots.push_back(item); unorderedSlots.push_back(item);
} else { } else {
orderedSlots[orderingPosition->second] = item; orderedSlots[orderingPosition->second] = item;
++ nOrderingSlotsUsed; ++nOrderingSlotsUsed;
} }
} }
if(nOrderingSlotsUsed != ordering->size()) if (nOrderingSlotsUsed != ordering->size())
throw std::invalid_argument( throw std::invalid_argument(
"The ordering provided to the JacobianFactor combine constructor\n" "The ordering provided to the JacobianFactor combine constructor\n"
"contained extra variables that did not appear in the factors to combine."); "contained extra variables that did not appear in the factors to combine.");
@ -271,7 +268,8 @@ namespace gtsam {
} else { } else {
// If no ordering is provided, arrange the slots as they were, which will be sorted // If no ordering is provided, arrange the slots as they were, which will be sorted
// numerically since VariableSlots uses a map sorting on Key. // numerically since VariableSlots uses a map sorting on Key.
for(VariableSlots::const_iterator item = variableSlots->begin(); item != variableSlots->end(); ++item) for (VariableSlots::const_iterator item = variableSlots->begin();
item != variableSlots->end(); ++item)
orderedSlots.push_back(item); orderedSlots.push_back(item);
} }
gttoc(Order_slots); gttoc(Order_slots);
@ -285,26 +283,30 @@ namespace gtsam {
gttic(allocate); gttic(allocate);
Ab_ = VerticalBlockMatrix(varDims, m, true); // Allocate augmented matrix Ab_ = VerticalBlockMatrix(varDims, m, true); // Allocate augmented matrix
Base::keys_.resize(orderedSlots.size()); Base::keys_.resize(orderedSlots.size());
boost::range::copy( // Get variable keys boost::range::copy(
orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin()); // Get variable keys
orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys,
Base::keys_.begin());
gttoc(allocate); gttoc(allocate);
// Loop over slots in combined factor and copy blocks from source factors // Loop over slots in combined factor and copy blocks from source factors
gttic(copy_blocks); gttic(copy_blocks);
size_t combinedSlot = 0; size_t combinedSlot = 0;
BOOST_FOREACH(VariableSlots::const_iterator varslot, orderedSlots) { BOOST_FOREACH(VariableSlots::const_iterator varslot, orderedSlots) {
JacobianFactor::ABlock destSlot(this->getA(this->begin()+combinedSlot)); JacobianFactor::ABlock destSlot(this->getA(this->begin() + combinedSlot));
// Loop over source jacobians // Loop over source jacobians
DenseIndex nextRow = 0; DenseIndex nextRow = 0;
for(size_t factorI = 0; factorI < jacobians.size(); ++factorI) { for (size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
// Slot in source factor // Slot in source factor
const size_t sourceSlot = varslot->second[factorI]; const size_t sourceSlot = varslot->second[factorI];
const DenseIndex sourceRows = jacobians[factorI]->rows(); const DenseIndex sourceRows = jacobians[factorI]->rows();
if(sourceRows > 0) { if (sourceRows > 0) {
JacobianFactor::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows)); JacobianFactor::ABlock::RowsBlockXpr destBlock(
destSlot.middleRows(nextRow, sourceRows));
// Copy if exists in source factor, otherwise set zero // Copy if exists in source factor, otherwise set zero
if(sourceSlot != VariableSlots::Empty) if (sourceSlot != VariableSlots::Empty)
destBlock = jacobians[factorI]->getA(jacobians[factorI]->begin()+sourceSlot); destBlock = jacobians[factorI]->getA(
jacobians[factorI]->begin() + sourceSlot);
else else
destBlock.setZero(); destBlock.setZero();
nextRow += sourceRows; nextRow += sourceRows;
@ -320,15 +322,16 @@ namespace gtsam {
boost::optional<Vector> sigmas; boost::optional<Vector> sigmas;
// Loop over source jacobians // Loop over source jacobians
DenseIndex nextRow = 0; DenseIndex nextRow = 0;
for(size_t factorI = 0; factorI < jacobians.size(); ++factorI) { for (size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
const DenseIndex sourceRows = jacobians[factorI]->rows(); const DenseIndex sourceRows = jacobians[factorI]->rows();
if(sourceRows > 0) { if (sourceRows > 0) {
this->getb().segment(nextRow, sourceRows) = jacobians[factorI]->getb(); this->getb().segment(nextRow, sourceRows) = jacobians[factorI]->getb();
if(jacobians[factorI]->get_model()) { if (jacobians[factorI]->get_model()) {
// If the factor has a noise model and we haven't yet allocated sigmas, allocate it. // If the factor has a noise model and we haven't yet allocated sigmas, allocate it.
if(!sigmas) if (!sigmas)
sigmas = Vector::Constant(m, 1.0); sigmas = Vector::Constant(m, 1.0);
sigmas->segment(nextRow, sourceRows) = jacobians[factorI]->get_model()->sigmas(); sigmas->segment(nextRow, sourceRows) =
jacobians[factorI]->get_model()->sigmas();
if (jacobians[factorI]->isConstrained()) if (jacobians[factorI]->isConstrained())
anyConstrained = true; anyConstrained = true;
} }
@ -337,44 +340,44 @@ namespace gtsam {
} }
gttoc(copy_vectors); gttoc(copy_vectors);
if(sigmas) if (sigmas)
this->setModel(anyConstrained, *sigmas); this->setModel(anyConstrained, *sigmas);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void JacobianFactor::print(const string& s, const KeyFormatter& formatter) const void JacobianFactor::print(const string& s,
{ const KeyFormatter& formatter) const {
if(!s.empty()) if (!s.empty())
cout << s << "\n"; cout << s << "\n";
for(const_iterator key = begin(); key != end(); ++key) { for (const_iterator key = begin(); key != end(); ++key) {
cout << cout
formatMatrixIndented((boost::format(" A[%1%] = ") % formatter(*key)).str(), getA(key)) << formatMatrixIndented(
(boost::format(" A[%1%] = ") % formatter(*key)).str(), getA(key))
<< endl; << endl;
} }
cout << formatMatrixIndented(" b = ", getb(), true) << "\n"; cout << formatMatrixIndented(" b = ", getb(), true) << "\n";
if(model_) if (model_)
model_->print(" Noise model: "); model_->print(" Noise model: ");
else else
cout << " No noise model" << endl; cout << " No noise model" << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Check if two linear factors are equal // Check if two linear factors are equal
bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const {
{ if (!dynamic_cast<const JacobianFactor*>(&f_))
if(!dynamic_cast<const JacobianFactor*>(&f_))
return false; return false;
else { else {
const JacobianFactor& f(static_cast<const JacobianFactor&>(f_)); const JacobianFactor& f(static_cast<const JacobianFactor&>(f_));
// Check keys // Check keys
if(keys() != f.keys()) if (keys() != f.keys())
return false; return false;
// Check noise model // Check noise model
if((model_ && !f.model_) || (!model_ && f.model_)) if ((model_ && !f.model_) || (!model_ && f.model_))
return false; return false;
if(model_ && f.model_ && !model_->equals(*f.model_, tol)) if (model_ && f.model_ && !model_->equals(*f.model_, tol))
return false; return false;
// Check matrix sizes // Check matrix sizes
@ -384,161 +387,183 @@ namespace gtsam {
// Check matrix contents // Check matrix contents
constABlock Ab1(Ab_.range(0, Ab_.nBlocks())); constABlock Ab1(Ab_.range(0, Ab_.nBlocks()));
constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks())); constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks()));
for(size_t row=0; row< (size_t) Ab1.rows(); ++row) for (size_t row = 0; row < (size_t) Ab1.rows(); ++row)
if(!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol) && if (!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol)
!equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) && !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol))
return false; return false;
return true; return true;
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector JacobianFactor::unweighted_error(const VectorValues& c) const { Vector JacobianFactor::unweighted_error(const VectorValues& c) const {
Vector e = -getb(); Vector e = -getb();
for(size_t pos=0; pos<size(); ++pos) for (size_t pos = 0; pos < size(); ++pos)
e += Ab_(pos) * c[keys_[pos]]; e += Ab_(pos) * c[keys_[pos]];
return e; return e;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector JacobianFactor::error_vector(const VectorValues& c) const { Vector JacobianFactor::error_vector(const VectorValues& c) const {
if(model_) if (model_)
return model_->whiten(unweighted_error(c)); return model_->whiten(unweighted_error(c));
else else
return unweighted_error(c); return unweighted_error(c);
} }
/* ************************************************************************* */ /* ************************************************************************* */
double JacobianFactor::error(const VectorValues& c) const { double JacobianFactor::error(const VectorValues& c) const {
if (empty()) return 0; if (empty())
return 0;
Vector weighted = error_vector(c); Vector weighted = error_vector(c);
return 0.5 * weighted.dot(weighted); return 0.5 * weighted.dot(weighted);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix JacobianFactor::augmentedInformation() const { Matrix JacobianFactor::augmentedInformation() const {
if(model_) { if (model_) {
Matrix AbWhitened = Ab_.full(); Matrix AbWhitened = Ab_.full();
model_->WhitenInPlace(AbWhitened); model_->WhitenInPlace(AbWhitened);
return AbWhitened.transpose() * AbWhitened; return AbWhitened.transpose() * AbWhitened;
} else { } else {
return Ab_.full().transpose() * Ab_.full(); return Ab_.full().transpose() * Ab_.full();
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix JacobianFactor::information() const { Matrix JacobianFactor::information() const {
if(model_) { if (model_) {
Matrix AWhitened = this->getA(); Matrix AWhitened = this->getA();
model_->WhitenInPlace(AWhitened); model_->WhitenInPlace(AWhitened);
return AWhitened.transpose() * AWhitened; return AWhitened.transpose() * AWhitened;
} else { } else {
return this->getA().transpose() * this->getA(); return this->getA().transpose() * this->getA();
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues JacobianFactor::hessianDiagonal() const { VectorValues JacobianFactor::hessianDiagonal() const {
VectorValues d; VectorValues d;
for(size_t pos=0; pos<size(); ++pos) for (size_t pos = 0; pos < size(); ++pos) {
{
Key j = keys_[pos]; Key j = keys_[pos];
size_t nj = Ab_(pos).cols(); size_t nj = Ab_(pos).cols();
Vector dj(nj); Vector dj(nj);
for (size_t k = 0; k < nj; ++k) { for (size_t k = 0; k < nj; ++k) {
Vector column_k = Ab_(pos).col(k); Vector column_k = Ab_(pos).col(k);
if (model_) column_k = model_->whiten(column_k); if (model_)
dj(k) = dot(column_k,column_k); column_k = model_->whiten(column_k);
dj(k) = dot(column_k, column_k);
} }
d.insert(j,dj); d.insert(j, dj);
} }
return d; return d;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void JacobianFactor::hessianDiagonal(double* d) const { // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
throw std::runtime_error("JacobianFactor::hessianDiagonal non implemented (use VectorValues version)"); void JacobianFactor::hessianDiagonal(double* d) const {
}
/* ************************************************************************* */ // Use eigen magic to access raw memory
map<Key,Matrix> JacobianFactor::hessianBlockDiagonal() const { typedef Eigen::Matrix<double, 9, 1> DVector;
map<Key,Matrix> blocks; typedef Eigen::Map<DVector> DMap;
for(size_t pos=0; pos<size(); ++pos)
{ // Loop over all variables in the factor
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
// Get the diagonal block, and insert its diagonal
DVector dj;
for (size_t k = 0; k < 9; ++k)
dj(k) = Ab_(j).col(k).squaredNorm();
DMap(d + 9 * j) += dj;
}
}
/* ************************************************************************* */
map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
map<Key, Matrix> blocks;
for (size_t pos = 0; pos < size(); ++pos) {
Key j = keys_[pos]; Key j = keys_[pos];
Matrix Aj = Ab_(pos); Matrix Aj = Ab_(pos);
if (model_) Aj = model_->Whiten(Aj); if (model_)
blocks.insert(make_pair(j,Aj.transpose()*Aj)); Aj = model_->Whiten(Aj);
blocks.insert(make_pair(j, Aj.transpose() * Aj));
} }
return blocks; return blocks;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector JacobianFactor::operator*(const VectorValues& x) const { Vector JacobianFactor::operator*(const VectorValues& x) const {
Vector Ax = zero(Ab_.rows()); Vector Ax = zero(Ab_.rows());
if (empty()) return Ax; if (empty())
return Ax;
// Just iterate over all A matrices and multiply in correct config part // Just iterate over all A matrices and multiply in correct config part
for(size_t pos=0; pos<size(); ++pos) for (size_t pos = 0; pos < size(); ++pos)
Ax += Ab_(pos) * x[keys_[pos]]; Ax += Ab_(pos) * x[keys_[pos]];
return model_ ? model_->whiten(Ax) : Ax; return model_ ? model_->whiten(Ax) : Ax;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e, void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
VectorValues& x) const VectorValues& x) const {
{
Vector E = alpha * (model_ ? model_->whiten(e) : e); Vector E = alpha * (model_ ? model_->whiten(e) : e);
// Just iterate over all A matrices and insert Ai^e into VectorValues // Just iterate over all A matrices and insert Ai^e into VectorValues
for(size_t pos=0; pos<size(); ++pos) for (size_t pos = 0; pos < size(); ++pos) {
{
Key j = keys_[pos]; Key j = keys_[pos];
// Create the value as a zero vector if it does not exist. // Create the value as a zero vector if it does not exist.
pair<VectorValues::iterator, bool> xi = x.tryInsert(j, Vector()); pair<VectorValues::iterator, bool> xi = x.tryInsert(j, Vector());
if(xi.second) if (xi.second)
xi.first->second = Vector::Zero(getDim(begin() + pos)); xi.first->second = Vector::Zero(getDim(begin() + pos));
gtsam::transposeMultiplyAdd(Ab_(pos), E, xi.first->second); gtsam::transposeMultiplyAdd(Ab_(pos), E, xi.first->second);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const { VectorValues& y) const {
Vector Ax = (*this)*x; Vector Ax = (*this) * x;
transposeMultiplyAdd(alpha,Ax,y); transposeMultiplyAdd(alpha, Ax, y);
} }
void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys) const { void JacobianFactor::multiplyHessianAdd(double alpha, const double* x,
double* y, std::vector<size_t> keys) const {
// Use eigen magic to access raw memory // Use eigen magic to access raw memory
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector; typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
typedef Eigen::Map<DVector> DMap; typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap; typedef Eigen::Map<const DVector> ConstDMap;
if (empty()) return; if (empty())
return;
Vector Ax = zero(Ab_.rows()); Vector Ax = zero(Ab_.rows());
// Just iterate over all A matrices and multiply in correct config part // Just iterate over all A matrices and multiply in correct config part
for(size_t pos=0; pos<size(); ++pos) for (size_t pos = 0; pos < size(); ++pos)
Ax += Ab_(pos) * ConstDMap(x + keys[keys_[pos]],keys[keys_[pos]+1]-keys[keys_[pos]]); Ax += Ab_(pos)
* ConstDMap(x + keys[keys_[pos]],
keys[keys_[pos] + 1] - keys[keys_[pos]]);
// Deal with noise properly, need to Double* whiten as we are dividing by variance // Deal with noise properly, need to Double* whiten as we are dividing by variance
if (model_) { model_->whitenInPlace(Ax); model_->whitenInPlace(Ax); } if (model_) {
model_->whitenInPlace(Ax);
model_->whitenInPlace(Ax);
}
// multiply with alpha // multiply with alpha
Ax *= alpha; Ax *= alpha;
// Again iterate over all A matrices and insert Ai^e into y // Again iterate over all A matrices and insert Ai^e into y
for(size_t pos=0; pos<size(); ++pos) for (size_t pos = 0; pos < size(); ++pos)
DMap(y + keys[keys_[pos]],keys[keys_[pos]+1]-keys[keys_[pos]]) += Ab_(pos).transpose() * Ax; DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_(
pos).transpose() * Ax;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues JacobianFactor::gradientAtZero() const { VectorValues JacobianFactor::gradientAtZero() const {
VectorValues g; VectorValues g;
Vector b = getb(); Vector b = getb();
// Gradient is really -A'*b / sigma^2 // Gradient is really -A'*b / sigma^2
@ -546,82 +571,82 @@ namespace gtsam {
Vector b_sigma = model_ ? model_->whiten(b) : b; Vector b_sigma = model_ ? model_->whiten(b) : b;
this->transposeMultiplyAdd(-1.0, b_sigma, g); // g -= A'*b/sigma^2 this->transposeMultiplyAdd(-1.0, b_sigma, g); // g -= A'*b/sigma^2
return g; return g;
} }
/* ************************************************************************* */ /* ************************************************************************* */
pair<Matrix,Vector> JacobianFactor::jacobian() const { pair<Matrix, Vector> JacobianFactor::jacobian() const {
pair<Matrix,Vector> result = jacobianUnweighted(); pair<Matrix, Vector> result = jacobianUnweighted();
// divide in sigma so error is indeed 0.5*|Ax-b| // divide in sigma so error is indeed 0.5*|Ax-b|
if (model_) if (model_)
model_->WhitenSystem(result.first, result.second); model_->WhitenSystem(result.first, result.second);
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
pair<Matrix,Vector> JacobianFactor::jacobianUnweighted() const { pair<Matrix, Vector> JacobianFactor::jacobianUnweighted() const {
Matrix A(Ab_.range(0, size())); Matrix A(Ab_.range(0, size()));
Vector b(getb()); Vector b(getb());
return make_pair(A, b); return make_pair(A, b);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix JacobianFactor::augmentedJacobian() const { Matrix JacobianFactor::augmentedJacobian() const {
Matrix Ab = augmentedJacobianUnweighted(); Matrix Ab = augmentedJacobianUnweighted();
if (model_) if (model_)
model_->WhitenInPlace(Ab); model_->WhitenInPlace(Ab);
return Ab; return Ab;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix JacobianFactor::augmentedJacobianUnweighted() const { Matrix JacobianFactor::augmentedJacobianUnweighted() const {
return Ab_.range(0, Ab_.nBlocks()); return Ab_.range(0, Ab_.nBlocks());
} }
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor JacobianFactor::whiten() const { JacobianFactor JacobianFactor::whiten() const {
JacobianFactor result(*this); JacobianFactor result(*this);
if(model_) { if (model_) {
result.model_->WhitenInPlace(result.Ab_.full()); result.model_->WhitenInPlace(result.Ab_.full());
result.model_ = SharedDiagonal(); result.model_ = SharedDiagonal();
} }
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactor::shared_ptr JacobianFactor::negate() const { GaussianFactor::shared_ptr JacobianFactor::negate() const {
HessianFactor hessian(*this); HessianFactor hessian(*this);
return hessian.negate(); return hessian.negate();
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> > std::pair<boost::shared_ptr<GaussianConditional>,
JacobianFactor::eliminate(const Ordering& keys) boost::shared_ptr<JacobianFactor> > JacobianFactor::eliminate(
{ const Ordering& keys) {
GaussianFactorGraph graph; GaussianFactorGraph graph;
graph.add(*this); graph.add(*this);
return EliminateQR(graph, keys); return EliminateQR(graph, keys);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void JacobianFactor::setModel(bool anyConstrained, const Vector& sigmas) { void JacobianFactor::setModel(bool anyConstrained, const Vector& sigmas) {
if((size_t) sigmas.size() != this->rows()) if ((size_t) sigmas.size() != this->rows())
throw InvalidNoiseModel(this->rows(), sigmas.size()); throw InvalidNoiseModel(this->rows(), sigmas.size());
if (anyConstrained) if (anyConstrained)
model_ = noiseModel::Constrained::MixedSigmas(sigmas); model_ = noiseModel::Constrained::MixedSigmas(sigmas);
else else
model_ = noiseModel::Diagonal::Sigmas(sigmas); model_ = noiseModel::Diagonal::Sigmas(sigmas);
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> > std::pair<boost::shared_ptr<GaussianConditional>,
EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys) boost::shared_ptr<JacobianFactor> > EliminateQR(
{ const GaussianFactorGraph& factors, const Ordering& keys) {
gttic(EliminateQR); gttic(EliminateQR);
// Combine and sort variable blocks in elimination order // Combine and sort variable blocks in elimination order
JacobianFactor::shared_ptr jointFactor; JacobianFactor::shared_ptr jointFactor;
try { try {
jointFactor = boost::make_shared<JacobianFactor>(factors, keys); jointFactor = boost::make_shared<JacobianFactor>(factors, keys);
} catch(std::invalid_argument&) { } catch (std::invalid_argument&) {
throw InvalidDenseElimination( throw InvalidDenseElimination(
"EliminateQR was called with a request to eliminate variables that are not\n" "EliminateQR was called with a request to eliminate variables that are not\n"
"involved in the provided factors."); "involved in the provided factors.");
@ -629,7 +654,7 @@ namespace gtsam {
// Do dense elimination // Do dense elimination
SharedDiagonal noiseModel; SharedDiagonal noiseModel;
if(jointFactor->model_) if (jointFactor->model_)
jointFactor->model_ = jointFactor->model_->QR(jointFactor->Ab_.matrix()); jointFactor->model_ = jointFactor->model_->QR(jointFactor->Ab_.matrix());
else else
inplace_QR(jointFactor->Ab_.matrix()); inplace_QR(jointFactor->Ab_.matrix());
@ -638,18 +663,20 @@ namespace gtsam {
jointFactor->Ab_.matrix().triangularView<Eigen::StrictlyLower>().setZero(); jointFactor->Ab_.matrix().triangularView<Eigen::StrictlyLower>().setZero();
// Split elimination result into conditional and remaining factor // Split elimination result into conditional and remaining factor
GaussianConditional::shared_ptr conditional = jointFactor->splitConditional(keys.size()); GaussianConditional::shared_ptr conditional = jointFactor->splitConditional(
keys.size());
return make_pair(conditional, jointFactor); return make_pair(conditional, jointFactor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFrontals) GaussianConditional::shared_ptr JacobianFactor::splitConditional(
{ size_t nrFrontals) {
gttic(JacobianFactor_splitConditional); gttic(JacobianFactor_splitConditional);
if(nrFrontals > size()) if (nrFrontals > size())
throw std::invalid_argument("Requesting to split more variables than exist using JacobianFactor::splitConditional"); throw std::invalid_argument(
"Requesting to split more variables than exist using JacobianFactor::splitConditional");
DenseIndex frontalDim = Ab_.range(0, nrFrontals).cols(); DenseIndex frontalDim = Ab_.range(0, nrFrontals).cols();
@ -658,18 +685,20 @@ namespace gtsam {
const DenseIndex originalRowEnd = Ab_.rowEnd(); const DenseIndex originalRowEnd = Ab_.rowEnd();
Ab_.rowEnd() = Ab_.rowStart() + frontalDim; Ab_.rowEnd() = Ab_.rowStart() + frontalDim;
SharedDiagonal conditionalNoiseModel; SharedDiagonal conditionalNoiseModel;
if(model_) { if (model_) {
if((DenseIndex)model_->dim() < frontalDim) if ((DenseIndex) model_->dim() < frontalDim)
throw IndeterminantLinearSystemException(this->keys().front()); throw IndeterminantLinearSystemException(this->keys().front());
conditionalNoiseModel = conditionalNoiseModel = noiseModel::Diagonal::Sigmas(
noiseModel::Diagonal::Sigmas(model_->sigmas().segment(Ab_.rowStart(), Ab_.rows())); model_->sigmas().segment(Ab_.rowStart(), Ab_.rows()));
} }
GaussianConditional::shared_ptr conditional = boost::make_shared<GaussianConditional>( GaussianConditional::shared_ptr conditional = boost::make_shared<
Base::keys_, nrFrontals, Ab_, conditionalNoiseModel); GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel);
const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd) - Ab_.rowStart() - frontalDim; const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd)
- Ab_.rowStart() - frontalDim;
const DenseIndex remainingRows = const DenseIndex remainingRows =
model_ ? std::min(model_->sigmas().size() - frontalDim, maxRemainingRows) model_ ?
: maxRemainingRows; std::min(model_->sigmas().size() - frontalDim, maxRemainingRows) :
maxRemainingRows;
Ab_.rowStart() += frontalDim; Ab_.rowStart() += frontalDim;
Ab_.rowEnd() = Ab_.rowStart() + remainingRows; Ab_.rowEnd() = Ab_.rowStart() + remainingRows;
Ab_.firstBlock() += nrFrontals; Ab_.firstBlock() += nrFrontals;
@ -679,16 +708,18 @@ namespace gtsam {
gttic(remaining_factor); gttic(remaining_factor);
keys_.erase(begin(), begin() + nrFrontals); keys_.erase(begin(), begin() + nrFrontals);
// Set sigmas with the right model // Set sigmas with the right model
if(model_) { if (model_) {
if (model_->isConstrained()) if (model_->isConstrained())
model_ = noiseModel::Constrained::MixedSigmas(model_->sigmas().tail(remainingRows)); model_ = noiseModel::Constrained::MixedSigmas(
model_->sigmas().tail(remainingRows));
else else
model_ = noiseModel::Diagonal::Sigmas(model_->sigmas().tail(remainingRows)); model_ = noiseModel::Diagonal::Sigmas(
model_->sigmas().tail(remainingRows));
assert(model_->dim() == (size_t)Ab_.rows()); assert(model_->dim() == (size_t)Ab_.rows());
} }
gttoc(remaining_factor); gttoc(remaining_factor);
return conditional; return conditional;
} }
} }