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

Removed ZDim parameter from JacobianSchurFactor

release/4.3a0
dellaert 2015-02-22 07:21:42 +01:00
parent 78fd7de1b9
commit ecc1cfd15d
4 changed files with 45 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/slam/JacobianFactorQ.h
View File

@ -24,9 +24,11 @@ namespace gtsam {
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D, size_t ZDim>
class JacobianFactorQ: public JacobianSchurFactor<D, ZDim> {
class JacobianFactorQ: public JacobianSchurFactor<D> {
typedef JacobianSchurFactor<D, ZDim> Base;
typedef JacobianSchurFactor<D> Base;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef std::pair<Key, MatrixZD> KeyMatrixZD;
public:
@ -37,7 +39,7 @@ public:
/// Empty constructor with keys
JacobianFactorQ(const FastVector<Key>& keys, //
const SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D, ZDim>() {
Base() {
Matrix zeroMatrix = Matrix::Zero(0, D);
Vector zeroVector = Vector::Zero(0);
typedef std::pair<Key, Matrix> KeyMatrix;
@ -49,10 +51,10 @@ public:
}
/// Constructor
JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D, ZDim>() {
JacobianFactorQ(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E,
const Matrix3& P, const Vector& b, const SharedDiagonal& model =
SharedDiagonal()) :
Base() {
size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
// Calculate projector Q
Matrix Q = eye(m2) - E * P * E.transpose();
@ -62,7 +64,7 @@ public:
std::vector<KeyMatrix> QF;
QF.reserve(m);
// Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D)
BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks)
BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
QF.push_back(
KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second));
// Which is then passed to the normal JacobianFactor constructor

20
gtsam/slam/JacobianFactorQR.h
View File

@ -18,24 +18,26 @@ class GaussianBayesNet;
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D, size_t ZDim>
class JacobianFactorQR: public JacobianSchurFactor<D, ZDim> {
class JacobianFactorQR: public JacobianSchurFactor<D> {
typedef JacobianSchurFactor<D, ZDim> Base;
typedef JacobianSchurFactor<D> Base;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef std::pair<Key, MatrixZD> KeyMatrixZD;
public:
/**
* Constructor
*/
JacobianFactorQR(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b,
JacobianFactorQR(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E,
const Matrix3& P, const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D, ZDim>() {
Base() {
// Create a number of Jacobian factors in a factor graph
GaussianFactorGraph gfg;
Symbol pointKey('p', 0);
size_t i = 0;
BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) {
BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) {
gfg.add(pointKey, E.block<ZDim, 3>(ZDim * i, 0), it.first, it.second,
b.segment<ZDim>(ZDim * i), model);
i += 1;
@ -45,7 +47,7 @@ public:
// eliminate the point
boost::shared_ptr<GaussianBayesNet> bn;
GaussianFactorGraph::shared_ptr fg;
std::vector < Key > variables;
std::vector<Key> variables;
variables.push_back(pointKey);
boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
//fg->print("fg");
@ -53,6 +55,6 @@ public:
JacobianFactor::operator=(JacobianFactor(*fg));
}
};
// class
// end class JacobianFactorQR
}// gtsam
}// end namespace gtsam

39
gtsam/slam/JacobianFactorSVD.h
View File

@ -12,43 +12,50 @@ namespace gtsam {
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D, size_t ZDim>
class JacobianFactorSVD: public JacobianSchurFactor<D, ZDim> {
class JacobianFactorSVD: public JacobianSchurFactor<D> {
typedef JacobianSchurFactor<D> Base;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD; // e.g 2 x 6 with Z=Point2
typedef std::pair<Key, MatrixZD> KeyMatrixZD;
typedef std::pair<Key, Matrix> KeyMatrix;
public:
typedef Eigen::Matrix<double, ZDim, D> Matrix2D; // e.g 2 x 6 with Z=Point2
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
typedef std::pair<Key, Matrix> KeyMatrix;
/// Default constructor
JacobianFactorSVD() {}
JacobianFactorSVD() {
}
/// Empty constructor with keys
JacobianFactorSVD(const FastVector<Key>& keys,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
Matrix zeroMatrix = Matrix::Zero(0,D);
JacobianFactorSVD(const FastVector<Key>& keys, //
const SharedDiagonal& model = SharedDiagonal()) :
Base() {
Matrix zeroMatrix = Matrix::Zero(0, D);
Vector zeroVector = Vector::Zero(0);
std::vector<KeyMatrix> QF;
QF.reserve(keys.size());
BOOST_FOREACH(const Key& key, keys)
QF.push_back(KeyMatrix(key, zeroMatrix));
QF.push_back(KeyMatrix(key, zeroMatrix));
JacobianFactor::fillTerms(QF, zeroVector, model);
}
/// Constructor
JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
JacobianFactorSVD(const std::vector<KeyMatrixZD>& Fblocks,
const Matrix& Enull, const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) :
Base() {
size_t numKeys = Enull.rows() / ZDim;
size_t j = 0, m2 = ZDim*numKeys-3;
size_t j = 0, m2 = ZDim * numKeys - 3;
// PLAIN NULL SPACE TRICK
// Matrix Q = Enull * Enull.transpose();
// BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
// BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
// QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
// JacobianFactor factor(QF, Q * b);
std::vector<KeyMatrix> QF;
QF.reserve(numKeys);
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second));
BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
QF.push_back(
KeyMatrix(it.first,
(Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second));
JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
}
};

5
gtsam/slam/JacobianSchurFactor.h
View File

@ -27,14 +27,11 @@ namespace gtsam {
* Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD
* Provides raw memory access versions of linear operator.
*/
template<size_t D, size_t ZDim>
template<size_t D>
class JacobianSchurFactor: public JacobianFactor {
public:
typedef Eigen::Matrix<double, ZDim, D> Matrix2D;
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;