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

Sidestep Scatter altogether and just use HessianFactor keys_. In theory, n^3 lookup cost, but in practice (as keys is contiguous memory) just as fast as map.

release/4.3a0
Frank Dellaert 2015-06-13 12:06:13 -07:00
parent 08f30966dd
commit f6575323d6
8 changed files with 39 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/linear/GaussianFactor.h
View File

@ -126,7 +126,7 @@ namespace gtsam {
* @param scatter A mapping from variable index to slot index in this HessianFactor
* @param info The information matrix to be updated
*/
virtual void updateHessian(const Scatter& scatter,
virtual void updateHessian(const FastVector<Key>& keys,
SymmetricBlockMatrix* info) const = 0;
/// y += alpha * A'*A*x
@ -141,6 +141,12 @@ namespace gtsam {
/// Gradient wrt a key at any values
virtual Vector gradient(Key key, const VectorValues& x) const = 0;
// Determine position of a given key
template <typename CONTAINER>
static DenseIndex Slot(const CONTAINER& keys, Key key) {
return std::find(keys.begin(), keys.end(), key) - keys.begin();
}
private:
/** Serialization function */
friend class boost::serialization::access;
@ -150,7 +156,7 @@ namespace gtsam {
}
}; // GaussianFactor
/// traits
template<>
struct traits<GaussianFactor> : public Testable<GaussianFactor> {

8
gtsam/linear/HessianFactor.cpp
View File

@ -248,7 +248,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
gttic(update);
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
if(factor)
factor->updateHessian(*scatter, &info_);
factor->updateHessian(keys_, &info_);
gttoc(update);
}
@ -346,15 +346,15 @@ double HessianFactor::error(const VectorValues& c) const {
}
/* ************************************************************************* */
void HessianFactor::updateHessian(const Scatter& scatter,
void HessianFactor::updateHessian(const FastVector<Key>& infoKeys,
SymmetricBlockMatrix* info) const {
gttic(updateHessian_HessianFactor);
// Apply updates to the upper triangle
DenseIndex n = size(), N = info->nBlocks()-1;
for (DenseIndex j = 0; j <= n; ++j) {
const DenseIndex J = j==n ? N : scatter.slot(keys_[j]);
const DenseIndex J = (j==n) ? N : Slot(infoKeys, keys_[j]);
for (DenseIndex i = 0; i <= j; ++i) {
const DenseIndex I = i==n ? N : scatter.slot(keys_[i]);
const DenseIndex I = (i==n) ? N : Slot(infoKeys, keys_[i]);
(*info)(I, J) += info_(i, j);
}
}

2
gtsam/linear/HessianFactor.h
View File

@ -344,7 +344,7 @@ namespace gtsam {
* @param scatter A mapping from variable index to slot index in this HessianFactor
* @param info The information matrix to be updated
*/
void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const;
void updateHessian(const FastVector<Key>& keys, SymmetricBlockMatrix* info) const;
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const;

8
gtsam/linear/JacobianFactor.cpp
View File

@ -498,7 +498,7 @@ map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
}
/* ************************************************************************* */
void JacobianFactor::updateHessian(const Scatter& scatter,
void JacobianFactor::updateHessian(const FastVector<Key>& infoKeys,
SymmetricBlockMatrix* info) const {
gttic(updateHessian_JacobianFactor);
@ -512,7 +512,7 @@ void JacobianFactor::updateHessian(const Scatter& scatter,
"JacobianFactor::updateHessian: cannot update information with "
"constrained noise model");
JacobianFactor whitenedFactor = whiten();
whitenedFactor.updateHessian(scatter, info);
whitenedFactor.updateHessian(infoKeys, info);
} else {
// Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below
DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1;
@ -520,10 +520,10 @@ void JacobianFactor::updateHessian(const Scatter& scatter,
// Apply updates to the upper triangle
// Loop over blocks of A, including RHS with j==n
for (DenseIndex j = 0; j <= n; ++j) {
const DenseIndex J = j==n ? N : scatter.slot(keys_[j]);
const DenseIndex J = (j==n) ? N : Slot(infoKeys, keys_[j]);
// Fill off-diagonal blocks with Ai'*Aj
for (DenseIndex i = 0; i < j; ++i) {
const DenseIndex I = scatter.slot(keys_[i]);
const DenseIndex I = Slot(infoKeys, keys_[i]);
(*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j);
}
// Fill diagonal block with Aj'*Aj

2
gtsam/linear/JacobianFactor.h
View File

@ -278,7 +278,7 @@ namespace gtsam {
* @param scatter A mapping from variable index to slot index in this HessianFactor
* @param info The information matrix to be updated
*/
void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const;
void updateHessian(const FastVector<Key>& keys, SymmetricBlockMatrix* info) const;
/** Return A*x */
Vector operator*(const VectorValues& x) const;

18
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -38,6 +38,16 @@ using namespace gtsam;
const double tol = 1e-5;
/* ************************************************************************* */
TEST(HessianFactor, Slot)
{
FastVector<Key> keys = list_of(2)(4)(1);
EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2));
EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4));
EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1));
EXPECT_LONGS_EQUAL(3, GaussianFactor::Slot(keys,5)); // does not exist
}
/* ************************************************************************* */
TEST(HessianFactor, emptyConstructor)
{
@ -302,15 +312,17 @@ TEST(HessianFactor, CombineAndEliminate)
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Matrix93 A0; A0 << A10, Z_3x3, Z_3x3;
Matrix93 A1; A1 << A11, A01, A21;
Vector9 b; b << b1, b0, b2;
Vector9 sigmas; sigmas << s1, s0, s2;
// create a full, uneliminated version of the factor
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
// Make sure combining works
EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6));
// perform elimination on jacobian
GaussianConditional::shared_ptr expectedConditional;
JacobianFactor::shared_ptr expectedRemainingFactor;

13
gtsam/slam/GeneralSFMFactor.h
View File

@ -149,7 +149,7 @@ namespace gtsam {
: JacobianFactor(i1, A1, i2, A2, b, model), AC_(A1), AL_(A2), b_(b) {}
// Fixed-size matrix update
void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const {
void updateHessian(const FastVector<Key>& infoKeys, SymmetricBlockMatrix* info) const {
gttic(updateHessian_LinearizedFactor);
// Whiten the factor if it has a noise model
@ -160,15 +160,12 @@ namespace gtsam {
"JacobianFactor::updateHessian: cannot update information with "
"constrained noise model");
JacobianFactor whitenedFactor = whiten();
whitenedFactor.updateHessian(scatter, info);
whitenedFactor.updateHessian(infoKeys, info);
} else {
// N is number of variables in information matrix
DenseIndex N = info->nBlocks() - 1;
// First build an array of slots
DenseIndex slotC = scatter.slot(keys_.front());
DenseIndex slotL = scatter.slot(keys_.back());
DenseIndex slotB = N;
DenseIndex slotC = Slot(infoKeys, keys_.front());
DenseIndex slotL = Slot(infoKeys, keys_.back());
DenseIndex slotB = info->nBlocks() - 1;
// We perform I += A'*A to the upper triangle
(*info)(slotC, slotC).knownOffDiagonal() += AC_.transpose() * AC_;

2
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -115,7 +115,7 @@ public:
return D;
}
virtual void updateHessian(const Scatter& scatter,
virtual void updateHessian(const FastVector<Key>& keys,
SymmetricBlockMatrix* info) const {
throw std::runtime_error(
"RegularImplicitSchurFactor::updateHessian non implemented");