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

Updating Hessian in-place instead of computing Hessian for each Jacobian

release/4.3a0
Richard Roberts 2013-10-03 16:50:16 +00:00
parent 4d31dd99f3
commit 95ac34aeeb
4 changed files with 105 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
gtsam/linear/HessianFactor.cpp
View File

@ -382,7 +382,6 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
// First build an array of slots
gttic(slots);
//size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google.
FastVector<DenseIndex> slots(update.size());
DenseIndex slot = 0;
BOOST_FOREACH(Key j, update) {
@ -409,10 +408,63 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
}
/* ************************************************************************* */
void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter)
{
if(update.rows() > 0) // Zero-row Jacobians are treated specially
updateATA(HessianFactor(update), scatter);
void HessianFactor::updateATA(const JacobianFactor& 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.
gttic(updateATA);
if(update.rows() > 0)
{
// First build an array of slots
gttic(slots);
FastVector<DenseIndex> slots(update.size());
DenseIndex slot = 0;
BOOST_FOREACH(Key j, update) {
slots[slot] = scatter.at(j).slot;
++ slot;
}
gttoc(slots);
gttic(whiten);
// Whiten the factor if it has a noise model
boost::optional<JacobianFactor> _whitenedFactor;
const JacobianFactor* whitenedFactor;
if(update.get_model())
{
if(update.get_model()->isConstrained())
throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model");
_whitenedFactor = update.whiten();
whitenedFactor = &(*_whitenedFactor);
}
else
{
whitenedFactor = &update;
}
gttoc(whiten);
const VerticalBlockMatrix& updateBlocks = whitenedFactor->matrixObject();
// Apply updates to the upper triangle
gttic(update);
for(DenseIndex j2=0; j2<updateBlocks.nBlocks(); ++j2) { // Horizontal block of Hessian
DenseIndex slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
for(DenseIndex j1=0; j1<=j2; ++j1) { // Vertical block of Hessian
DenseIndex slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
DenseIndex off0 = updateBlocks.offset(0);
if(slot2 > slot1)
info_(slot1, slot2).noalias() += updateBlocks(j1).transpose() * updateBlocks(j2);
else if(slot1 > slot2)
info_(slot2, slot1).noalias() += updateBlocks(j2).transpose() * updateBlocks(j1);
else
info_(slot1, slot2).triangularView<Eigen::Upper>() += updateBlocks(j1).transpose() * updateBlocks(j2);
}
}
gttoc(update);
}
}
/* ************************************************************************* */

4
gtsam/linear/JacobianFactor.cpp
View File

@ -481,8 +481,8 @@ namespace gtsam {
JacobianFactor JacobianFactor::whiten() const {
JacobianFactor result(*this);
if(model_) {
result.model_->WhitenInPlace(result.Ab_.matrix());
result.model_ = noiseModel::Unit::Create(result.model_->dim());
result.model_->WhitenInPlace(result.Ab_.full());
result.model_ = SharedDiagonal();
}
return result;
}

27
gtsam/linear/NoiseModel.cpp
View File

@ -106,6 +106,11 @@ void Gaussian::WhitenInPlace(Matrix& H) const {
H = thisR() * H;
}
/* ************************************************************************* */
void Gaussian::WhitenInPlace(Eigen::Block<Matrix>& H) const {
H = thisR() * H;
}
/* ************************************************************************* */
// General QR, see also special version in Constrained
SharedDiagonal Gaussian::QR(Matrix& Ab) const {
@ -232,6 +237,11 @@ void Diagonal::WhitenInPlace(Matrix& H) const {
vector_scale_inplace(invsigmas(), H);
}
/* ************************************************************************* */
void Diagonal::WhitenInPlace(Eigen::Block<Matrix>& H) const {
H = invsigmas().asDiagonal() * H;
}
/* ************************************************************************* */
// Constrained
/* ************************************************************************* */
@ -304,6 +314,18 @@ void Constrained::WhitenInPlace(Matrix& H) const {
vector_scale_inplace(invsigmas(), H, true);
}
/* ************************************************************************* */
void Constrained::WhitenInPlace(Eigen::Block<Matrix>& H) const {
// selective scaling
// Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
// Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
// indicating a hard constraint, we leave H's row i in place.
const Vector& _invsigmas = invsigmas();
for(DenseIndex row = 0; row < _invsigmas.size(); ++row)
if(isfinite(_invsigmas(row)))
H.row(row) *= _invsigmas(row);
}
/* ************************************************************************* */
Constrained::shared_ptr Constrained::unit(size_t augmentedDim) const {
Vector sigmas = ones(dim()+augmentedDim);
@ -443,6 +465,11 @@ void Isotropic::WhitenInPlace(Matrix& H) const {
H *= invsigma_;
}
/* ************************************************************************* */
void Isotropic::WhitenInPlace(Eigen::Block<Matrix>& H) const {
H *= invsigma_;
}
/* ************************************************************************* */
// Unit
/* ************************************************************************* */

19
gtsam/linear/NoiseModel.h
View File

@ -93,6 +93,16 @@ namespace gtsam {
v = unwhiten(v);
}
/** in-place whiten, override if can be done more efficiently */
virtual void whitenInPlace(Eigen::Block<Vector>& v) const {
v = whiten(v);
}
/** in-place unwhiten, override if can be done more efficiently */
virtual void unwhitenInPlace(Eigen::Block<Vector>& v) const {
v = unwhiten(v);
}
private:
/** Serialization function */
friend class boost::serialization::access;
@ -186,6 +196,11 @@ namespace gtsam {
*/
virtual void WhitenInPlace(Matrix& H) const;
/**
* In-place version
*/
virtual void WhitenInPlace(Eigen::Block<Matrix>& H) const;
/**
* Whiten a system, in place as well
*/
@ -282,6 +297,7 @@ namespace gtsam {
virtual Vector unwhiten(const Vector& v) const;
virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
virtual void WhitenInPlace(Eigen::Block<Matrix>& H) const;
/**
* Return standard deviations (sqrt of diagonal)
@ -431,6 +447,7 @@ namespace gtsam {
/// with a non-zero sigma. Other rows remain untouched.
virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
virtual void WhitenInPlace(Eigen::Block<Matrix>& H) const;
/**
* Apply QR factorization to the system [A b], taking into account constraints
@ -510,6 +527,7 @@ namespace gtsam {
virtual Vector unwhiten(const Vector& v) const;
virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
virtual void WhitenInPlace(Eigen::Block<Matrix>& H) const;
/**
* Return standard deviation
@ -557,6 +575,7 @@ namespace gtsam {
virtual Vector unwhiten(const Vector& v) const { return v; }
virtual Matrix Whiten(const Matrix& H) const { return H; }
virtual void WhitenInPlace(Matrix& H) const {}
virtual void WhitenInPlace(Eigen::Block<Matrix>& H) const {}
private:
/** Serialization function */