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