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
*/
template<size_t D>
class JacobianFactorQ: public JacobianSchurFactor<D> {
template<size_t D, size_t ZDim>
class JacobianFactorQ: public JacobianSchurFactor<D, ZDim> {
typedef JacobianSchurFactor<D> Base;
typedef JacobianSchurFactor<D, ZDim> Base;
public:
@ -25,7 +25,7 @@ public:
/// Empty constructor with 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);
Vector zeroVector = Vector::Zero(0);
typedef std::pair<Key, Matrix> KeyMatrix;
@ -37,11 +37,11 @@ public:
}
/// 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 SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D>() {
size_t j = 0, m2 = E.rows(), m = m2 / 2;
JacobianSchurFactor<D, ZDim>() {
size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
// Calculate projector Q
Matrix Q = eye(m2) - E * P * E.transpose();
// Calculate pre-computed Jacobian matrices
@ -51,7 +51,7 @@ public:
QF.reserve(m);
// 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)
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
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
*/
template<size_t D>
class JacobianFactorQR: public JacobianSchurFactor<D> {
template<size_t D, size_t ZDim>
class JacobianFactorQR: public JacobianSchurFactor<D, ZDim> {
typedef JacobianSchurFactor<D> Base;
typedef JacobianSchurFactor<D, ZDim> Base;
public:
@ -25,14 +25,14 @@ public:
JacobianFactorQR(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D>() {
JacobianSchurFactor<D, ZDim>() {
// 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) {
gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second,
b.segment<2>(2 * i), model);
gfg.add(pointKey, E.block<ZDim, 3>(ZDim * i, 0), it.first, it.second,
b.segment<ZDim>(ZDim * i), model);
i += 1;
}
//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
*/
template<size_t D, class Z>
class JacobianFactorSVD: public JacobianSchurFactor<D> {
template<size_t D, size_t ZDim>
class JacobianFactorSVD: public JacobianSchurFactor<D, ZDim> {
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, Matrix> KeyMatrix;
@ -25,7 +25,7 @@ public:
/// Empty constructor with 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);
Vector zeroVector = Vector::Zero(0);
std::vector<KeyMatrix> QF;
@ -36,10 +36,10 @@ public:
}
/// Constructor
JacobianFactorSVD(const std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D> >& Fblocks, const Matrix& Enull, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
size_t numKeys = Enull.rows() / Z::Dim();
size_t j = 0, m2 = Z::Dim()*numKeys-3;
JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
size_t numKeys = Enull.rows() / ZDim;
size_t j = 0, m2 = ZDim*numKeys-3;
// PLAIN NULL SPACE TRICK
// Matrix Q = Enull * Enull.transpose();
// BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
@ -48,7 +48,7 @@ public:
std::vector<KeyMatrix> QF;
QF.reserve(numKeys);
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);
}
};

4
gtsam/slam/JacobianSchurFactor.h
View File

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

38
gtsam/slam/SmartFactorBase.h
View File

@ -34,7 +34,6 @@
#include <boost/make_shared.hpp>
#include <vector>
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
//#include <gtsam/3rdparty/Eigen/Eigen/StdVector>
namespace gtsam {
/// 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)
typedef traits::dimension<Z> ZDim_t; ///< Dimension trait of measurement type
/// Definitions for blocks of F
typedef Eigen::Matrix<double, Z::dimension, D> Matrix2D; // F
typedef Eigen::Matrix<double, D, Z::dimension> MatrixD2; // F'
typedef Eigen::Matrix<double, ZDim_t::value, D> Matrix2D; // F
typedef Eigen::Matrix<double, D, ZDim_t::value> MatrixD2; // F'
typedef std::pair<Key, Matrix2D> KeyMatrix2D; // Fblocks
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, Z::dimension, Z::dimension> Matrix2;
typedef Eigen::Matrix<double, ZDim_t::value, ZDim_t::value> Matrix2;
/// shorthand for base class type
typedef NonlinearFactor Base;
@ -65,6 +66,7 @@ protected:
/// shorthand for this class
typedef SmartFactorBase<POSE, Z, CAMERA, D> This;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@ -262,7 +264,7 @@ public:
// ****************************************************************************************************
/// Compute F, E only (called below in both vanilla and SVD versions)
/// 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 {
size_t numKeys = this->keys_.size();
@ -292,11 +294,11 @@ public:
if (D == 6) { // optimize only camera pose
Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi));
} else {
Hcam.block<Z::dimension, 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, 6>(0, 0) = Fi; // Z::Dim() x 6 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));
}
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);
}
return f;
@ -304,7 +306,7 @@ public:
// ****************************************************************************************************
/// 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,
double lambda = 0.0, bool diagonalDamping = false) const {
@ -335,20 +337,20 @@ public:
const double lambda = 0.0) const {
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,
lambda);
F = zeros(Z::Dim() * numKeys, D * numKeys);
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;
}
// ****************************************************************************************************
/// 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 =
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,
bool diagonalDamping = false) const {
std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D> > Fblocks;
std::vector<KeyMatrix2D> Fblocks;
Matrix E;
Matrix3 PointCov;
Vector b;
computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
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(
const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
size_t numKeys = this->keys_.size();
std::vector < KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D> > Fblocks;
std::vector<KeyMatrix2D> Fblocks;
Vector b;
Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3);
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:

12
gtsam/slam/SmartProjectionFactor.h
View File

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

4
gtsam/slam/SmartStereoProjectionFactor.h
View File

@ -107,6 +107,8 @@ protected:
/// shorthand for this class
typedef SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This;
typedef traits::dimension<gtsam::StereoPoint2> ZDim_t; ///< Dimension trait of measurement type
public:
/// shorthand for a smart pointer to a factor
@ -480,7 +482,7 @@ public:
if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda);
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

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

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