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

Renamed to BinaryJacobianFactor

release/4.3a0
dellaert 2015-06-14 15:02:48 -07:00
parent 9fcd498d6a
commit a94c2e7323
1 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/slam/GeneralSFMFactor.h
View File

@ -133,29 +133,29 @@ public:
} }
} }
class LinearizedFactor : public JacobianFactor { class BinaryJacobianFactor : public JacobianFactor {
// Fixed size matrices // Fixed size matrices
// TODO(frank): implement generic BinaryJacobianFactor<N,M1,M2> next to // TODO(frank): implement generic BinaryJacobianFactor<N,M1,M2> next to
// JacobianFactor // JacobianFactor
public: public:
/// Constructor /// Constructor
LinearizedFactor(Key i1, const JacobianC& A1, Key i2, const JacobianL& A2, BinaryJacobianFactor(Key key1, const JacobianC& A1, Key key2, const JacobianL& A2,
const Vector2& b, const Vector2& b,
const SharedDiagonal& model = SharedDiagonal()) const SharedDiagonal& model = SharedDiagonal())
: JacobianFactor(i1, A1, i2, A2, b, model) {} : JacobianFactor(key1, A1, key2, A2, b, model) {}
// Fixed-size matrix update // Fixed-size matrix update
void updateHessian(const FastVector<Key>& infoKeys, SymmetricBlockMatrix* info) const { void updateHessian(const FastVector<Key>& infoKeys, SymmetricBlockMatrix* info) const {
gttic(updateHessian_LinearizedFactor); gttic(updateHessian_BinaryJacobianFactor);
// Whiten the factor if it has a noise model // Whiten the factor if it has a noise model
const SharedDiagonal& model = get_model(); const SharedDiagonal& model = get_model();
if (model && !model->isUnit()) { if (model && !model->isUnit()) {
if (model->isConstrained()) if (model->isConstrained())
throw std::invalid_argument( throw std::invalid_argument(
"JacobianFactor::updateHessian: cannot update information with " "BinaryJacobianFactor::updateHessian: cannot update information with "
"constrained noise model"); "constrained noise model");
JacobianFactor whitenedFactor = whiten(); JacobianFactor whitenedFactor = whiten(); // TODO: make BinaryJacobianFactor
whitenedFactor.updateHessian(infoKeys, info); whitenedFactor.updateHessian(infoKeys, info);
} else { } else {
// First build an array of slots // First build an array of slots
@ -174,7 +174,7 @@ public:
(*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b; (*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b;
(*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose()); (*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose());
(*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b; (*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b;
(*info)(slotB, slotB).selfadjointView().rankUpdate(b.transpose()); (*info)(slotB, slotB)(0,0) = b.transpose() * b;
} }
} }
}; };
@ -182,7 +182,7 @@ public:
/// Linearize using fixed-size matrices /// Linearize using fixed-size matrices
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const { boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
// Only linearize if the factor is active // Only linearize if the factor is active
if (!this->active(values)) return boost::shared_ptr<LinearizedFactor>(); if (!this->active(values)) return boost::shared_ptr<BinaryJacobianFactor>();
const Key key1 = this->key1(), key2 = this->key2(); const Key key1 = this->key1(), key2 = this->key2();
JacobianC H1; JacobianC H1;
@ -212,11 +212,11 @@ public:
if (noiseModel && noiseModel->isConstrained()) { if (noiseModel && noiseModel->isConstrained()) {
using noiseModel::Constrained; using noiseModel::Constrained;
return boost::make_shared<LinearizedFactor>( return boost::make_shared<BinaryJacobianFactor>(
key1, H1, key2, H2, b, key1, H1, key2, H2, b,
boost::static_pointer_cast<Constrained>(noiseModel)->unit()); boost::static_pointer_cast<Constrained>(noiseModel)->unit());
} else { } else {
return boost::make_shared<LinearizedFactor>(key1, H1, key2, H2, b); return boost::make_shared<BinaryJacobianFactor>(key1, H1, key2, H2, b);
} }
} }