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

Add some changes

release/4.3a0
Sungtae An 2014-11-14 14:10:08 -05:00
parent ee3c7ce182
commit 03ebcb6185
2 changed files with 110 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
gtsam/slam/RegularHessianFactor.h
View File

@ -31,9 +31,6 @@ private:
typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block
typedef Eigen::Matrix<double, D, 1> VectorD;
// Use eigen magic to access raw memory
typedef Eigen::Map<VectorD> DMap;
typedef Eigen::Map<const VectorD> ConstDMap;
public:
@ -54,30 +51,57 @@ public:
HessianFactor(keys, augmentedInformation) {
}
/** Return the diagonal of the Hessian for this factor */
VectorValues hessianDiagonal() const {
return HessianFactor::hessianDiagonal();
}
/** Return the diagonal of the Hessian for this factor (raw memory version) */
void hessianDiagonal(double* d) const {
// Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal
const Matrix& B = info_(pos, pos).selfadjointView();
DMap(d + D * j) += B.diagonal();
}
}
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const {
HessianFactor::multiplyHessianAdd(alpha, x, y);
}
// Scratch space for multiplyHessianAdd
typedef Eigen::Matrix<double, D, 1> DVector;
mutable std::vector<DVector> y;
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)
yi.setZero();
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
// 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);
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);
// blocks on the diagonal are only half
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);
}
// copy to yvalues
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
Key key = keys_[i];
DMap(yvalues + key * D) += alpha * y[i];
}
}
void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
std::vector<size_t> offsets) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
// Create a vector of temporary y values, corresponding to rows i
std::vector<Vector> y;
y.reserve(size());
@ -110,52 +134,45 @@ public:
alpha * y[i];
}
// Scratch space for multiplyHessianAdd
mutable std::vector<VectorD> y;
/** Return the diagonal of the Hessian for this factor */
VectorValues hessianDiagonal() const {
return HessianFactor::hessianDiagonal();
}
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(VectorD & yi, y)
yi.setZero();
/** Return the diagonal of the Hessian for this factor (raw memory version) */
void hessianDiagonal(double* d) const {
// 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
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);
// blocks on the diagonal are only half
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);
}
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, 9, 1> DVector;
typedef Eigen::Map<DVector> DMap;
// copy to yvalues
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
Key key = keys_[i];
DMap(yvalues + key * D) += alpha * y[i];
// Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal
const Matrix& B = info_(pos, pos).selfadjointView();
DMap(d + 9 * j) += B.diagonal();
}
}
/** eta for Hessian */
VectorValues gradientAtZero() const {
return HessianFactor::gradientAtZero();
}
/** eta for Hessian (raw memory version) */
/* ************************************************************************* */
// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
void gradientAtZero(double* d) const {
// Loop over all variables in the factor
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, 9, 1> DVector;
typedef Eigen::Map<DVector> DMap;
// Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal
VectorD dj = -info_(pos,size()).knownOffDiagonal();
DMap(d + D * j) += dj;
DMap(d + 9 * j) += dj;
}
}

70
gtsam/slam/RegularJacobianFactor.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp>
#include <vector>
@ -27,14 +28,6 @@ namespace gtsam {
template<size_t D>
class RegularJacobianFactor: public JacobianFactor {
private:
typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block
typedef Eigen::Matrix<double, D, 1> VectorD;
// Use eigen magic to access raw memory
typedef Eigen::Map<VectorD> DMap;
typedef Eigen::Map<const VectorD> ConstDMap;
public:
/** Construct an n-ary factor
@ -57,24 +50,6 @@ public:
JacobianFactor(keys, augmentedMatrix, sigmas) {
}
/// Return the diagonal of the Hessian for this factor
VectorValues hessianDiagonal() const {
return JacobianFactor::hessianDiagonal();
}
/// Raw memory access version of hessianDiagonal
void hessianDiagonal(double* d) const {
// 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 < D; ++k)
dj(k) = Ab_(j).col(k).squaredNorm();
DMap(d + D * j) += dj;
}
}
/// y += alpha * A'*A*x
void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const {
@ -83,16 +58,24 @@ public:
void multiplyHessianAdd(double alpha, const double* x, double* y,
std::vector<size_t> keys) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
if (empty())
return;
Vector Ax = zero(Ab_.rows());
// Just iterate over all A matrices and multiply in correct config part
for (size_t pos = 0; pos < size(); ++pos)
{
std::cout << "pos: " << pos << " keys_[pos]: " << keys_[pos] << " keys[keys_[pos]]: " << keys[keys_[pos]] << std::endl;
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
if (model_) {
model_->whitenInPlace(Ax);
@ -109,6 +92,12 @@ public:
}
void multiplyHessianAdd(double alpha, const double* x, double* y) const {
// 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;
if (empty()) return;
Vector Ax = zero(Ab_.rows());
@ -127,6 +116,33 @@ public:
DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
}
/// Return the diagonal of the Hessian for this factor
VectorValues hessianDiagonal() const {
return JacobianFactor::hessianDiagonal();
}
/// Raw memory access version of hessianDiagonal
/* ************************************************************************* */
// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
void hessianDiagonal(double* d) const {
// Use eigen magic to access raw memory
//typedef Eigen::Matrix<double, 9, 1> DVector;
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
// 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)
for (size_t k = 0; k < D; ++k)
dj(k) = Ab_(j).col(k).squaredNorm();
//DMap(d + 9 * j) += dj;
DMap(d + D * j) += dj;
}
}
VectorValues gradientAtZero() const {
return JacobianFactor::gradientAtZero();
}