Refactoring to get grouped factors to compile again
Frank Dellaert
parent
b68f763fe7
commit
ca2aa0cfd4
|
|
@ -19,6 +19,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/linear/HessianFactor.h>
|
||||
#include <gtsam/slam/RegularJacobianFactor.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <vector>
|
||||
|
||||
|
|
@ -27,15 +28,11 @@ namespace gtsam {
|
|||
template<size_t D>
|
||||
class RegularHessianFactor: public HessianFactor {
|
||||
|
||||
private:
|
||||
|
||||
// Use Eigen magic to access raw memory
|
||||
typedef Eigen::Matrix<double, D, 1> DVector;
|
||||
typedef Eigen::Map<DVector> DMap;
|
||||
typedef Eigen::Map<const DVector> ConstDMap;
|
||||
|
||||
public:
|
||||
|
||||
typedef Eigen::Matrix<double, D, 1> VectorD;
|
||||
typedef Eigen::Matrix<double, D, D> MatrixD;
|
||||
|
||||
/** Construct an n-way factor. Gs contains the upper-triangle blocks of the
|
||||
* quadratic term (the Hessian matrix) provided in row-order, gs the pieces
|
||||
* of the linear vector term, and f the constant term.
|
||||
|
|
@ -43,27 +40,68 @@ public:
|
|||
RegularHessianFactor(const std::vector<Key>& js,
|
||||
const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
|
||||
HessianFactor(js, Gs, gs, f) {
|
||||
checkInvariants();
|
||||
}
|
||||
|
||||
/** Construct a binary factor. Gxx are the upper-triangle blocks of the
|
||||
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
|
||||
* term, and f the constant term.
|
||||
*/
|
||||
RegularHessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12,
|
||||
const Vector& g1, const Matrix& G22, const Vector& g2, double f) :
|
||||
RegularHessianFactor(Key j1, Key j2, const MatrixD& G11, const MatrixD& G12,
|
||||
const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) :
|
||||
HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) {
|
||||
}
|
||||
|
||||
/** Construct a ternary factor. Gxx are the upper-triangle blocks of the
|
||||
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
|
||||
* term, and f the constant term.
|
||||
*/
|
||||
RegularHessianFactor(Key j1, Key j2, Key j3,
|
||||
const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& g1,
|
||||
const MatrixD& G22, const MatrixD& G23, const VectorD& g2,
|
||||
const MatrixD& G33, const VectorD& g3, double f) :
|
||||
HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) {
|
||||
}
|
||||
|
||||
/** Constructor with an arbitrary number of keys and with the augmented information matrix
|
||||
* specified as a block matrix. */
|
||||
template<typename KEYS>
|
||||
RegularHessianFactor(const KEYS& keys,
|
||||
const SymmetricBlockMatrix& augmentedInformation) :
|
||||
HessianFactor(keys, augmentedInformation) {
|
||||
checkInvariants();
|
||||
}
|
||||
|
||||
/// Construct from RegularJacobianFactor
|
||||
RegularHessianFactor(const RegularJacobianFactor<D>& jf)
|
||||
: HessianFactor(jf) {}
|
||||
|
||||
/// Construct from a GaussianFactorGraph
|
||||
RegularHessianFactor(const GaussianFactorGraph& factors,
|
||||
boost::optional<const Scatter&> scatter = boost::none)
|
||||
: HessianFactor(factors, scatter) {
|
||||
checkInvariants();
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/// Check invariants after construction
|
||||
void checkInvariants() {
|
||||
if (info_.cols() != 1 + (info_.nBlocks()-1) * (DenseIndex)D)
|
||||
throw std::invalid_argument(
|
||||
"RegularHessianFactor constructor was given non-regular factors");
|
||||
}
|
||||
|
||||
// Use Eigen magic to access raw memory
|
||||
typedef Eigen::Map<VectorD> DMap;
|
||||
typedef Eigen::Map<const VectorD> ConstDMap;
|
||||
|
||||
// Scratch space for multiplyHessianAdd
|
||||
mutable std::vector<DVector> y;
|
||||
// According to link below this is thread-safe.
|
||||
// http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe
|
||||
mutable std::vector<VectorD> y_;
|
||||
|
||||
public:
|
||||
|
||||
/** y += alpha * A'*A*x */
|
||||
virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
|
||||
|
|
@ -74,32 +112,32 @@ public:
|
|||
/** y += alpha * A'*A*x */
|
||||
void multiplyHessianAdd(double alpha, const double* x,
|
||||
double* yvalues) const {
|
||||
// Create a vector of temporary y values, corresponding to rows i
|
||||
y.resize(size());
|
||||
BOOST_FOREACH(DVector & yi, y)
|
||||
// Create a vector of temporary y_ values, corresponding to rows i
|
||||
y_.resize(size());
|
||||
BOOST_FOREACH(VectorD & yi, y_)
|
||||
yi.setZero();
|
||||
|
||||
// Accessing the VectorValues one by one is expensive
|
||||
// So we will loop over columns to access x only once per column
|
||||
// And fill the above temporary y values, to be added into yvalues after
|
||||
DVector xj(D);
|
||||
// And fill the above temporary y_ values, to be added into yvalues after
|
||||
VectorD xj(D);
|
||||
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
|
||||
Key key = keys_[j];
|
||||
const double* xj = x + key * D;
|
||||
DenseIndex i = 0;
|
||||
for (; i < j; ++i)
|
||||
y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj);
|
||||
y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj);
|
||||
// blocks on the diagonal are only half
|
||||
y[i] += info_(j, j).selfadjointView() * ConstDMap(xj);
|
||||
y_[i] += info_(j, j).selfadjointView() * ConstDMap(xj);
|
||||
// for below diagonal, we take transpose block from upper triangular part
|
||||
for (i = j + 1; i < (DenseIndex) size(); ++i)
|
||||
y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj);
|
||||
y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj);
|
||||
}
|
||||
|
||||
// copy to yvalues
|
||||
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
|
||||
Key key = keys_[i];
|
||||
DMap(yvalues + key * D) += alpha * y[i];
|
||||
DMap(yvalues + key * D) += alpha * y_[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -107,28 +145,27 @@ public:
|
|||
void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
|
||||
std::vector<size_t> offsets) const {
|
||||
|
||||
// Create a vector of temporary y values, corresponding to rows i
|
||||
std::vector<Vector> y;
|
||||
y.reserve(size());
|
||||
for (const_iterator it = begin(); it != end(); it++)
|
||||
y.push_back(zero(getDim(it)));
|
||||
// Create a vector of temporary y_ values, corresponding to rows i
|
||||
y_.resize(size());
|
||||
BOOST_FOREACH(VectorD & yi, y_)
|
||||
yi.setZero();
|
||||
|
||||
// Accessing the VectorValues one by one is expensive
|
||||
// So we will loop over columns to access x only once per column
|
||||
// And fill the above temporary y values, to be added into yvalues after
|
||||
// And fill the above temporary y_ values, to be added into yvalues after
|
||||
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
|
||||
DenseIndex i = 0;
|
||||
for (; i < j; ++i)
|
||||
y[i] += info_(i, j).knownOffDiagonal()
|
||||
y_[i] += info_(i, j).knownOffDiagonal()
|
||||
* ConstDMap(x + offsets[keys_[j]],
|
||||
offsets[keys_[j] + 1] - offsets[keys_[j]]);
|
||||
// blocks on the diagonal are only half
|
||||
y[i] += info_(j, j).selfadjointView()
|
||||
y_[i] += info_(j, j).selfadjointView()
|
||||
* ConstDMap(x + offsets[keys_[j]],
|
||||
offsets[keys_[j] + 1] - offsets[keys_[j]]);
|
||||
// for below diagonal, we take transpose block from upper triangular part
|
||||
for (i = j + 1; i < (DenseIndex) size(); ++i)
|
||||
y[i] += info_(i, j).knownOffDiagonal()
|
||||
y_[i] += info_(i, j).knownOffDiagonal()
|
||||
* ConstDMap(x + offsets[keys_[j]],
|
||||
offsets[keys_[j] + 1] - offsets[keys_[j]]);
|
||||
}
|
||||
|
|
@ -136,7 +173,7 @@ public:
|
|||
// copy to yvalues
|
||||
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i)
|
||||
DMap(yvalues + offsets[keys_[i]],
|
||||
offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y[i];
|
||||
offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y_[i];
|
||||
}
|
||||
|
||||
/** Return the diagonal of the Hessian for this factor (raw memory version) */
|
||||
|
|
@ -158,7 +195,7 @@ public:
|
|||
for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
|
||||
Key j = keys_[pos];
|
||||
// Get the diagonal block, and insert its diagonal
|
||||
DVector dj = -info_(pos, size()).knownOffDiagonal();
|
||||
VectorD dj = -info_(pos, size()).knownOffDiagonal();
|
||||
DMap(d + D * j) += dj;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -16,6 +16,7 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/slam/RegularHessianFactor.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -29,30 +30,58 @@ using namespace gtsam;
|
|||
using namespace boost::assign;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(RegularHessianFactor, ConstructorNWay)
|
||||
TEST(RegularHessianFactor, Constructors)
|
||||
{
|
||||
Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114).finished();
|
||||
Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124).finished();
|
||||
Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134).finished();
|
||||
// First construct a regular JacobianFactor
|
||||
Matrix A1 = I_2x2, A2 = I_2x2, A3 = I_2x2;
|
||||
Vector2 b(1,2);
|
||||
vector<pair<Key, Matrix> > terms;
|
||||
terms += make_pair(0, A1), make_pair(1, A2), make_pair(3, A3);
|
||||
RegularJacobianFactor<2> jf(terms, b);
|
||||
|
||||
Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224).finished();
|
||||
Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234).finished();
|
||||
// Test conversion from JacobianFactor
|
||||
RegularHessianFactor<2> factor(jf);
|
||||
|
||||
Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334).finished();
|
||||
Matrix G11 = I_2x2;
|
||||
Matrix G12 = I_2x2;
|
||||
Matrix G13 = I_2x2;
|
||||
|
||||
Vector g1 = (Vector(2) << -7, -9).finished();
|
||||
Vector g2 = (Vector(2) << -9, 1).finished();
|
||||
Vector g3 = (Vector(2) << 2, 3).finished();
|
||||
Matrix G22 = I_2x2;
|
||||
Matrix G23 = I_2x2;
|
||||
|
||||
Matrix G33 = I_2x2;
|
||||
|
||||
Vector2 g1 = b, g2 = b, g3 = b;
|
||||
|
||||
double f = 10;
|
||||
|
||||
std::vector<Key> js;
|
||||
js.push_back(0); js.push_back(1); js.push_back(3);
|
||||
std::vector<Matrix> Gs;
|
||||
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;
|
||||
gs.push_back(g1); gs.push_back(g2); gs.push_back(g3);
|
||||
RegularHessianFactor<2> factor(js, Gs, gs, f);
|
||||
// Test ternary constructor
|
||||
RegularHessianFactor<2> factor2(0, 1, 3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
|
||||
EXPECT(assert_equal(factor,factor2));
|
||||
|
||||
// Test n-way constructor
|
||||
vector<Key> keys; keys += 0, 1, 3;
|
||||
vector<Matrix> Gs; Gs += G11, G12, G13, G22, G23, G33;
|
||||
vector<Vector> gs; gs += g1, g2, g3;
|
||||
RegularHessianFactor<2> factor3(keys, Gs, gs, f);
|
||||
EXPECT(assert_equal(factor, factor3));
|
||||
|
||||
// Test constructor from Gaussian Factor Graph
|
||||
GaussianFactorGraph gfg;
|
||||
gfg += jf;
|
||||
RegularHessianFactor<2> factor4(gfg);
|
||||
EXPECT(assert_equal(factor, factor4));
|
||||
GaussianFactorGraph gfg2;
|
||||
gfg2 += factor;
|
||||
RegularHessianFactor<2> factor5(gfg);
|
||||
EXPECT(assert_equal(factor, factor5));
|
||||
|
||||
// Test constructor from Information matrix
|
||||
Matrix info = factor.augmentedInformation();
|
||||
vector<size_t> dims; dims += 2, 2, 2;
|
||||
SymmetricBlockMatrix sym(dims, info, true);
|
||||
RegularHessianFactor<2> factor6(keys, sym);
|
||||
EXPECT(assert_equal(factor, factor6));
|
||||
|
||||
// multiplyHessianAdd:
|
||||
{
|
||||
|
|
@ -61,13 +90,13 @@ TEST(RegularHessianFactor, ConstructorNWay)
|
|||
HessianFactor::const_iterator i1 = factor.begin();
|
||||
HessianFactor::const_iterator i2 = i1 + 1;
|
||||
Vector X(6); X << 1,2,3,4,5,6;
|
||||
Vector Y(6); Y << 2633, 2674, 4465, 4501, 5669, 5696;
|
||||
Vector Y(6); Y << 9, 12, 9, 12, 9, 12;
|
||||
EXPECT(assert_equal(Y,AtA*X));
|
||||
|
||||
VectorValues x = map_list_of<Key, Vector>
|
||||
(0, (Vector(2) << 1,2).finished())
|
||||
(1, (Vector(2) << 3,4).finished())
|
||||
(3, (Vector(2) << 5,6).finished());
|
||||
(0, Vector2(1,2))
|
||||
(1, Vector2(3,4))
|
||||
(3, Vector2(5,6));
|
||||
|
||||
VectorValues expected;
|
||||
expected.insert(0, Y.segment<2>(0));
|
||||
|
|
@ -77,15 +106,15 @@ TEST(RegularHessianFactor, ConstructorNWay)
|
|||
// VectorValues version
|
||||
double alpha = 1.0;
|
||||
VectorValues actualVV;
|
||||
actualVV.insert(0, zero(2));
|
||||
actualVV.insert(1, zero(2));
|
||||
actualVV.insert(3, zero(2));
|
||||
actualVV.insert(0, Vector2::Zero());
|
||||
actualVV.insert(1, Vector2::Zero());
|
||||
actualVV.insert(3, Vector2::Zero());
|
||||
factor.multiplyHessianAdd(alpha, x, actualVV);
|
||||
EXPECT(assert_equal(expected, actualVV));
|
||||
|
||||
// RAW ACCESS
|
||||
Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696;
|
||||
Vector fast_y = gtsam::zero(8);
|
||||
Vector expected_y(8); expected_y << 9, 12, 9, 12, 0, 0, 9, 12;
|
||||
Vector fast_y = Vector8::Zero();
|
||||
double xvalues[8] = {1,2,3,4,0,0,5,6};
|
||||
factor.multiplyHessianAdd(alpha, xvalues, fast_y.data());
|
||||
EXPECT(assert_equal(expected_y, fast_y));
|
||||
|
|
|
|||
Loading…
Reference in New Issue