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

slight change in implementation of hessianBlockDiagonal

release/4.3a0
Luca 2014-04-23 19:00:15 -04:00
parent d9e93f316a
commit 8c657f8857
1 changed files with 14 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam_unstable/slam/ImplicitSchurFactor.h
View File

@ -189,19 +189,24 @@ public:
/// Return the block diagonal of the Hessian for this factor /// Return the block diagonal of the Hessian for this factor
virtual std::map<Key, Matrix> hessianBlockDiagonal() const { virtual std::map<Key, Matrix> hessianBlockDiagonal() const {
std::map<Key, Matrix> blocks; std::map<Key, Matrix> blocks;
// F'*(I - E*P*E')*F
for (size_t pos = 0; pos < size(); ++pos) { for (size_t pos = 0; pos < size(); ++pos) {
Key j = keys_[pos]; Key j = keys_[pos];
const Matrix2D& Fj = Fblocks_[pos].second;
// F'*F - F'*E*P*E'*F (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) // F'*F - F'*E*P*E'*F (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
Eigen::Matrix<double, D, 3> FtE = Fj.transpose() const Matrix2D& Fj = Fblocks_[pos].second;
* E_.block<2, 3>(2 * pos, 0); // Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
blocks[j] = Fj.transpose() * Fj // * E_.block<2, 3>(2 * pos, 0);
- FtE * PointCovariance_ * FtE.transpose(); // blocks[j] = Fj.transpose() * Fj
// - FtE * PointCovariance_ * FtE.transpose();
const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0);
blocks[j] = Fj.transpose() * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj);
// F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
// static const Eigen::Matrix<double, 2, 2> I2 = eye(2); // static const Eigen::Matrix<double, 2, 2> I2 = eye(2);
// Eigen::Matrix<double, 2, 2> Q = // // Eigen::Matrix<double, 2, 2> Q = //
// I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); // I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose();
// blocks[j] = Fj.transpose() * Q * Fj; // blocks[j] = Fj.transpose() * Q * Fj;
} }
return blocks; return blocks;
} }