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

Remove raw memory access codes

release/4.3a0
Sungtae An 2014-11-12 04:21:46 -05:00
parent 84f6018481
commit fe7fc8a6ef
2 changed files with 0 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
gtsam/linear/JacobianFactor.cpp
View File

@ -460,25 +460,6 @@ VectorValues JacobianFactor::hessianDiagonal() const {
return d; return d;
} }
/* ************************************************************************* */
// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
void JacobianFactor::hessianDiagonal(double* d) const {
// 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 j = 0; j < (DenseIndex) size(); ++j) {
// Get the diagonal block, and insert its diagonal
DVector dj;
for (size_t k = 0; k < 9; ++k)
dj(k) = Ab_(j).col(k).squaredNorm();
DMap(d + 9 * j) += dj;
}
}
/* ************************************************************************* */ /* ************************************************************************* */
map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const { map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
map<Key, Matrix> blocks; map<Key, Matrix> blocks;
@ -528,40 +509,6 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
transposeMultiplyAdd(alpha, Ax, y); transposeMultiplyAdd(alpha, Ax, y);
} }
void JacobianFactor::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)
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);
model_->whitenInPlace(Ax);
}
// multiply with alpha
Ax *= alpha;
// Again iterate over all A matrices and insert Ai^e into y
for (size_t pos = 0; pos < size(); ++pos)
DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_(
pos).transpose() * Ax;
}
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues JacobianFactor::gradientAtZero() const { VectorValues JacobianFactor::gradientAtZero() const {
VectorValues g; VectorValues g;
@ -573,11 +520,6 @@ VectorValues JacobianFactor::gradientAtZero() const {
return g; return g;
} }
/* ************************************************************************* */
void JacobianFactor::gradientAtZero(double* d) const {
//throw std::runtime_error("gradientAtZero not implemented for Jacobian factor");
}
/* ************************************************************************* */ /* ************************************************************************* */
pair<Matrix, Vector> JacobianFactor::jacobian() const { pair<Matrix, Vector> JacobianFactor::jacobian() const {
pair<Matrix, Vector> result = jacobianUnweighted(); pair<Matrix, Vector> result = jacobianUnweighted();

10
gtsam/linear/JacobianFactor.h
View File

@ -185,9 +185,6 @@ namespace gtsam {
/// Return the diagonal of the Hessian for this factor /// Return the diagonal of the Hessian for this factor
virtual VectorValues hessianDiagonal() const; virtual VectorValues hessianDiagonal() const;
/// Raw memory access version of hessianDiagonal
virtual void hessianDiagonal(double* d) const;
/// Return the block diagonal of the Hessian for this factor /// Return the block diagonal of the Hessian for this factor
virtual std::map<Key,Matrix> hessianBlockDiagonal() const; virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
@ -279,16 +276,9 @@ namespace gtsam {
/** y += alpha * A'*A*x */ /** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const;
void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys) const;
void multiplyHessianAdd(double alpha, const double* x, double* y) const {};
/// A'*b for Jacobian /// A'*b for Jacobian
VectorValues gradientAtZero() const; VectorValues gradientAtZero() const;
/* ************************************************************************* */
virtual void gradientAtZero(double* d) const;
/** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */
JacobianFactor whiten() const; JacobianFactor whiten() const;