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

formatting only

release/4.3a0
Frank Dellaert 2009-10-24 04:07:32 +00:00
parent baef89ccf0
commit 563abc2b3f
1 changed files with 24 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
cpp/LinearFactor.cpp
View File

@ -137,11 +137,11 @@ pair<Matrix,Vector> LinearFactor::matrix(const Ordering& ordering) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void LinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m, const size_t pos) void LinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m,
{ const size_t pos) {
// iterate over all matrices from the factor f // iterate over all matrices from the factor f
LinearFactor::const_iterator it = f->begin(); LinearFactor::const_iterator it = f->begin();
for(; it != f->end(); it++) { for (; it != f->end(); it++) {
string j = it->first; string j = it->first;
Matrix A = it->second; Matrix A = it->second;
@ -150,19 +150,19 @@ void LinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m, con
// find the corresponding matrix among As // find the corresponding matrix among As
const_iterator mine = As.find(j); const_iterator mine = As.find(j);
const bool exists = mine!=As.end(); const bool exists = mine != As.end();
// create the matrix or use existing // create the matrix or use existing
Matrix Anew = exists ? mine->second : zeros(m,n); Matrix Anew = exists ? mine->second : zeros(m, n);
// copy the values in the existing matrix // copy the values in the existing matrix
for (size_t i=0;i<mrhs;i++) for (size_t i = 0; i < mrhs; i++)
for(size_t j=0;j<n;j++) for (size_t j = 0; j < n; j++)
Anew(pos+i,j)=A(i,j); Anew(pos + i, j) = A(i, j);
// insert the matrix into the factor // insert the matrix into the factor
if(exists) As.erase(j); if (exists) As.erase(j);
insert(j,Anew); insert(j, Anew);
} }
} }
@ -221,9 +221,9 @@ LinearFactor::eliminate(const string& key)
// update RHS, b -= (beta * inner_prod(v,b)) * v; // update RHS, b -= (beta * inner_prod(v,b)) * v;
double inner = 0; double inner = 0;
for(size_t r = j ; r < m; r++) for(size_t r = j ; r < m; r++)
inner += vjm(r-j) * (b)(r); inner += vjm(r-j) * b(r);
for(size_t r = j ; r < m; r++) for(size_t r = j ; r < m; r++)
(b)(r) -= beta*inner*vjm(r-j); b(r) -= beta*inner*vjm(r-j);
} // column j } // column j
// create ConditionalGaussian with first n rows // create ConditionalGaussian with first n rows