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

templated SmartFactor classes on measurement dimension, ZDim

release/4.3a0
cbeall3 2014-11-17 19:10:27 -05:00
parent d24b799988
commit dde32f7fe2
8 changed files with 57 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/slam/JacobianFactorQ.h
View File

@ -12,10 +12,10 @@ namespace gtsam {
/** /**
* JacobianFactor for Schur complement that uses Q noise model * JacobianFactor for Schur complement that uses Q noise model
*/ */
template<size_t D> template<size_t D, size_t ZDim>
class JacobianFactorQ: public JacobianSchurFactor<D> { class JacobianFactorQ: public JacobianSchurFactor<D, ZDim> {
typedef JacobianSchurFactor<D> Base; typedef JacobianSchurFactor<D, ZDim> Base;
public: public:
@ -25,7 +25,7 @@ public:
/// Empty constructor with keys /// Empty constructor with keys
JacobianFactorQ(const FastVector<Key>& keys, JacobianFactorQ(const FastVector<Key>& keys,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() { const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
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;
@ -37,11 +37,11 @@ public:
} }
/// Constructor /// Constructor
JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D, Eigen::aligned_allocator<typename Base::KeyMatrix2D> >& Fblocks, JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b, const Matrix& E, const Matrix3& P, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) : const SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D>() { JacobianSchurFactor<D, ZDim>() {
size_t j = 0, m2 = E.rows(), m = m2 / 2; 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();
// Calculate pre-computed Jacobian matrices // Calculate pre-computed Jacobian matrices
@ -51,7 +51,7 @@ public:
QF.reserve(m); QF.reserve(m);
// Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D) // Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D)
BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) BOOST_FOREACH(const typename Base::KeyMatrix2D& 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, ZDim * j++, m2, ZDim) * it.second));
// Which is then passed to the normal JacobianFactor constructor // Which is then passed to the normal JacobianFactor constructor
JacobianFactor::fillTerms(QF, Q * b, model); JacobianFactor::fillTerms(QF, Q * b, model);
} }

12
gtsam/slam/JacobianFactorQR.h
View File

