diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index d98596a9f..d1dc7625c 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -82,20 +82,22 @@ namespace gtsam { class GTSAM_EXPORT JacobianFactor : public GaussianFactor { public: + typedef JacobianFactor This; ///< Typedef to this class typedef GaussianFactor Base; ///< Typedef to base class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - protected: - VerticalBlockMatrix Ab_; // the block view of the full matrix - noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix - - public: typedef VerticalBlockMatrix::Block ABlock; typedef VerticalBlockMatrix::constBlock constABlock; typedef ABlock::ColXpr BVector; typedef constABlock::ConstColXpr constBVector; + protected: + + VerticalBlockMatrix Ab_; // the block view of the full matrix + noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix + + public: /** Convert from other GaussianFactor */ explicit JacobianFactor(const GaussianFactor& gf); @@ -140,25 +142,6 @@ namespace gtsam { JacobianFactor( const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); - /** - * Constructor - * @param keys in some order - * @param diemnsions of the variables in same order - * @param m output dimension - * @param model noise model (default NULL) - */ - template - JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m, - const SharedDiagonal& model = SharedDiagonal()) : - Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) { - Ab_.matrix().setZero(); - } - - /// Direct access to VerticalBlockMatrix - VerticalBlockMatrix& Ab() { - return Ab_; - } - /** * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots * structure computed for \c graph is already available, providing it will reduce the amount of @@ -347,6 +330,21 @@ namespace gtsam { private: + /** Unsafe Constructor that creates an uninitialized Jacobian of right size + * @param keys in some order + * @param diemnsions of the variables in same order + * @param m output dimension + * @param model noise model (default NULL) + */ + template + JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m, + const SharedDiagonal& model = SharedDiagonal()) : + Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) { + } + + // be very selective on who can access these private methods: + template friend class ExpressionFactor; + /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 0394fbe5c..1d5d0cb12 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -107,24 +107,26 @@ public: virtual boost::shared_ptr linearize(const Values& x) const { - // Create noise model - SharedDiagonal model; + // Check whether noise model is constrained or not noiseModel::Constrained::shared_ptr constrained = // boost::dynamic_pointer_cast(this->noiseModel_); - if (constrained) - model = constrained->unit(); // Create a writeable JacobianFactor in advance boost::shared_ptr factor( - new JacobianFactor(keys_, dimensions_, traits::dimension::value, - model)); + constrained ? new JacobianFactor(keys_, dimensions_, Dim, + constrained->unit()) : + new JacobianFactor(keys_, dimensions_, Dim)); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ - JacobianMap map(keys_, factor->Ab()); + VerticalBlockMatrix& Ab = factor->matrixObject(); + JacobianMap map(keys_, Ab); + + // Zero out Jacobian so we can simply add to it + Ab.matrix().setZero(); // Evaluate error to get Jacobians and RHS vector b T value = expression_.value(x, map); // <<< Reverse AD happens here ! - factor->Ab()(size()).col(0) = -measurement_.localCoordinates(value); + Ab(size()).col(0) = -measurement_.localCoordinates(value); // Whiten the corresponding system now // TODO ! this->noiseModel_->WhitenSystem(Ab); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 08496e5ff..23c2fa1ce 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -423,19 +423,6 @@ TEST(ExpressionFactor, composeTernary) { EXPECT( assert_equal(expected, *jf,1e-9)); } -/* ************************************************************************* */ -// Test Writeable JacobianFactor -TEST(ExpressionFactor, WriteableJacobianFactor) { - std::list keys = list_of(1)(2); - vector dimensions(2); - dimensions[0] = 6; - dimensions[1] = 3; - SharedDiagonal model; - JacobianFactor actual(keys, dimensions, 2, model); - JacobianFactor expected(1, zeros(2, 6), 2, zeros(2, 3), zero(2)); - EXPECT( assert_equal(expected, actual,1e-9)); -} - /* ************************************************************************* */ int main() { TestResult tr;