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 SmartFactorBase<CAMERA> This;
typedef typename CAMERA::Measurement Z; 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: protected:
/** /**
* As of Feb 22, 2015, the noise model is the same for all measurements and * 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 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 // Cache for Fblocks, to avoid a malloc ever time we re-linearize
mutable std::vector<MatrixZD> Fblocks; mutable std::vector<MatrixZD> Fblocks;
public: public:
// Definitions for blocks of F, externally visible
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
@ -101,10 +94,9 @@ public:
/// Constructor /// Constructor
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none, size_t expectedNumberCameras=10) : boost::optional<Pose3> body_P_sensor = boost::none,
body_P_sensor_(body_P_sensor) { size_t expectedNumberCameras = 10)
: body_P_sensor_(body_P_sensor), Fblocks(expectedNumberCameras) {
Fblocks.reserve(expectedNumberCameras);
if (!sharedNoiseModel) if (!sharedNoiseModel)
throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required"); throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
@ -293,8 +285,7 @@ public:
computeJacobians(Fblocks, E, b, cameras, point); computeJacobians(Fblocks, E, b, cameras, point);
// build augmented hessian // build augmented hessian
SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E, SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E, b);
b);
return boost::make_shared<RegularHessianFactor<Dim> >(keys_, return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
augmentedHessian); augmentedHessian);
@ -311,10 +302,8 @@ public:
const FastVector<Key> allKeys) const { const FastVector<Key> allKeys) const {
Matrix E; Matrix E;
Vector b; Vector b;
std::vector<MatrixZD> Fblocks;
computeJacobians(Fblocks, E, b, cameras, point); computeJacobians(Fblocks, E, b, cameras, point);
Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_, Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_, augmentedHessian);
augmentedHessian);
} }
/// Whiten the Jacobians computed by computeJacobians using noiseModel_ /// Whiten the Jacobians computed by computeJacobians using noiseModel_