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

Allow starting with empty VectorValues in JF::transposeMultiplyAdd

release/4.3a0
Richard Roberts 2013-07-16 20:22:07 +00:00
parent 7ad8c15028
commit 223f89d931
2 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/linear/JacobianFactorUnordered.cpp
View File

@ -423,11 +423,18 @@ namespace gtsam {
/* ************************************************************************* */
void JacobianFactorUnordered::transposeMultiplyAdd(double alpha, const Vector& e,
VectorValuesUnordered& x) const {
VectorValuesUnordered& x) const
{
Vector E = alpha * model_->whiten(e);
// Just iterate over all A matrices and insert Ai^e into VectorValues
for(size_t pos=0; pos<size(); ++pos)
gtsam::transposeMultiplyAdd(1.0, Ab_(pos), E, x[keys_[pos]]);
{
// Create the value as a zero vector if it does not exist.
pair<VectorValuesUnordered::iterator, bool> xi = x.tryInsert(keys_[pos], Vector());
if(xi.second)
xi.first->second = Vector::Zero(getDim(begin() + pos));
gtsam::transposeMultiplyAdd(1.0, Ab_(pos), E, xi.first->second);
}
}
/* ************************************************************************* */

3
gtsam/linear/JacobianFactorUnordered.h
View File

@ -245,7 +245,8 @@ namespace gtsam {
/** Return A*x */
Vector operator*(const VectorValuesUnordered& x) const;
/** x += A'*e */
/** x += A'*e. If x is initially missing any values, they are created and assumed to start as
* zero vectors. */
void transposeMultiplyAdd(double alpha, const Vector& e, VectorValuesUnordered& x) const;
/** Return a whitened version of the factor, i.e. with unit diagonal noise model. */