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

Prototype cacheing

release/4.3a0
dellaert 2016-02-25 09:34:13 -08:00
parent eef3fdd3db
commit e76f838d2f
1 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/slam/SmartFactorBase.h
View File

@ -80,6 +80,9 @@ protected:
typedef Eigen::Matrix<double, Dim, 1> VectorD; typedef Eigen::Matrix<double, Dim, 1> VectorD;
typedef Eigen::Matrix<double, ZDim, ZDim> Matrix2; 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: public:
// Definitions for blocks of F, externally visible // Definitions for blocks of F, externally visible
@ -98,8 +101,10 @@ public:
/// Constructor /// Constructor
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none) : boost::optional<Pose3> body_P_sensor = boost::none, size_t expectedNumberCameras=10) :
body_P_sensor_(body_P_sensor){ body_P_sensor_(body_P_sensor) {
Fblocks.reserve(expectedNumberCameras);
if (!sharedNoiseModel) if (!sharedNoiseModel)
throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required"); throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
@ -283,7 +288,6 @@ public:
const Cameras& cameras, const Point3& point, const double lambda = 0.0, const Cameras& cameras, const Point3& point, const double lambda = 0.0,
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
std::vector<MatrixZD> Fblocks;
Matrix E; Matrix E;
Vector b; Vector b;
computeJacobians(Fblocks, E, b, cameras, point); computeJacobians(Fblocks, E, b, cameras, point);