@ -12,10 +12,10 @@ namespace gtsam {
/** /**
* JacobianFactor for Schur complement that uses Q noise model * JacobianFactor for Schur complement that uses Q noise model
*/ */
template<size_t D> template<size_t D, size_t ZDim>
class JacobianFactorQR: public JacobianSchurFactor<D> { class JacobianFactorQR: public JacobianSchurFactor<D, ZDim> {
typedef JacobianSchurFactor<D> Base; typedef JacobianSchurFactor<D, ZDim> Base;
public: public:
@ -25,14 +25,14 @@ public:
JacobianFactorQR(const std::vector<typename Base::KeyMatrix2D>& Fblocks, JacobianFactorQR(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b, const Matrix& E, const Matrix3& P, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) : const SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D>() { JacobianSchurFactor<D, ZDim>() {
// 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 typename Base::KeyMatrix2D& it, Fblocks) {
gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second, gfg.add(pointKey, E.block<ZDim, 3>(ZDim * i, 0), it.first, it.second,
b.segment<2>(2 * i), model); b.segment<ZDim>(ZDim * i), model);
i += 1; i += 1;
} }
//gfg.print("gfg"); //gfg.print("gfg");

18
gtsam/slam/JacobianFactorSVD.h
View File

@ -11,12 +11,12 @@ namespace gtsam {
/** /**
* JacobianFactor for Schur complement that uses Q noise model * JacobianFactor for Schur complement that uses Q noise model
*/ */
template<size_t D, class Z> template<size_t D, size_t ZDim>
class JacobianFactorSVD: public JacobianSchurFactor<D> { class JacobianFactorSVD: public JacobianSchurFactor<D, ZDim> {
public: public:
typedef Eigen::Matrix<double, Z::dimension, D> Matrix2D; // e.g 2 x 6 with Z=Point2 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, Matrix2D> KeyMatrix2D;
typedef std::pair<Key, Matrix> KeyMatrix; typedef std::pair<Key, Matrix> KeyMatrix;
@ -25,7 +25,7 @@ public:
/// Empty constructor with keys /// Empty constructor with keys
JacobianFactorSVD(const FastVector<Key>& keys, JacobianFactorSVD(const FastVector<Key>& keys,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() { const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
Matrix zeroMatrix = Matrix::Zero(0,D); Matrix zeroMatrix = Matrix::Zero(0,D);
Vector zeroVector = Vector::Zero(0); Vector zeroVector = Vector::Zero(0);
std::vector<KeyMatrix> QF; std::vector<KeyMatrix> QF;
@ -36,10 +36,10 @@ public:
} }
/// Constructor /// Constructor
JacobianFactorSVD(const std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D> >& Fblocks, const Matrix& Enull, const Vector& b, JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() { const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
size_t numKeys = Enull.rows() / Z::Dim(); size_t numKeys = Enull.rows() / ZDim;
size_t j = 0, m2 = Z::Dim()*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 KeyMatrix2D& it, Fblocks)
@ -48,7 +48,7 @@ public:
std::vector<KeyMatrix> QF; std::vector<KeyMatrix> QF;
QF.reserve(numKeys); QF.reserve(numKeys);
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, Z::Dim() * j++, m2, Z::Dim()) * 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);
} }
}; };

4
gtsam/slam/JacobianSchurFactor.h
View File

@ -18,12 +18,12 @@ namespace gtsam {
/** /**
* JacobianFactor for Schur complement that uses Q noise model * JacobianFactor for Schur complement that uses Q noise model
*/ */
template<size_t D> template<size_t D, size_t ZDim>
class JacobianSchurFactor: public JacobianFactor { class JacobianSchurFactor: public JacobianFactor {
public: public:
typedef Eigen::Matrix<double, 2, D> Matrix2D; typedef Eigen::Matrix<double, ZDim, D> Matrix2D;
typedef std::pair<Key, Matrix2D> KeyMatrix2D; typedef std::pair<Key, Matrix2D> KeyMatrix2D;
// Use eigen magic to access raw memory // Use eigen magic to access raw memory

38
gtsam/slam/SmartFactorBase.h
View File

@ -34,7 +34,6 @@
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <vector> #include <vector>
#include <gtsam/3rdparty/gtsam_eigen_includes.h> #include <gtsam/3rdparty/gtsam_eigen_includes.h>
//#include <gtsam/3rdparty/Eigen/Eigen/StdVector>
namespace gtsam { namespace gtsam {
/// Base class with no internal point, completely functional /// Base class with no internal point, completely functional
@ -50,14 +49,16 @@ protected:
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras)
typedef traits::dimension<Z> ZDim_t; ///< Dimension trait of measurement type
/// Definitions for blocks of F /// Definitions for blocks of F
typedef Eigen::Matrix<double, Z::dimension, D> Matrix2D; // F typedef Eigen::Matrix<double, ZDim_t::value, D> Matrix2D; // F
typedef Eigen::Matrix<double, D, Z::dimension> MatrixD2; // F' typedef Eigen::Matrix<double, D, ZDim_t::value> MatrixD2; // F'
typedef std::pair<Key, Matrix2D> KeyMatrix2D; // Fblocks typedef std::pair<Key, Matrix2D> KeyMatrix2D; // Fblocks
typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block
typedef Eigen::Matrix<double, Z::dimension, 3> Matrix23; typedef Eigen::Matrix<double, ZDim_t::value, 3> Matrix23;
typedef Eigen::Matrix<double, D, 1> VectorD; typedef Eigen::Matrix<double, D, 1> VectorD;
typedef Eigen::Matrix<double, Z::dimension, Z::dimension> Matrix2; typedef Eigen::Matrix<double, ZDim_t::value, ZDim_t::value> Matrix2;
/// shorthand for base class type /// shorthand for base class type
typedef NonlinearFactor Base; typedef NonlinearFactor Base;
@ -65,6 +66,7 @@ protected:
/// shorthand for this class /// shorthand for this class
typedef SmartFactorBase<POSE, Z, CAMERA, D> This; typedef SmartFactorBase<POSE, Z, CAMERA, D> This;
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@ -262,7 +264,7 @@ public:
// **************************************************************************************************** // ****************************************************************************************************
/// Compute F, E only (called below in both vanilla and SVD versions) /// Compute F, E only (called below in both vanilla and SVD versions)
/// Given a Point3, assumes dimensionality is 3 /// Given a Point3, assumes dimensionality is 3
double computeJacobians(std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D> >& Fblocks, Matrix& E, double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
Vector& b, const Cameras& cameras, const Point3& point) const { Vector& b, const Cameras& cameras, const Point3& point) const {
size_t numKeys = this->keys_.size(); size_t numKeys = this->keys_.size();
@ -292,11 +294,11 @@ public:
if (D == 6) { // optimize only camera pose if (D == 6) { // optimize only camera pose
Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi)); Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi));
} else { } else {
Hcam.block<Z::dimension, 6>(0, 0) = Fi; // Z::Dim() x 6 block for the cameras Hcam.block<ZDim_t::value, 6>(0, 0) = Fi; // Z::Dim() x 6 block for the cameras
Hcam.block<Z::dimension, D - 6>(0, 6) = Hcali; // Z::Dim() x nrCal block for the cameras Hcam.block<ZDim_t::value, D - 6>(0, 6) = Hcali; // Z::Dim() x nrCal block for the cameras
Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam));
} }
E.block<Z::dimension, 3>(Z::dimension * i, 0) = Ei; E.block<ZDim_t::value, 3>(ZDim_t::value * i, 0) = Ei;
subInsert(b, bi, Z::Dim() * i); subInsert(b, bi, Z::Dim() * i);
} }
return f; return f;
@ -304,7 +306,7 @@ public:
// **************************************************************************************************** // ****************************************************************************************************
/// Version that computes PointCov, with optional lambda parameter /// Version that computes PointCov, with optional lambda parameter
double computeJacobians(std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D> >& Fblocks, Matrix& E, double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point,
double lambda = 0.0, bool diagonalDamping = false) const { double lambda = 0.0, bool diagonalDamping = false) const {
@ -335,20 +337,20 @@ public:
const double lambda = 0.0) const { const double lambda = 0.0) const {
size_t numKeys = this->keys_.size(); size_t numKeys = this->keys_.size();
std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D> > Fblocks; std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
lambda); lambda);
F = zeros(Z::Dim() * numKeys, D * numKeys); F = zeros(Z::Dim() * numKeys, D * numKeys);
for (size_t i = 0; i < this->keys_.size(); ++i) { for (size_t i = 0; i < this->keys_.size(); ++i) {
F.block<Z::dimension, D>(Z::Dim() * i, D * i) = Fblocks.at(i).second; // Z::Dim() x 6 block for the cameras F.block<ZDim_t::value, D>(ZDim_t::value * i, D * i) = Fblocks.at(i).second; // Z::Dim() x 6 block for the cameras
} }
return f; return f;
} }
// **************************************************************************************************** // ****************************************************************************************************
/// SVD version /// SVD version
double computeJacobiansSVD(std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D> >& Fblocks, Matrix& Enull, double computeJacobiansSVD(std::vector<KeyMatrix2D>& Fblocks, Matrix& Enull,
Vector& b, const Cameras& cameras, const Point3& point, double lambda = Vector& b, const Cameras& cameras, const Point3& point, double lambda =
0.0, bool diagonalDamping = false) const { 0.0, bool diagonalDamping = false) const {
@ -645,27 +647,27 @@ public:
} }
// **************************************************************************************************** // ****************************************************************************************************
boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor( boost::shared_ptr<JacobianFactorQ<D, ZDim_t::value> > createJacobianQFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0, const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D> > Fblocks; std::vector<KeyMatrix2D> Fblocks;
Matrix E; Matrix E;
Matrix3 PointCov; Matrix3 PointCov;
Vector b; Vector b;
computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping); diagonalDamping);
return boost::make_shared<JacobianFactorQ<D> >(Fblocks, E, PointCov, b); return boost::make_shared<JacobianFactorQ<D, ZDim_t::value> >(Fblocks, E, PointCov, b);
} }
// **************************************************************************************************** // ****************************************************************************************************
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor( boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0) const { const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
size_t numKeys = this->keys_.size(); size_t numKeys = this->keys_.size();
std::vector < KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D> > Fblocks; std::vector<KeyMatrix2D> Fblocks;
Vector b; Vector b;
Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3); Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3);
computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda);
return boost::make_shared< JacobianFactorSVD<6, Z> >(Fblocks, Enull, b); return boost::make_shared< JacobianFactorSVD<6, ZDim_t::value> >(Fblocks, Enull, b);
} }
private: private:

