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( QF.push_back(
KeyMatrix(it.first, KeyMatrix(it.first,
(Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second)); (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second));
JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); JacobianFactor::fillTerms(QF, - Enull.transpose() * b, model);
} }
}; };

6
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -128,7 +128,7 @@ public:
* Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix * Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix
* Fast version - works on with sparsity * 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, const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) { /*output ->*/SymmetricBlockMatrix& augmentedHessian) {
// Schur complement trick // Schur complement trick
@ -167,7 +167,7 @@ public:
* Applies Schur complement (exploiting block structure) to get a smart factor on cameras, * 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. * 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 std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix3& P /*Point Covariance*/, const Vector& b, const double f, const Matrix3& P /*Point Covariance*/, const Vector& b, const double f,
const FastVector<Key>& allKeys, const FastVector<Key>& keys, const FastVector<Key>& allKeys, const FastVector<Key>& keys,
@ -248,7 +248,7 @@ public:
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// Do the Schur complement // Do the Schur complement
sparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian); SparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian);
return augmentedHessian.matrix(); return augmentedHessian.matrix();
} }

8
gtsam/slam/SmartFactorBase.h
View File

@ -342,7 +342,8 @@ public:
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// build augmented hessian // build augmented hessian
sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); RegularImplicitSchurFactor<Dim>::SparseSchurComplement(Fblocks, E, P, b,
augmentedHessian);
augmentedHessian(m, m)(0, 0) = f; augmentedHessian(m, m)(0, 0) = f;
return boost::make_shared<RegularHessianFactor<Dim> >(keys_, return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
@ -363,7 +364,7 @@ public:
std::vector<KeyMatrix2D> Fblocks; std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobians(Fblocks, E, b, cameras, point); double f = computeJacobians(Fblocks, E, b, cameras, point);
Matrix3 P = PointCov(E, lambda, diagonalDamping); Matrix3 P = PointCov(E, lambda, diagonalDamping);
RegularImplicitSchurFactor<Dim, ZDim>::updateSparseSchurComplement(Fblocks, RegularImplicitSchurFactor<Dim, ZDim>::UpdateSparseSchurComplement(Fblocks,
E, P, b, f, allKeys, keys_, augmentedHessian); E, P, b, f, allKeys, keys_, augmentedHessian);
} }
@ -420,7 +421,8 @@ public:
const size_t M = ZDim * m; const size_t M = ZDim * m;
Matrix E0(M, M - 3); Matrix E0(M, M - 3);
computeJacobiansSVD(F, E0, b, cameras, point); 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); return boost::make_shared<JacobianFactorSVD<Dim, ZDim> >(F, E0, b, n);
} }