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

Got rid of obsolete types and mate FBlocks a mutable cache

release/4.3a0
Frank 2016-02-25 16:53:17 -08:00
parent a611cd078d
commit 55bebdc72d
1 changed files with 11 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam/slam/SmartFactorBase.h
View File

@ -50,6 +50,12 @@ private:
typedef SmartFactorBase<CAMERA> This;
typedef typename CAMERA::Measurement Z;
public:
static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrpt camera)
protected:
/**
* As of Feb 22, 2015, the noise model is the same for all measurements and
@ -70,24 +76,11 @@ protected:
const boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
/// @}
static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
// Definitions for block matrices used internally
typedef Eigen::Matrix<double, Dim, ZDim> MatrixD2; // F'
typedef Eigen::Matrix<double, Dim, Dim> MatrixDD; // camera hessian block
typedef Eigen::Matrix<double, ZDim, 3> Matrix23;
typedef Eigen::Matrix<double, Dim, 1> VectorD;
typedef Eigen::Matrix<double, ZDim, ZDim> Matrix2;
// Cache for Fblocks, to avoid a malloc ever time we re-linearize
mutable std::vector<MatrixZD> Fblocks;
public:
// Definitions for blocks of F, externally visible
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for a smart pointer to a factor
@ -101,10 +94,9 @@ public:
/// Constructor
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none, size_t expectedNumberCameras=10) :
body_P_sensor_(body_P_sensor) {
Fblocks.reserve(expectedNumberCameras);
boost::optional<Pose3> body_P_sensor = boost::none,
size_t expectedNumberCameras = 10)
: body_P_sensor_(body_P_sensor), Fblocks(expectedNumberCameras) {
if (!sharedNoiseModel)
throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
@ -293,8 +285,7 @@ public:
computeJacobians(Fblocks, E, b, cameras, point);
// build augmented hessian
SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E,
b);
SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E, b);
return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
augmentedHessian);
@ -311,10 +302,8 @@ public:
const FastVector<Key> allKeys) const {
Matrix E;
Vector b;
std::vector<MatrixZD> Fblocks;
computeJacobians(Fblocks, E, b, cameras, point);
Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_,
augmentedHessian);
Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_, augmentedHessian);
}
/// Whiten the Jacobians computed by computeJacobians using noiseModel_