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

Case change

release/4.3a0
dellaert 2015-03-04 17:21:22 -08:00
parent a95e5c7e05
commit f2a5e5c683
3 changed files with 11 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/slam/JacobianFactorSVD.h
View File

@ -63,7 +63,7 @@ public:
QF.push_back(
KeyMatrix(it.first,
(Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second));
JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
JacobianFactor::fillTerms(QF, - Enull.transpose() * b, model);
}
};

8
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -128,7 +128,7 @@ public:
* Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix
* Fast version - works on with sparsity
*/
static void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
static void SparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) {
// Schur complement trick
@ -167,7 +167,7 @@ public:
* Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
* and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
*/
static void updateSparseSchurComplement(
static void UpdateSparseSchurComplement(
const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix3& P /*Point Covariance*/, const Vector& b, const double f,
const FastVector<Key>& allKeys, const FastVector<Key>& keys,
@ -248,7 +248,7 @@ public:
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// Do the Schur complement
sparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian);
SparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian);
return augmentedHessian.matrix();
}
@ -257,7 +257,7 @@ public:
Matrix augmented = augmentedInformation();
int m = this->keys_.size();
size_t M = D * m;
return augmented.block(0,0,M,M);
return augmented.block(0, 0, M, M);
}
/// Return the diagonal of the Hessian for this factor

10
gtsam/slam/SmartFactorBase.h
View File

@ -272,7 +272,7 @@ public:
EtE.diagonal() += lambda * EtE.diagonal();
} else {
DenseIndex n = E.cols();
EtE += lambda * Eigen::MatrixXd::Identity(n,n);
EtE += lambda * Eigen::MatrixXd::Identity(n, n);
}
return (EtE).inverse();
@ -342,7 +342,8 @@ public:
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// build augmented hessian
sparseSchurComplement(Fblocks, E, P, b, augmentedHessian);
RegularImplicitSchurFactor<Dim>::SparseSchurComplement(Fblocks, E, P, b,
augmentedHessian);
augmentedHessian(m, m)(0, 0) = f;
return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
@ -363,7 +364,7 @@ public:
std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobians(Fblocks, E, b, cameras, point);
Matrix3 P = PointCov(E, lambda, diagonalDamping);
RegularImplicitSchurFactor<Dim, ZDim>::updateSparseSchurComplement(Fblocks,
RegularImplicitSchurFactor<Dim, ZDim>::UpdateSparseSchurComplement(Fblocks,
E, P, b, f, allKeys, keys_, augmentedHessian);
}
@ -420,7 +421,8 @@ public:
const size_t M = ZDim * m;
Matrix E0(M, M - 3);
computeJacobiansSVD(F, E0, b, cameras, point);
SharedIsotropic n = noiseModel::Isotropic::Sigma(M-3, noiseModel_->sigma());
SharedIsotropic n = noiseModel::Isotropic::Sigma(M - 3,
noiseModel_->sigma());
return boost::make_shared<JacobianFactorSVD<Dim, ZDim> >(F, E0, b, n);
}