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

Created BinaryJacobianFactor template

release/4.3a0
dellaert 2015-06-14 15:50:15 -07:00
parent 06902209b0
commit 7698c52ce9
3 changed files with 104 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
gtsam/linear/BinaryJacobianFactor.h Normal file
View File

@ -0,0 +1,91 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file BinaryJacobianFactor.h
*
* @brief A binary JacobianFactor specialization that uses fixed matrix math for speed
*
* @date June 2015
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/timing.h>
namespace gtsam {
/**
* A binary JacobianFactor specialization that uses fixed matrix math for speed
*/
template<int M, int N1, int N2>
struct BinaryJacobianFactor: JacobianFactor {
/// Constructor
BinaryJacobianFactor(Key key1, const Eigen::Matrix<double, M, N1>& A1,
Key key2, const Eigen::Matrix<double, M, N2>& A2,
const Eigen::Matrix<double, M, 1>& b, //
const SharedDiagonal& model = SharedDiagonal()) :
JacobianFactor(key1, A1, key2, A2, b, model) {
}
inline Key key1() const {
return keys_[0];
}
inline Key key2() const {
return keys_[1];
}
// Fixed-size matrix update
void updateHessian(const FastVector<Key>& infoKeys,
SymmetricBlockMatrix* info) const {
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(
"BinaryJacobianFactor::updateHessian: cannot update information with "
"constrained noise model");
BinaryJacobianFactor whitenedFactor(key1(), model->Whiten(getA(begin())),
key2(), model->Whiten(getA(end())), model->whiten(getb()));
whitenedFactor.updateHessian(infoKeys, info);
} else {
// First build an array of slots
DenseIndex slot1 = Slot(infoKeys, key1());
DenseIndex slot2 = Slot(infoKeys, key2());
DenseIndex slotB = info->nBlocks() - 1;
const Matrix& Ab = Ab_.matrix();
Eigen::Block<const Matrix, M, N1> A1(Ab, 0, 0);
Eigen::Block<const Matrix, M, N2> A2(Ab, 0, N1);
Eigen::Block<const Matrix, M, 1> b(Ab, 0, N1 + N2);
// We perform I += A'*A to the upper triangle
(*info)(slot1, slot1).selfadjointView().rankUpdate(A1.transpose());
(*info)(slot1, slot2).knownOffDiagonal() += A1.transpose() * A2;
(*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b;
(*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose());
(*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b;
(*info)(slotB, slotB)(0, 0) += b.transpose() * b;
}
}
};
template<int M, int N1, int N2>
struct traits<BinaryJacobianFactor<M, N1, N2> > : Testable<
BinaryJacobianFactor<M, N1, N2> > {
};
} //namespace gtsam

61
gtsam/slam/GeneralSFMFactor.h
View File

@ -25,7 +25,7 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/BinaryJacobianFactor.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/concepts.h>
#include <gtsam/base/Manifold.h>
@ -133,56 +133,10 @@ public:
}
}
class BinaryJacobianFactor : public JacobianFactor {
// Fixed size matrices
// TODO(frank): implement generic BinaryJacobianFactor<N,M1,M2> next to
// JacobianFactor
public:
/// Constructor
BinaryJacobianFactor(Key key1, const JacobianC& A1, Key key2, const JacobianL& A2,
const Vector2& b,
const SharedDiagonal& model = SharedDiagonal())
: JacobianFactor(key1, A1, key2, A2, b, model) {}
// Fixed-size matrix update
void updateHessian(const FastVector<Key>& infoKeys, SymmetricBlockMatrix* info) const {
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(
"BinaryJacobianFactor::updateHessian: cannot update information with "
"constrained noise model");
JacobianFactor whitenedFactor = whiten(); // TODO: make BinaryJacobianFactor
whitenedFactor.updateHessian(infoKeys, info);
} else {
// First build an array of slots
DenseIndex slot1 = Slot(infoKeys, keys_.front());
DenseIndex slot2 = Slot(infoKeys, keys_.back());
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);
// We perform I += A'*A to the upper triangle
(*info)(slot1, slot1).selfadjointView().rankUpdate(A1.transpose());
(*info)(slot1, slot2).knownOffDiagonal() += A1.transpose() * A2;
(*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b;
(*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose());
(*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b;
(*info)(slotB, slotB)(0,0) += b.transpose() * b;
}
}
};
/// 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<BinaryJacobianFactor>();
if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
const Key key1 = this->key1(), key2 = this->key2();
JacobianC H1;
@ -210,14 +164,13 @@ public:
b = noiseModel->Whiten(b);
}
// Create new (unit) noiseModel, preserving constraints if applicable
SharedDiagonal model;
if (noiseModel && noiseModel->isConstrained()) {
using noiseModel::Constrained;
return boost::make_shared<BinaryJacobianFactor>(
key1, H1, key2, H2, b,
boost::static_pointer_cast<Constrained>(noiseModel)->unit());
} else {
return boost::make_shared<BinaryJacobianFactor>(key1, H1, key2, H2, b);
model = boost::static_pointer_cast<noiseModel::Constrained>(noiseModel)->unit();
}
return boost::make_shared<BinaryJacobianFactor<2, DimC, DimL> >(key1, H1, key2, H2, b, model);
}
/** return the measured */

12
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -135,11 +135,11 @@ TEST( GeneralSFMFactor, equals ) {
const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection> factor1(
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
boost::shared_ptr<Projection> factor2(
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
EXPECT(assert_equal(*factor1, *factor2));
}
@ -157,8 +157,8 @@ TEST( GeneralSFMFactor, error ) {
Point3 l1;
values.insert(L(1), l1);
EXPECT(
assert_equal(((Vector ) Vector2(-3., 0.)),
factor.unwhitenedError(values)));
assert_equal(((Vector ) Vector2(-3., 0.)),
factor.unwhitenedError(values)));
}
/* ************************************************************************* */