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 * JacobianFactor for Schur complement that uses Q noise model
*/ */
template<size_t D, size_t ZDim> 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: public:
@ -37,7 +39,7 @@ public:
/// Empty constructor with keys /// Empty constructor with keys
JacobianFactorQ(const FastVector<Key>& keys, // JacobianFactorQ(const FastVector<Key>& keys, //
const SharedDiagonal& model = SharedDiagonal()) : const SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D, ZDim>() { Base() {
Matrix zeroMatrix = Matrix::Zero(0, D); Matrix zeroMatrix = Matrix::Zero(0, D);
Vector zeroVector = Vector::Zero(0); Vector zeroVector = Vector::Zero(0);
typedef std::pair<Key, Matrix> KeyMatrix; typedef std::pair<Key, Matrix> KeyMatrix;
@ -49,10 +51,10 @@ public:
} }
/// Constructor /// Constructor
JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D>& Fblocks, JacobianFactorQ(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E,
const Matrix& E, const Matrix3& P, const Vector& b, const Matrix3& P, const Vector& b, const SharedDiagonal& model =
const SharedDiagonal& model = SharedDiagonal()) : SharedDiagonal()) :
JacobianSchurFactor<D, ZDim>() { Base() {
size_t j = 0, m2 = E.rows(), m = m2 / ZDim; size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
// Calculate projector Q // Calculate projector Q
Matrix Q = eye(m2) - E * P * E.transpose(); Matrix Q = eye(m2) - E * P * E.transpose();
@ -62,7 +64,7 @@ public:
std::vector<KeyMatrix> QF; std::vector<KeyMatrix> QF;
QF.reserve(m); QF.reserve(m);
// Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D) // 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( QF.push_back(
KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second));
// Which is then passed to the normal JacobianFactor constructor // 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 * JacobianFactor for Schur complement that uses Q noise model
*/ */
template<size_t D, size_t ZDim> 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: public:
/** /**
* Constructor * Constructor
*/ */
JacobianFactorQR(const std::vector<typename Base::KeyMatrix2D>& Fblocks, JacobianFactorQR(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E,
const Matrix& E, const Matrix3& P, const Vector& b, const Matrix3& P, const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) : const SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D, ZDim>() { Base() {
// Create a number of Jacobian factors in a factor graph // Create a number of Jacobian factors in a factor graph
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
Symbol pointKey('p', 0); Symbol pointKey('p', 0);
size_t i = 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, gfg.add(pointKey, E.block<ZDim, 3>(ZDim * i, 0), it.first, it.second,
b.segment<ZDim>(ZDim * i), model); b.segment<ZDim>(ZDim * i), model);
i += 1; i += 1;
@ -45,7 +47,7 @@ public:
// eliminate the point // eliminate the point
boost::shared_ptr<GaussianBayesNet> bn; boost::shared_ptr<GaussianBayesNet> bn;
GaussianFactorGraph::shared_ptr fg; GaussianFactorGraph::shared_ptr fg;
std::vector < Key > variables; std::vector<Key> variables;
variables.push_back(pointKey); variables.push_back(pointKey);
boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR); boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
//fg->print("fg"); //fg->print("fg");
@ -53,6 +55,6 @@ public:
JacobianFactor::operator=(JacobianFactor(*fg)); 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 * JacobianFactor for Schur complement that uses Q noise model
*/ */
template<size_t D, size_t ZDim> 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: 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 /// Default constructor
JacobianFactorSVD() {} JacobianFactorSVD() {
}
/// Empty constructor with keys /// Empty constructor with keys
JacobianFactorSVD(const FastVector<Key>& keys, JacobianFactorSVD(const FastVector<Key>& keys, //
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() { const SharedDiagonal& model = SharedDiagonal()) :
Matrix zeroMatrix = Matrix::Zero(0,D); Base() {
Matrix zeroMatrix = Matrix::Zero(0, D);
Vector zeroVector = Vector::Zero(0); Vector zeroVector = Vector::Zero(0);
std::vector<KeyMatrix> QF; std::vector<KeyMatrix> QF;
QF.reserve(keys.size()); QF.reserve(keys.size());
BOOST_FOREACH(const Key& key, keys) BOOST_FOREACH(const Key& key, keys)
QF.push_back(KeyMatrix(key, zeroMatrix)); QF.push_back(KeyMatrix(key, zeroMatrix));
JacobianFactor::fillTerms(QF, zeroVector, model); JacobianFactor::fillTerms(QF, zeroVector, model);
} }
/// Constructor /// Constructor
JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b, JacobianFactorSVD(const std::vector<KeyMatrixZD>& Fblocks,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() { const Matrix& Enull, const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) :
Base() {
size_t numKeys = Enull.rows() / ZDim; 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 // PLAIN NULL SPACE TRICK
// Matrix Q = Enull * Enull.transpose(); // 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)); // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
// JacobianFactor factor(QF, Q * b); // JacobianFactor factor(QF, Q * b);
std::vector<KeyMatrix> QF; std::vector<KeyMatrix> QF;
QF.reserve(numKeys); QF.reserve(numKeys);
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second)); QF.push_back(
KeyMatrix(it.first,
(Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second));
JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); 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 * Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD
* Provides raw memory access versions of linear operator. * Provides raw memory access versions of linear operator.
*/ */
template<size_t D, size_t ZDim> template<size_t D>
class JacobianSchurFactor: public JacobianFactor { class JacobianSchurFactor: public JacobianFactor {
public: public:
typedef Eigen::Matrix<double, ZDim, D> Matrix2D;
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
// Use eigen magic to access raw memory // Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector; typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap; typedef Eigen::Map<DVector> DMap;