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);
} }
}; };

8
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();
} }
@ -257,7 +257,7 @@ public:
Matrix augmented = augmentedInformation(); Matrix augmented = augmentedInformation();
int m = this->keys_.size(); int m = this->keys_.size();
size_t M = D * m; 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 /// 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(); EtE.diagonal() += lambda * EtE.diagonal();
} else { } else {
DenseIndex n = E.cols(); DenseIndex n = E.cols();
EtE += lambda * Eigen::MatrixXd::Identity(n,n); EtE += lambda * Eigen::MatrixXd::Identity(n, n);
} }
return (EtE).inverse(); return (EtE).inverse();
@ -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);
} }