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

28
gtsam/slam/JacobianSchurFactor.h
View File

@ -23,7 +23,9 @@
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>
class JacobianSchurFactor: public JacobianFactor {
@ -44,7 +46,8 @@ public:
*/
Vector operator*(const double* x) const {
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
for (size_t pos = 0; pos < size(); ++pos)
@ -57,8 +60,7 @@ public:
* @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
*/
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);
// Just iterate over all A matrices and insert Ai^e into y
for (size_t pos = 0; pos < size(); ++pos)
@ -66,7 +68,8 @@ public:
}
/** 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 {
JacobianFactor::multiplyHessianAdd(alpha, x, y);
}
@ -75,9 +78,8 @@ public:
* 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 {
// Vector Ax = (*this)*x;
// this->transposeMultiplyAdd(alpha,Ax,y);
if (empty()) return;
if (empty())
return;
Vector Ax = zero(Ab_.rows());
// Just iterate over all A matrices and multiply in correct config part
@ -85,7 +87,10 @@ public:
Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
// 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
Ax *= alpha;
@ -95,6 +100,7 @@ public:
DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
}
}; // class
};
// end class JacobianSchurFactor
} // gtsam
}// end namespace gtsam