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

Deal with possibility of degenerate E version (M*2 rather than M*3)

release/4.3a0
dellaert 2015-03-04 10:55:39 -08:00
parent cc4083d33e
commit 2f2cc078dc
2 changed files with 31 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/slam/SmartFactorBase.h
View File

@ -258,24 +258,21 @@ public:
}
/// Computes Point Covariance P from E
static Matrix3 PointCov(Matrix& E) {
static Matrix PointCov(Matrix& E) {
return (E.transpose() * E).inverse();
}
/// Computes Point Covariance P, with lambda parameter
static Matrix3 PointCov(Matrix& E, double lambda,
static Matrix PointCov(Matrix& E, double lambda,
bool diagonalDamping = false) {
Matrix3 EtE = E.transpose() * E;
Matrix EtE = E.transpose() * E;
if (diagonalDamping) { // diagonal of the hessian
EtE(0, 0) += lambda * EtE(0, 0);
EtE(1, 1) += lambda * EtE(1, 1);
EtE(2, 2) += lambda * EtE(2, 2);
EtE.diagonal() += lambda * EtE.diagonal();
} else {
EtE(0, 0) += lambda;
EtE(1, 1) += lambda;
EtE(2, 2) += lambda;
DenseIndex n = E.cols();
EtE += lambda * Eigen::MatrixXd::Identity(n,n);
}
return (EtE).inverse();

49
gtsam/slam/SmartProjectionFactor.h
View File

@ -280,7 +280,7 @@ public:
{
std::vector<typename Base::KeyMatrix2D> Fblocks;
f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
Base::whitenJacobians(Fblocks,E,b);
Base::whitenJacobians(Fblocks, E, b);
Base::FillDiagonalF(Fblocks, F); // expensive !
}
@ -290,7 +290,8 @@ public:
Matrix H(Base::Dim * numKeys, Base::Dim * numKeys);
Vector gs_vector;
Matrix3 P = Base::PointCov(E, lambda);
// Note P can 2*2 or 3*3
Matrix P = Base::PointCov(E, lambda);
// Create reduced Hessian matrix via Schur complement F'*F - F'*E*P*E'*F
H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
@ -448,29 +449,29 @@ public:
else
result_ = triangulateSafe(cameras);
// if we don't want to manage the exceptions we discard the factor
if (!manageDegeneracy_ && !result_)
return 0.0;
if (isPointBehindCamera()) { // if we want to manage the exceptions with rotation-only factors
std::cout
<< "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!"
<< std::endl;
}
if (isDegenerate()) {
// return 0.0; // TODO: this maybe should be zero?
std::cout
<< "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!"
<< std::endl;
// 3D parameterization of point at infinity
const Point2& z0 = this->measured_.at(0);
result_ = TriangulationResult(
cameras.front().backprojectPointAtInfinity(z0));
return Base::totalReprojectionErrorAtInfinity(cameras, *result_);
} else {
// Just use version in base class
if (result_)
// All good, just use version in base class
return Base::totalReprojectionError(cameras, *result_);
else {
// if we don't want to manage the exceptions we discard the factor
if (!manageDegeneracy_)
return 0.0;
if (isPointBehindCamera()) { // if we want to manage the exceptions with rotation-only factors
throw std::runtime_error(
"SmartProjectionFactor::totalReprojectionError does not handle point behind camera yet");
}
if (isDegenerate()) {
// 3D parameterization of point at infinity
const Point2& z0 = this->measured_.at(0);
result_ = TriangulationResult(
cameras.front().backprojectPointAtInfinity(z0));
return Base::totalReprojectionErrorAtInfinity(cameras, *result_);
}
// should not reach here. TODO use switch
throw std::runtime_error(
"SmartProjectionFactor::totalReprojectionError internal error");
}
}