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

Moved PointCov to CameraSet

release/4.3a0
dellaert 2015-03-10 22:08:55 -07:00
parent dd7d9cd6fc
commit 421ad49048
1 changed files with 5 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/slam/SmartFactorBase.h
View File

@ -243,22 +243,6 @@ public:
return (E.transpose() * E).inverse(); return (E.transpose() * E).inverse();
} }
/// Computes Point Covariance P, with lambda parameter
static Matrix PointCov(Matrix& E, double lambda,
bool diagonalDamping = false) {
Matrix EtE = E.transpose() * E;
if (diagonalDamping) { // diagonal of the hessian
EtE.diagonal() += lambda * EtE.diagonal();
} else {
DenseIndex n = E.cols();
EtE += lambda * Eigen::MatrixXd::Identity(n, n);
}
return (EtE).inverse();
}
/** /**
* Compute F, E, and b (called below in both vanilla and SVD versions), where * Compute F, E, and b (called below in both vanilla and SVD versions), where
* F is a vector of derivatives wrpt the cameras, and E the stacked derivatives * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives
@ -301,11 +285,10 @@ public:
Matrix E; Matrix E;
Vector b; Vector b;
computeJacobians(Fblocks, E, b, cameras, point); computeJacobians(Fblocks, E, b, cameras, point);
Matrix P = PointCov(E, lambda, diagonalDamping);
// build augmented hessian // build augmented hessian
SymmetricBlockMatrix augmentedHessian = CameraSet<CAMERA>::SchurComplement( SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E,
Fblocks, E, P, b); b);
return boost::make_shared<RegularHessianFactor<Dim> >(keys_, return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
augmentedHessian); augmentedHessian);
@ -324,8 +307,7 @@ public:
Vector b; Vector b;
std::vector<MatrixZD> Fblocks; std::vector<MatrixZD> Fblocks;
computeJacobians(Fblocks, E, b, cameras, point); computeJacobians(Fblocks, E, b, cameras, point);
Matrix P = PointCov(E, lambda, diagonalDamping); Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_,
CameraSet<CAMERA>::UpdateSchurComplement(Fblocks, E, P, b, allKeys, keys_,
augmentedHessian); augmentedHessian);
} }
@ -346,7 +328,7 @@ public:
std::vector<MatrixZD> F; std::vector<MatrixZD> F;
computeJacobians(F, E, b, cameras, point); computeJacobians(F, E, b, cameras, point);
whitenJacobians(F, E, b); whitenJacobians(F, E, b);
Matrix P = PointCov(E, lambda, diagonalDamping); Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_, F, E, return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_, F, E,
P, b); P, b);
} }
@ -362,7 +344,7 @@ public:
std::vector<MatrixZD> F; std::vector<MatrixZD> F;
computeJacobians(F, E, b, cameras, point); computeJacobians(F, E, b, cameras, point);
const size_t M = b.size(); const size_t M = b.size();
Matrix P = PointCov(E, lambda, diagonalDamping); Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma()); SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma());
return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(keys_, F, E, P, b, n); return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(keys_, F, E, P, b, n);
} }