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

Formatting and comments

release/4.3a0
dellaert 2015-02-22 07:06:41 +01:00
parent 772db8850a
commit 78fd7de1b9
1 changed files with 22 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
gtsam/slam/JacobianSchurFactor.h
View File

@ -23,7 +23,9 @@
namespace gtsam { namespace gtsam {
/** /**
* JacobianFactor for Schur complement that uses Q noise model * JacobianFactor for Schur complement
* Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD
* Provides raw memory access versions of linear operator.
*/ */
template<size_t D, size_t ZDim> template<size_t D, size_t ZDim>
class JacobianSchurFactor: public JacobianFactor { class JacobianSchurFactor: public JacobianFactor {
@ -44,10 +46,11 @@ public:
*/ */
Vector operator*(const double* x) const { Vector operator*(const double* x) const {
Vector Ax = zero(Ab_.rows()); Vector Ax = zero(Ab_.rows());
if (empty()) return Ax; if (empty())
return Ax;
// Just iterate over all A matrices and multiply in correct config part // Just iterate over all A matrices and multiply in correct config part
for(size_t pos=0; pos<size(); ++pos) for (size_t pos = 0; pos < size(); ++pos)
Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]); Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
return model_ ? model_->whiten(Ax) : Ax; return model_ ? model_->whiten(Ax) : Ax;
@ -57,17 +60,17 @@ public:
* @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e
* RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way
*/ */
void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const {
{
Vector E = alpha * (model_ ? model_->whiten(e) : e); Vector E = alpha * (model_ ? model_->whiten(e) : e);
// Just iterate over all A matrices and insert Ai^e into y // Just iterate over all A matrices and insert Ai^e into y
for(size_t pos=0; pos<size(); ++pos) for (size_t pos = 0; pos < size(); ++pos)
DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E; DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E;
} }
/** 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,
JacobianFactor::multiplyHessianAdd(alpha,x,y); VectorValues& y) const {
JacobianFactor::multiplyHessianAdd(alpha, x, y);
} }
/** /**
@ -75,26 +78,29 @@ public:
* RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way
*/ */
void multiplyHessianAdd(double alpha, const double* x, double* y) const { void multiplyHessianAdd(double alpha, const double* x, double* y) const {
// Vector Ax = (*this)*x; if (empty())
// this->transposeMultiplyAdd(alpha,Ax,y); return;
if (empty()) return;
Vector Ax = zero(Ab_.rows()); Vector Ax = zero(Ab_.rows());
// Just iterate over all A matrices and multiply in correct config part // Just iterate over all A matrices and multiply in correct config part
for(size_t pos=0; pos<size(); ++pos) for (size_t pos = 0; pos < size(); ++pos)
Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]); Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
// Deal with noise properly, need to Double* whiten as we are dividing by variance // Deal with noise properly, need to Double* whiten as we are dividing by variance
if (model_) { model_->whitenInPlace(Ax); model_->whitenInPlace(Ax); } if (model_) {
model_->whitenInPlace(Ax);
model_->whitenInPlace(Ax);
}
// multiply with alpha // multiply with alpha
Ax *= alpha; Ax *= alpha;
// Again iterate over all A matrices and insert Ai^e into y // Again iterate over all A matrices and insert Ai^e into y
for(size_t pos=0; pos<size(); ++pos) for (size_t pos = 0; pos < size(); ++pos)
DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax; DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
} }
}; // class };
// end class JacobianSchurFactor
} // gtsam }// end namespace gtsam