diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index ba3d27202..e44576e5f 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -25,13 +25,16 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include +#include #include #include @@ -61,6 +64,8 @@ namespace gtsam { static const int DimC = FixedDimension::value; static const int DimL = FixedDimension::value; + typedef Eigen::Matrix JacobianC; + typedef Eigen::Matrix JacobianL; protected: @@ -121,21 +126,69 @@ namespace gtsam { return reprojError.vector(); } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, DimC); - if (H2) *H2 = zeros(2, DimL); + if (H1) *H1 = JacobianC::Zero(); + if (H2) *H2 = JacobianL::Zero(); // TODO warn if verbose output asked for return zero(2); } } + class LinearizedFactor : public JacobianFactor { + // Fixed size matrices + // TODO: implement generic BinaryJacobianFactor next to + // JacobianFactor + JacobianC AC_; + JacobianL AL_; + Vector2 b_; + + public: + /// Constructor + LinearizedFactor(Key i1, const JacobianC& A1, Key i2, const JacobianL& A2, + const Vector2& b, + const SharedDiagonal& model = SharedDiagonal()) + : JacobianFactor(i1, A1, i2, A2, b, model), AC_(A1), AL_(A2), b_(b) {} + + // Fixed-size matrix update + void updateATA(const Scatter& scatter, SymmetricBlockMatrix* info) const { + gttic(updateATA_LinearizedFactor); + + // Whiten the factor if it has a noise model + const SharedDiagonal& model = get_model(); + if (model && !model->isUnit()) { + if (model->isConstrained()) + throw std::invalid_argument( + "JacobianFactor::updateATA: cannot update information with " + "constrained noise model"); + JacobianFactor whitenedFactor = whiten(); + whitenedFactor.updateATA(scatter, info); + } else { + // N is number of variables in information matrix + DenseIndex N = info->nBlocks() - 1; + + // First build an array of slots + DenseIndex slotC = scatter.at(this->keys().front()).slot; + DenseIndex slotL = scatter.at(this->keys().back()).slot; + DenseIndex slotB = N; + + // We perform I += A'*A to the upper triangle + (*info)(slotC, slotC).knownOffDiagonal() += AC_.transpose() * AC_; + (*info)(slotC, slotL).knownOffDiagonal() += AC_.transpose() * AL_; + (*info)(slotC, slotB).knownOffDiagonal() += AC_.transpose() * b_; + (*info)(slotL, slotL).knownOffDiagonal() += AL_.transpose() * AL_; + (*info)(slotL, slotB).knownOffDiagonal() += AL_.transpose() * b_; + (*info)(slotB, slotB).knownOffDiagonal() += b_.transpose() * b_; + } + } + }; + /// Linearize using fixed-size matrices - boost::shared_ptr linearize(const Values& values) { + boost::shared_ptr linearize(const Values& values) const { // Only linearize if the factor is active - if (!this->active(values)) return boost::shared_ptr(); + if (!this->active(values)) return boost::shared_ptr(); const Key key1 = this->key1(), key2 = this->key2(); - Eigen::Matrix H1; - Eigen::Matrix H2; + JacobianC H1; + JacobianL H2; Vector2 b; try { const CAMERA& camera = values.at(key1); @@ -159,11 +212,11 @@ namespace gtsam { if (noiseModel && noiseModel->isConstrained()) { using noiseModel::Constrained; - return boost::make_shared( + return boost::make_shared( key1, H1, key2, H2, b, boost::static_pointer_cast(noiseModel)->unit()); } else { - return boost::make_shared(key1, H1, key2, H2, b); + return boost::make_shared(key1, H1, key2, H2, b); } }