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

26
gtsam/slam/GeneralSFMFactor.h
View File

@ -133,29 +133,29 @@ public:
}
}
class LinearizedFactor : public JacobianFactor {
class BinaryJacobianFactor : public JacobianFactor {
// Fixed size matrices
// TODO(frank): implement generic BinaryJacobianFactor<N,M1,M2> next to
// JacobianFactor
public:
/// 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 SharedDiagonal& model = SharedDiagonal())
: JacobianFactor(i1, A1, i2, A2, b, model) {}
: JacobianFactor(key1, A1, key2, A2, b, model) {}
// Fixed-size matrix update
void updateHessian(const FastVector<Key>& infoKeys, SymmetricBlockMatrix* info) const {
gttic(updateHessian_LinearizedFactor);
gttic(updateHessian_BinaryJacobianFactor);
// 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::updateHessian: cannot update information with "
"BinaryJacobianFactor::updateHessian: cannot update information with "
"constrained noise model");
JacobianFactor whitenedFactor = whiten();
JacobianFactor whitenedFactor = whiten(); // TODO: make BinaryJacobianFactor
whitenedFactor.updateHessian(infoKeys, info);
} else {
// First build an array of slots
@ -164,9 +164,9 @@ public:
DenseIndex slotB = info->nBlocks() - 1;
const Matrix& Ab = Ab_.matrix();
Eigen::Block<const Matrix,2,DimC> A1(Ab,0,0);
Eigen::Block<const Matrix,2,DimL> A2(Ab,0,DimC);
Eigen::Block<const Matrix,2,1> b(Ab,0,DimC+DimL);
Eigen::Block<const Matrix,2,DimC> A1(Ab, 0, 0);
Eigen::Block<const Matrix,2,DimL> A2(Ab, 0, DimC);
Eigen::Block<const Matrix,2,1> b(Ab, 0, DimC + DimL);
// We perform I += A'*A to the upper triangle
(*info)(slot1, slot1).selfadjointView().rankUpdate(A1.transpose());
@ -174,7 +174,7 @@ public:
(*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b;
(*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose());
(*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
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
// 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();
JacobianC H1;
@ -212,11 +212,11 @@ public:
if (noiseModel && noiseModel->isConstrained()) {
using noiseModel::Constrained;
return boost::make_shared<LinearizedFactor>(
return boost::make_shared<BinaryJacobianFactor>(
key1, H1, key2, H2, b,
boost::static_pointer_cast<Constrained>(noiseModel)->unit());
} else {
return boost::make_shared<LinearizedFactor>(key1, H1, key2, H2, b);
return boost::make_shared<BinaryJacobianFactor>(key1, H1, key2, H2, b);
}
}