12
gtsam/slam/SmartProjectionFactor.h
View File

@ -104,6 +104,8 @@ protected:
/// shorthand for this class /// shorthand for this class
typedef SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This; typedef SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This;
typedef traits::dimension<gtsam::Point2> ZDim_t; ///< Dimension trait of measurement type
public: public:
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
@ -418,16 +420,16 @@ public:
} }
/// create factor /// create factor
boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor( boost::shared_ptr<JacobianFactorQ<D, ZDim_t::value> > createJacobianQFactor(
const Cameras& cameras, double lambda) const { const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
return Base::createJacobianQFactor(cameras, point_, lambda); return Base::createJacobianQFactor(cameras, point_, lambda);
else else
return boost::make_shared< JacobianFactorQ<D> >(this->keys_); return boost::make_shared< JacobianFactorQ<D, ZDim_t::value> >(this->keys_);
} }
/// Create a factor, takes values /// Create a factor, takes values
boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor( boost::shared_ptr<JacobianFactorQ<D, ZDim_t::value> > createJacobianQFactor(
const Values& values, double lambda) const { const Values& values, double lambda) const {
Cameras myCameras; Cameras myCameras;
// TODO triangulate twice ?? // TODO triangulate twice ??
@ -435,7 +437,7 @@ public:
if (nonDegenerate) if (nonDegenerate)
return createJacobianQFactor(myCameras, lambda); return createJacobianQFactor(myCameras, lambda);
else else
return boost::make_shared< JacobianFactorQ<D> >(this->keys_); return boost::make_shared< JacobianFactorQ<D, ZDim_t::value> >(this->keys_);
} }
/// different (faster) way to compute Jacobian factor /// different (faster) way to compute Jacobian factor
@ -444,7 +446,7 @@ public:
if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda); return Base::createJacobianSVDFactor(cameras, point_, lambda);
else else
return boost::make_shared< JacobianFactorSVD<D, Point2> >(this->keys_); return boost::make_shared< JacobianFactorSVD<D, ZDim_t::value> >(this->keys_);
} }
/// Returns true if nonDegenerate /// Returns true if nonDegenerate

