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

Using keys not indices

release/4.3a0
Frank Dellaert 2019-04-04 23:01:18 -04:00
parent a7826ab417
commit 334c85a298
1 changed files with 11 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -508,8 +508,10 @@ Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
Errors::iterator ei = e.begin();
for (size_t i = 0; i < y.size(); ++i, ++ei)
*ei = y[i];
for(const auto& key_value: y) {
*ei = key_value.second;
++ei;
}
// Add A2 contribution
VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y
@ -522,8 +524,10 @@ VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {
Errors::const_iterator it = e.begin();
VectorValues y = zero();
for (size_t i = 0; i < y.size(); ++i, ++it)
y[i] = *it;
for(auto& key_value: y) {
key_value.second = *it;
++it;
}
transposeMultiplyAdd2(1.0, it, e.end(), y);
return y;
}
@ -534,9 +538,10 @@ void SubgraphPreconditioner::transposeMultiplyAdd
(double alpha, const Errors& e, VectorValues& y) const {
Errors::const_iterator it = e.begin();
for (size_t i = 0; i < y.size(); ++i, ++it) {
for(auto& key_value: y) {
const Vector& ei = *it;
axpy(alpha, ei, y[i]);
axpy(alpha, ei, key_value.second);
++it;
}
transposeMultiplyAdd2(alpha, it, e.end(), y);
}