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

Remove non raw memory access functions

release/4.3a0
Sungtae An 2014-11-17 16:18:41 -05:00
parent 960d10582d
commit cad6736b72
5 changed files with 21 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
gtsam/slam/RegularHessianFactor.h
View File

@ -51,16 +51,11 @@ public:
HessianFactor(keys, augmentedInformation) {
}
/** 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;
/** 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());
@ -134,16 +129,12 @@ public:
alpha * y[i];
}
/** 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 {
virtual void hessianDiagonal(double* d) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, 9, 1> DVector;
//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
@ -151,20 +142,18 @@ public:
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();
//DMap(d + 9 * j) += B.diagonal();
DMap(d + D * j) += B.diagonal();
}
}
VectorValues gradientAtZero() const {
return HessianFactor::gradientAtZero();
}
/* ************************************************************************* */
// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
void gradientAtZero(double* d) const {
virtual void gradientAtZero(double* d) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, 9, 1> DVector;
//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
@ -172,7 +161,8 @@ public:
Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal
VectorD dj = -info_(pos,size()).knownOffDiagonal();
DMap(d + 9 * j) += dj;
//DMap(d + 9 * j) += dj;
DMap(d + D * j) += dj;
}
}

4
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -160,7 +160,7 @@ public:
* @brief add the contribution of this factor to the diagonal of the hessian
* d(output) = d(input) + deltaHessianFactor
*/
void hessianDiagonal(double* d) const {
virtual void hessianDiagonal(double* d) const {
// diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;
@ -435,7 +435,7 @@ public:
/**
* Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS
*/
void gradientAtZero(double* d) const {
virtual void gradientAtZero(double* d) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;

30
gtsam/slam/RegularJacobianFactor.h
View File

@ -57,13 +57,7 @@ public:
JacobianFactor(keys, augmentedMatrix, sigmas) {
}
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const {
JacobianFactor::multiplyHessianAdd(alpha, x, y);
}
/** Raw memory access version of multiplyHessianAdd
/** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x
* Note: this is not assuming a fixed dimension for the variables,
* but requires the vector accumulatedDims to tell the dimension of
* each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2,
@ -126,16 +120,10 @@ 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 D (templated) and keys arranged from 0 to n
*
*/
void hessianDiagonal(double* d) const {
virtual 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
@ -153,22 +141,18 @@ public:
}
}
/** // Gradient is really -A'*b / sigma^2 */
VectorValues gradientAtZero() const {
return JacobianFactor::gradientAtZero();
}
/** Raw memory access version of gradientAtZero */
void gradientAtZero(double* d) const {
virtual void gradientAtZero(double* d) const {
// Get vector b not weighted
Vector b = getb();
Vector b_sigma = model_ ? (model_->whiten(b)*model_->whiten(b)) : b;
// gradient -= A'*b/sigma^2
// Loop over all variables in the factor
// Just iterate over all A matrices
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
// Get the diagonal block, and insert its diagonal
DMap(d + D * j) = DVector::Zero();
DVector dj;
// gradient -= A'*b/sigma^2
// Computing with each column
for (size_t k = 0; k < D; ++k){
Vector column_k = Ab_(j).col(k);
dj(k) = -1.0*dot(b_sigma,column_k);

9
gtsam/slam/tests/testRegularHessianFactor.cpp
View File

@ -77,15 +77,6 @@ TEST(RegularHessianFactor, ConstructorNWay)
expected.insert(1, Y.segment<2>(2));
expected.insert(3, Y.segment<2>(4));
// VectorValues way
VectorValues actual;
factor.multiplyHessianAdd(1, x, actual);
EXPECT(assert_equal(expected, actual));
// now, do it with non-zero y
factor.multiplyHessianAdd(1, x, actual);
EXPECT(assert_equal(2*expected, actual));
// RAW ACCESS
Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696;
Vector fast_y = gtsam::zero(8);

18
gtsam/slam/tests/testRegularJacobianFactor.cpp
View File

@ -98,12 +98,6 @@ TEST(RegularJacobianFactor, hessianDiagonal)
// we compute hessian diagonal from the standard Jacobian
VectorValues expectedHessianDiagonal = factor.hessianDiagonal();
// we compute hessian diagonal from the regular Jacobian, but still using the
// implementation of hessianDiagonal in the base class
//VectorValues actualHessianDiagonal = regularFactor.hessianDiagonal();
//EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonal));
// we compare against the Raw memory access implementation of hessianDiagonal
double actualValue[9];
regularFactor.hessianDiagonal(actualValue);
@ -122,18 +116,13 @@ TEST(RegularJacobian, gradientAtZero)
// we compute gradient at zero from the standard Jacobian
VectorValues expectedGradientAtZero = factor.gradientAtZero();
// we compute gradient at zero from the regular Jacobian, but still using the
// implementation of gradientAtZero in the base class
VectorValues actualGradientAtZero = regularFactor.gradientAtZero();
EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero()));
//EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero()));
// we compare against the Raw memory access implementation of gradientAtZero
double actualValue[9];
regularFactor.gradientAtZero(actualValue);
VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim);
EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw));
}
/* ************************************************************************* */
@ -161,11 +150,6 @@ TEST(RegularJacobian, multiplyHessianAdd)
VectorValues expectedMHA = Y;
factor.multiplyHessianAdd(alpha, X, expectedMHA);
VectorValues actualMHA = Y;
regularFactor.multiplyHessianAdd(alpha, X, actualMHA);
EXPECT(assert_equal(expectedMHA, actualMHA));
// create data for raw memory access
double XRaw[9];
vv2double(X, XRaw, nrKeys, fixedDim);