4
gtsam/slam/SmartStereoProjectionFactor.h
View File

@ -107,6 +107,8 @@ protected:
/// shorthand for this class /// shorthand for this class
typedef SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This; typedef SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This;
typedef traits::dimension<gtsam::StereoPoint2> ZDim_t; ///< Dimension trait of measurement type
public: public:
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
@ -480,7 +482,7 @@ public:
if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda); return Base::createJacobianSVDFactor(cameras, point_, lambda);
else else
return boost::make_shared< JacobianFactorSVD<D, StereoPoint2> >(this->keys_); return boost::make_shared< JacobianFactorSVD<D, ZDim_t::value> >(this->keys_);
} }
/// Returns true if nonDegenerate /// Returns true if nonDegenerate

4
gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
View File

@ -115,7 +115,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
// Create JacobianFactor with same error // Create JacobianFactor with same error
const SharedDiagonal model; const SharedDiagonal model;
JacobianFactorQ<6> jf(Fblocks, E, P, b, model); JacobianFactorQ<6, 2> jf(Fblocks, E, P, b, model);
{ // error { // error
double expectedError = jf.error(xvalues); double expectedError = jf.error(xvalues);
@ -165,7 +165,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
} }
// Create JacobianFactorQR // Create JacobianFactorQR
JacobianFactorQR<6> jfq(Fblocks, E, P, b, model); JacobianFactorQR<6, 2> jfq(Fblocks, E, P, b, model);
{ {
const SharedDiagonal model; const SharedDiagonal model;
VectorValues yActual = zero; VectorValues yActual = zero;