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 * @file RegularHessianFactor.h
* @brief HessianFactor class with constant sized blcoks * @brief HessianFactor class with constant sized blocks
* @author Richard Roberts * @author Sungtae An
* @date Dec 8, 2010 * @date March 2014
*/ */
#pragma once #pragma once
@ -29,8 +29,10 @@ class RegularHessianFactor: public HessianFactor {
private: private:
typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block // Use Eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> VectorD; typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
public: public:
@ -61,7 +63,6 @@ public:
} }
// Scratch space for multiplyHessianAdd // Scratch space for multiplyHessianAdd
typedef Eigen::Matrix<double, D, 1> DVector;
mutable std::vector<DVector> y; mutable std::vector<DVector> y;
/** y += alpha * A'*A*x */ /** y += alpha * A'*A*x */
@ -78,9 +79,6 @@ public:
BOOST_FOREACH(DVector & yi, y) BOOST_FOREACH(DVector & yi, y)
yi.setZero(); yi.setZero();
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
// Accessing the VectorValues one by one is expensive // Accessing the VectorValues one by one is expensive
// So we will loop over columns to access x only once per column // 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
@ -109,11 +107,6 @@ public:
void multiplyHessianAdd(double alpha, const double* x, double* yvalues, void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
std::vector<size_t> offsets) const { 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 // Create a vector of temporary y values, corresponding to rows i
std::vector<Vector> y; std::vector<Vector> y;
y.reserve(size()); y.reserve(size());
@ -149,10 +142,6 @@ public:
/** Return the diagonal of the Hessian for this factor (raw memory version) */ /** Return the diagonal of the Hessian for this factor (raw memory version) */
virtual void hessianDiagonal(double* d) const { 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 // Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
Key j = keys_[pos]; Key j = keys_[pos];
@ -165,22 +154,19 @@ public:
/// Add gradient at zero to d TODO: is it really the goal to add ?? /// Add gradient at zero to d TODO: is it really the goal to add ??
virtual 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;
typedef Eigen::Map<DVector> DMap;
// Loop over all variables in the factor // Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
Key j = keys_[pos]; Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal // 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; DMap(d + D * j) += dj;
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
}; // end class RegularHessianFactor };
// end class RegularHessianFactor
// traits // traits
template<size_t D> struct traits<RegularHessianFactor<D> > : public Testable< template<size_t D> struct traits<RegularHessianFactor<D> > : public Testable<