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

minor speedup

release/4.3a0
Chris Beall 2013-08-28 14:47:57 +00:00
parent bf8621aa3a
commit 19ddf3f228
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -232,32 +232,32 @@ namespace gtsam {
// Allocate inv(Hl'Hl)
Matrix3 C = zeros(3,3);
for(size_t i1 = 0; i1 < keys_.size(); i1++) {
C += Hl.at(i1).transpose() * Hl.at(i1);
C.noalias() += Hl.at(i1).transpose() * Hl.at(i1);
}
C = C.inverse().eval(); // this is very important: without eval, because of eigen aliasing the results will be incorrect
Matrix3 Cinv = C.inverse(); // this is very important: without eval, because of eigen aliasing the results will be incorrect
// Calculate sub blocks
for(size_t i1 = 0; i1 < keys_.size(); i1++) {
for(size_t i2 = 0; i2 < keys_.size(); i2++) {
// we only need the upper triangular entries
Hxl[i1][i2] = Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose();
Hxl[i1][i2].noalias() = Hx.at(i1).transpose() * Hl.at(i1) * Cinv * Hl.at(i2).transpose();
}
}
// Populate Gs and gs
int GsCount = 0;
for(size_t i1 = 0; i1 < numKeys; i1++) {
gs.at(i1) = Hx.at(i1).transpose() * b.at(i1);
gs.at(i1).noalias() = Hx.at(i1).transpose() * b.at(i1);
for(size_t i2 = 0; i2 < numKeys; i2++) {
gs.at(i1) -= Hxl[i1][i2] * b.at(i2);
gs.at(i1).noalias() -= Hxl[i1][i2] * b.at(i2);
if (i2 == i1){
Gs.at(GsCount) = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2);
Gs.at(GsCount).noalias() = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2);
GsCount++;
}
if (i2 > i1) {
Gs.at(GsCount) = - Hxl[i1][i2] * Hx.at(i2);
Gs.at(GsCount).noalias() = - Hxl[i1][i2] * Hx.at(i2);
GsCount++;
}
}