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

Correct gradientAtZero: remove zero initialization

release/4.3a0
Sungtae An 2014-12-22 14:50:26 -05:00
parent d64af0d626
commit 1affae697c
1 changed files with 5 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/slam/RegularJacobianFactor.h
View File

@ -141,14 +141,11 @@ public:
}
}
/** Raw memory access version of gradientAtZero */
/** Raw memory access version of gradientAtZero
* TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n
*/
virtual void gradientAtZero(double* d) const {
// Initialize d as a zero vector
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
DMap(d + D * j) = DVector::Zero();
}
// Get vector b not weighted
Vector b = getb();
@ -159,13 +156,12 @@ public:
}
// Just iterate over all A matrices
for (size_t pos = 0; pos < size(); ++pos) {
Key j = keys_[pos];
for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
DVector dj;
// gradient -= A'*b/sigma^2
// Computing with each column
for (size_t k = 0; k < D; ++k){
Vector column_k = Ab_(pos).col(k);
Vector column_k = Ab_(j).col(k);
dj(k) = -1.0*dot(b, column_k);
}
DMap(d + D * j) += dj;