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

Formatting and redundancy

release/4.3a0
dellaert 2015-02-22 08:09:46 +01:00
parent ecc1cfd15d
commit 3d8f980577
1 changed files with 11 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam/slam/RegularHessianFactor.h
View File

@ -11,9 +11,9 @@
/**
* @file RegularHessianFactor.h
* @brief HessianFactor class with constant sized blcoks
* @author Richard Roberts
* @date Dec 8, 2010
* @brief HessianFactor class with constant sized blocks
* @author Sungtae An
* @date March 2014
*/
#pragma once
@ -29,8 +29,10 @@ class RegularHessianFactor: public HessianFactor {
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::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
public:
@ -61,7 +63,6 @@ public:
}
// Scratch space for multiplyHessianAdd
typedef Eigen::Matrix<double, D, 1> DVector;
mutable std::vector<DVector> y;
/** y += alpha * A'*A*x */
@ -78,9 +79,6 @@ public:
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
@ -109,11 +107,6 @@ public:
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());
@ -149,10 +142,6 @@ public:
/** Return the diagonal of the Hessian for this factor (raw memory version) */
virtual void hessianDiagonal(double* d) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 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];
@ -165,22 +154,19 @@ public:
/// Add gradient at zero to d TODO: is it really the goal to add ??
virtual void gradientAtZero(double* d) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 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();
DVector dj = -info_(pos, size()).knownOffDiagonal();
DMap(d + D * j) += dj;
}
}
/* ************************************************************************* */
}; // end class RegularHessianFactor
};
// end class RegularHessianFactor
// traits
template<size_t D> struct traits<RegularHessianFactor<D> > : public Testable<