From 4ca9d5757fec99013005799e185e4fecd39f9de3 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Sat, 13 Sep 2014 01:34:33 -0400 Subject: [PATCH] Support optional dual key for constrained Jacobian and nonlinear factors. Default boost::none for unconstrained factors. --- gtsam/linear/JacobianFactor-inl.h | 165 ++++---- gtsam/linear/JacobianFactor.cpp | 147 ++++--- gtsam/linear/JacobianFactor.h | 625 ++++++++++++++++-------------- gtsam/nonlinear/NonlinearFactor.h | 462 ++++++++++++++-------- 4 files changed, 811 insertions(+), 588 deletions(-) diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index 7b0741ed5..2e6d15308 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -26,90 +26,93 @@ namespace gtsam { - /* ************************************************************************* */ - template - JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, const SharedDiagonal& model) - { - fillTerms(terms, b, model); +/* ************************************************************************* */ +template +JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, + const SharedDiagonal& model, const boost::optional& dualKey) : + dualKey_(dualKey) { + fillTerms(terms, b, model); +} + +/* ************************************************************************* */ +template +JacobianFactor::JacobianFactor(const KEYS& keys, + const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model, + const boost::optional& dualKey) : + Base(keys), Ab_(augmentedMatrix), dualKey_(dualKey) { + // Check noise model dimension + if (model && (DenseIndex) model->dim() != augmentedMatrix.rows()) + throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim()); + + // Check number of variables + if ((DenseIndex) Base::keys_.size() != augmentedMatrix.nBlocks() - 1) + throw std::invalid_argument( + "Error in JacobianFactor constructor input. Number of provided keys plus\n" + "one for the RHS vector must equal the number of provided matrix blocks."); + + // Check RHS dimension + if (augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1) + throw std::invalid_argument( + "Error in JacobianFactor constructor input. The last provided matrix block\n" + "must be the RHS vector, but the last provided block had more than one column."); + + // Take noise model + model_ = model; +} + +/* ************************************************************************* */ +namespace internal { +static inline DenseIndex getColsJF(const std::pair& p) { + return p.second.cols(); +} +} + +/* ************************************************************************* */ +template +void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b, + const SharedDiagonal& noiseModel) { + using boost::adaptors::transformed; + namespace br = boost::range; + + // Check noise model dimension + if (noiseModel && (DenseIndex) noiseModel->dim() != b.size()) + throw InvalidNoiseModel(b.size(), noiseModel->dim()); + + // Resize base class key vector + Base::keys_.resize(terms.size()); + + // Construct block matrix - this uses the boost::range 'transformed' construct to apply + // internal::getColsJF (defined just above here in this file) to each term. This + // transforms the list of terms into a list of variable dimensions, by extracting the + // number of columns in each matrix. This is done to avoid separately allocating an + // array of dimensions before constructing the VerticalBlockMatrix. + Ab_ = VerticalBlockMatrix(terms | transformed(&internal::getColsJF), b.size(), + true); + + // Check and add terms + DenseIndex i = 0; // For block index + for (typename TERMS::const_iterator termIt = terms.begin(); + termIt != terms.end(); ++termIt) { + const std::pair& term = *termIt; + + // Check block rows + if (term.second.rows() != Ab_.rows()) + throw InvalidMatrixBlock(Ab_.rows(), term.second.rows()); + + // Assign key and matrix + Base::keys_[i] = term.first; + Ab_(i) = term.second; + + // Increment block index + ++i; } - /* ************************************************************************* */ - template - JacobianFactor::JacobianFactor( - const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) : - Base(keys), Ab_(augmentedMatrix) - { - // Check noise model dimension - if(model && (DenseIndex)model->dim() != augmentedMatrix.rows()) - throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim()); + // Assign RHS vector + getb() = b; - // Check number of variables - if((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1) - throw std::invalid_argument( - "Error in JacobianFactor constructor input. Number of provided keys plus\n" - "one for the RHS vector must equal the number of provided matrix blocks."); - - // Check RHS dimension - if(augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1) - throw std::invalid_argument( - "Error in JacobianFactor constructor input. The last provided matrix block\n" - "must be the RHS vector, but the last provided block had more than one column."); - - // Take noise model - model_ = model; - } - - /* ************************************************************************* */ - namespace internal { - static inline DenseIndex getColsJF(const std::pair& p) { - return p.second.cols(); - } - } - - /* ************************************************************************* */ - template - void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel) - { - using boost::adaptors::transformed; - namespace br = boost::range; - - // Check noise model dimension - if(noiseModel && (DenseIndex)noiseModel->dim() != b.size()) - throw InvalidNoiseModel(b.size(), noiseModel->dim()); - - // Resize base class key vector - Base::keys_.resize(terms.size()); - - // Construct block matrix - this uses the boost::range 'transformed' construct to apply - // internal::getColsJF (defined just above here in this file) to each term. This - // transforms the list of terms into a list of variable dimensions, by extracting the - // number of columns in each matrix. This is done to avoid separately allocating an - // array of dimensions before constructing the VerticalBlockMatrix. - Ab_ = VerticalBlockMatrix(terms | transformed(&internal::getColsJF), b.size(), true); - - // Check and add terms - DenseIndex i = 0; // For block index - for(typename TERMS::const_iterator termIt = terms.begin(); termIt != terms.end(); ++termIt) { - const std::pair& term = *termIt; - - // Check block rows - if(term.second.rows() != Ab_.rows()) - throw InvalidMatrixBlock(Ab_.rows(), term.second.rows()); - - // Assign key and matrix - Base::keys_[i] = term.first; - Ab_(i) = term.second; - - // Increment block index - ++ i; - } - - // Assign RHS vector - getb() = b; - - // Assign noise model - model_ = noiseModel; - } + // Assign noise model + model_ = noiseModel; +} } // gtsam diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index ecaf05155..7e05322c7 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -59,7 +59,7 @@ namespace gtsam { /* ************************************************************************* */ JacobianFactor::JacobianFactor() : - Ab_(cref_list_of<1>(1), 0) { + Ab_(cref_list_of<1>(1), 0), dualKey_(boost::none) { getb().setZero(); } @@ -77,26 +77,30 @@ JacobianFactor::JacobianFactor(const GaussianFactor& gf) { /* ************************************************************************* */ JacobianFactor::JacobianFactor(const Vector& b_in) : - Ab_(cref_list_of<1>(1), b_in.size()) { + Ab_(cref_list_of<1>(1), b_in.size()), dualKey_(boost::none) { getb() = b_in; } /* ************************************************************************* */ JacobianFactor::JacobianFactor(Key i1, const Matrix& A1, const Vector& b, - const SharedDiagonal& model) { + const SharedDiagonal& model, const boost::optional& dualKey) : + dualKey_(dualKey) { fillTerms(cref_list_of<1>(make_pair(i1, A1)), b, model); } /* ************************************************************************* */ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, - const Matrix& A2, const Vector& b, const SharedDiagonal& model) { + const Matrix& A2, const Vector& b, const SharedDiagonal& model, + const boost::optional& dualKey) : + dualKey_(dualKey) { fillTerms(cref_list_of<2>(make_pair(i1, A1))(make_pair(i2, A2)), b, model); } /* ************************************************************************* */ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, const Matrix& A3, const Vector& b, - const SharedDiagonal& model) { + const SharedDiagonal& model, const boost::optional& dualKey) : + dualKey_(dualKey) { fillTerms( cref_list_of<3>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)), b, model); @@ -105,36 +109,41 @@ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, /* ************************************************************************* */ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, - const Vector& b, const SharedDiagonal& model) { + const Vector& b, const SharedDiagonal& model, + const boost::optional& dualKey) : + dualKey_(dualKey) { fillTerms( - cref_list_of<4>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)) - (make_pair(i4, A4)), b, model); + cref_list_of<4>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3))( + make_pair(i4, A4)), b, model); } /* ************************************************************************* */ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, - Key i5, const Matrix& A5, const Vector& b, const SharedDiagonal& model) { + Key i5, const Matrix& A5, const Vector& b, const SharedDiagonal& model, + const boost::optional& dualKey) : + dualKey_(dualKey) { fillTerms( - cref_list_of<5>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)) - (make_pair(i4, A4))(make_pair(i5, A5)), b, model); + cref_list_of<5>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3))( + make_pair(i4, A4))(make_pair(i5, A5)), b, model); } /* ************************************************************************* */ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5, Key i6, const Matrix& A6, const Vector& b, - const SharedDiagonal& model) { + const SharedDiagonal& model, const boost::optional& dualKey) : + dualKey_(dualKey) { fillTerms( - cref_list_of<6>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)) - (make_pair(i4, A4))(make_pair(i5, A5))(make_pair(i6, A6)), b, model); + cref_list_of<6>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3))( + make_pair(i4, A4))(make_pair(i5, A5))(make_pair(i6, A6)), b, model); } /* ************************************************************************* */ JacobianFactor::JacobianFactor(const HessianFactor& factor) : Base(factor), Ab_( VerticalBlockMatrix::LikeActiveViewOf(factor.matrixObject(), - factor.rows())) { + factor.rows())), dualKey_(boost::none) { // Copy Hessian into our matrix and then do in-place Cholesky Ab_.full() = factor.matrixObject().full(); @@ -214,14 +223,14 @@ boost::tuple, DenseIndex, DenseIndex> _countDims( "Unable to determine dimensionality for all variables"); } - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) { - m += factor->rows(); - } + BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors){ + m += factor->rows(); +} #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - BOOST_FOREACH(DenseIndex d, varDims) { - assert(d != numeric_limits::max()); - } +BOOST_FOREACH(DenseIndex d, varDims) { + assert(d != numeric_limits::max()); +} #endif return boost::make_tuple(varDims, m, n); @@ -233,15 +242,15 @@ FastVector _convertOrCastToJacobians( gttic(Convert_to_Jacobians); FastVector jacobians; jacobians.reserve(factors.size()); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - if (factor) { - if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast< - JacobianFactor>(factor)) - jacobians.push_back(jf); - else - jacobians.push_back(boost::make_shared(*factor)); - } + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors){ + if (factor) { + if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast< + JacobianFactor>(factor)) + jacobians.push_back(jf); + else + jacobians.push_back(boost::make_shared(*factor)); } +} return jacobians; } } @@ -249,7 +258,8 @@ FastVector _convertOrCastToJacobians( /* ************************************************************************* */ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, boost::optional ordering, - boost::optional variableSlots) { + boost::optional variableSlots) : + dualKey_(boost::none) { gttic(JacobianFactor_combine_constructor); // Compute VariableSlots if one was not provided @@ -291,16 +301,16 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, "The ordering provided to the JacobianFactor combine constructor\n" "contained extra variables that did not appear in the factors to combine."); // Add the remaining slots - BOOST_FOREACH(VariableSlots::const_iterator item, unorderedSlots) { - orderedSlots.push_back(item); - } - } else { - // If no ordering is provided, arrange the slots as they were, which will be sorted - // numerically since VariableSlots uses a map sorting on Key. - for (VariableSlots::const_iterator item = variableSlots->begin(); - item != variableSlots->end(); ++item) - orderedSlots.push_back(item); + BOOST_FOREACH(VariableSlots::const_iterator item, unorderedSlots){ + orderedSlots.push_back(item); } +} else { + // If no ordering is provided, arrange the slots as they were, which will be sorted + // numerically since VariableSlots uses a map sorting on Key. + for (VariableSlots::const_iterator item = variableSlots->begin(); + item != variableSlots->end(); ++item) + orderedSlots.push_back(item); +} gttoc(Order_slots); // Count dimensions @@ -321,28 +331,28 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, // Loop over slots in combined factor and copy blocks from source factors gttic(copy_blocks); size_t combinedSlot = 0; - BOOST_FOREACH(VariableSlots::const_iterator varslot, orderedSlots) { - JacobianFactor::ABlock destSlot(this->getA(this->begin() + combinedSlot)); - // Loop over source jacobians - DenseIndex nextRow = 0; - for (size_t factorI = 0; factorI < jacobians.size(); ++factorI) { - // Slot in source factor - const size_t sourceSlot = varslot->second[factorI]; - const DenseIndex sourceRows = jacobians[factorI]->rows(); - if (sourceRows > 0) { - JacobianFactor::ABlock::RowsBlockXpr destBlock( - destSlot.middleRows(nextRow, sourceRows)); - // Copy if exists in source factor, otherwise set zero - if (sourceSlot != VariableSlots::Empty) - destBlock = jacobians[factorI]->getA( - jacobians[factorI]->begin() + sourceSlot); - else - destBlock.setZero(); - nextRow += sourceRows; - } + BOOST_FOREACH(VariableSlots::const_iterator varslot, orderedSlots){ + JacobianFactor::ABlock destSlot(this->getA(this->begin() + combinedSlot)); + // Loop over source jacobians + DenseIndex nextRow = 0; + for (size_t factorI = 0; factorI < jacobians.size(); ++factorI) { + // Slot in source factor + const size_t sourceSlot = varslot->second[factorI]; + const DenseIndex sourceRows = jacobians[factorI]->rows(); + if (sourceRows > 0) { + JacobianFactor::ABlock::RowsBlockXpr destBlock( + destSlot.middleRows(nextRow, sourceRows)); + // Copy if exists in source factor, otherwise set zero + if (sourceSlot != VariableSlots::Empty) + destBlock = jacobians[factorI]->getA( + jacobians[factorI]->begin() + sourceSlot); + else + destBlock.setZero(); + nextRow += sourceRows; } - ++combinedSlot; } + ++combinedSlot; +} gttoc(copy_blocks); // Copy the RHS vectors and sigmas @@ -592,7 +602,8 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, } /* ************************************************************************* */ -VectorValues JacobianFactor::gradientAtZero(const boost::optional dual) const { +VectorValues JacobianFactor::gradientAtZero( + const boost::optional dual) const { VectorValues g; Vector b = getb(); // Gradient is really -A'*b / sigma^2 @@ -601,7 +612,8 @@ VectorValues JacobianFactor::gradientAtZero(const boost::optional dual) // scale b by the dual vector if it exists if (dual) { if (dual->size() != b_sigma.size()) - throw std::runtime_error("Fail to scale the gradient with the dual vector: Size mismatch!"); + throw std::runtime_error( + "Fail to scale the gradient with the dual vector: Size mismatch!"); b_sigma = b_sigma.cwiseProduct(*dual); } this->transposeMultiplyAdd(-1.0, b_sigma, g); // g -= A'*b/sigma^2 @@ -613,6 +625,13 @@ void JacobianFactor::gradientAtZero(double* d) const { //throw std::runtime_error("gradientAtZero not implemented for Jacobian factor"); } +/* ************************************************************************* */ +Vector JacobianFactor::gradient(Key key, const VectorValues& x) const { + // TODO: optimize it for JacobianFactor without converting to a HessianFactor + HessianFactor hessian(*this); + return hessian.gradient(key, x); +} + /* ************************************************************************* */ pair JacobianFactor::jacobian() const { pair result = jacobianUnweighted(); @@ -742,8 +761,8 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional( const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd) - Ab_.rowStart() - frontalDim; const DenseIndex remainingRows = - model_ ? - std::min(model_->sigmas().size() - frontalDim, maxRemainingRows) : + model_ ? std::min(model_->sigmas().size() - frontalDim, + maxRemainingRows) : maxRemainingRows; Ab_.rowStart() += frontalDim; Ab_.rowEnd() = Ab_.rowStart() + remainingRows; @@ -760,7 +779,7 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional( model_ = noiseModel::Constrained::MixedSigmas(sigmas); else model_ = noiseModel::Diagonal::Sigmas(sigmas, false); // I don't want to be smart here - assert(model_->dim() == (size_t)Ab_.rows()); + assert(model_->dim() == (size_t )Ab_.rows()); } gttoc(remaining_factor); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index a5cc90b62..2c887209b 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -27,336 +27,393 @@ namespace gtsam { - // Forward declarations - class HessianFactor; - class VariableSlots; - class GaussianFactorGraph; - class GaussianConditional; - class HessianFactor; - class VectorValues; - class Ordering; - class JacobianFactor; +// Forward declarations +class HessianFactor; +class VariableSlots; +class GaussianFactorGraph; +class GaussianConditional; +class HessianFactor; +class VectorValues; +class Ordering; +class JacobianFactor; - GTSAM_EXPORT std::pair, boost::shared_ptr > - EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); +GTSAM_EXPORT std::pair, + boost::shared_ptr > +EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); + +/** + * A Gaussian factor in the squared-error form. + * + * JacobianFactor implements a + * Gaussian, which has quadratic negative log-likelihood + * \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f] + * where \f$ \Sigma \f$ is a \a diagonal covariance matrix. The + * matrix \f$ A \f$, r.h.s. vector \f$ b \f$, and diagonal noise model + * \f$ \Sigma \f$ are stored in this class. + * + * This factor represents the sum-of-squares error of a \a linear + * measurement function, and is created upon linearization of a NoiseModelFactor, + * which in turn is a sum-of-squares factor with a nonlinear measurement function. + * + * Here is an example of how this factor represents a sum-of-squares error: + * + * Letting \f$ h(x) \f$ be a \a linear measurement prediction function, \f$ z \f$ be + * the actual observed measurement, the residual is + * \f[ f(x) = h(x) - z . \f] + * If we expect noise with diagonal covariance matrix \f$ \Sigma \f$ on this + * measurement, then the negative log-likelihood of the Gaussian induced by this + * measurement model is + * \f[ E(x) = \frac{1}{2} (h(x) - z)^T \Sigma^{-1} (h(x) - z) . \f] + * Because \f$ h(x) \f$ is linear, we can write it as + * \f[ h(x) = Ax + e \f] + * and thus we have + * \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f] + * where \f$ b = z - e \f$. + * + * This factor can involve an arbitrary number of variables, and in the + * above example \f$ x \f$ would almost always be only be a subset of the variables + * in the entire factor graph. There are special constructors for 1-, 2-, and 3- + * way JacobianFactors, and additional constructors for creating n-way JacobianFactors. + * The Jacobian matrix \f$ A \f$ is passed to these constructors in blocks, + * for example, for a 2-way factor, the constructor would accept \f$ A1 \f$ and \f$ A2 \f$, + * as well as the variable indices \f$ j1 \f$ and \f$ j2 \f$ + * and the negative log-likelihood represented by this factor would be + * \f[ E(x) = \frac{1}{2} (A_1 x_{j1} + A_2 x_{j2} - b)^T \Sigma^{-1} (A_1 x_{j1} + A_2 x_{j2} - b) . \f] + */ +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 + boost::optional dualKey_; // Key of the dual variable associated with this factor if it is a constraint + +public: + typedef VerticalBlockMatrix::Block ABlock; + typedef VerticalBlockMatrix::constBlock constABlock; + typedef ABlock::ColXpr BVector; + typedef constABlock::ConstColXpr constBVector; + + /** Convert from other GaussianFactor */ + explicit JacobianFactor(const GaussianFactor& gf); + + /** Copy constructor */ + JacobianFactor(const JacobianFactor& jf) : + Base(jf), Ab_(jf.Ab_), model_(jf.model_), dualKey_(jf.dualKey_) { + } + + /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ + explicit JacobianFactor(const HessianFactor& hf); + + /** default constructor for I/O */ + JacobianFactor(); + + /** Construct Null factor */ + explicit JacobianFactor(const Vector& b_in); + + /** Construct unary factor */ + JacobianFactor(Key i1, const Matrix& A1, const Vector& b, + const SharedDiagonal& model = SharedDiagonal(), + const boost::optional& dualKey = boost::none); + + /** Construct binary factor */ + JacobianFactor(Key i1, const Matrix& A1, Key i2, const Matrix& A2, + const Vector& b, const SharedDiagonal& model = SharedDiagonal(), + const boost::optional& dualKey = boost::none); + + /** Construct ternary factor */ + JacobianFactor(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, const Vector& b, const SharedDiagonal& model = + SharedDiagonal(), const boost::optional& dualKey = boost::none); + + /** Construct four-ary factor */ + JacobianFactor(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, Key i4, const Matrix& A4, const Vector& b, + const SharedDiagonal& model = SharedDiagonal(), + const boost::optional& dualKey = boost::none); + + /** Construct five-ary factor */ + JacobianFactor(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5, + const Vector& b, const SharedDiagonal& model = SharedDiagonal(), + const boost::optional& dualKey = boost::none); + + /** Construct six-ary factor */ + JacobianFactor(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5, + Key i6, const Matrix& A6, const Vector& b, const SharedDiagonal& model = + SharedDiagonal(), const boost::optional& dualKey = boost::none); + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + JacobianFactor(const TERMS& terms, const Vector& b, + const SharedDiagonal& model = SharedDiagonal(), + const boost::optional& dualKey = boost::none); + + /** Constructor with arbitrary number keys, and where the augmented matrix is given all together + * instead of in block terms. Note that only the active view of the provided augmented matrix + * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed + * factor. */ + template + JacobianFactor(const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, + const SharedDiagonal& sigmas = SharedDiagonal(), + const boost::optional& dualKey = boost::none); /** - * A Gaussian factor in the squared-error form. - * - * JacobianFactor implements a - * Gaussian, which has quadratic negative log-likelihood - * \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f] - * where \f$ \Sigma \f$ is a \a diagonal covariance matrix. The - * matrix \f$ A \f$, r.h.s. vector \f$ b \f$, and diagonal noise model - * \f$ \Sigma \f$ are stored in this class. - * - * This factor represents the sum-of-squares error of a \a linear - * measurement function, and is created upon linearization of a NoiseModelFactor, - * which in turn is a sum-of-squares factor with a nonlinear measurement function. - * - * Here is an example of how this factor represents a sum-of-squares error: - * - * Letting \f$ h(x) \f$ be a \a linear measurement prediction function, \f$ z \f$ be - * the actual observed measurement, the residual is - * \f[ f(x) = h(x) - z . \f] - * If we expect noise with diagonal covariance matrix \f$ \Sigma \f$ on this - * measurement, then the negative log-likelihood of the Gaussian induced by this - * measurement model is - * \f[ E(x) = \frac{1}{2} (h(x) - z)^T \Sigma^{-1} (h(x) - z) . \f] - * Because \f$ h(x) \f$ is linear, we can write it as - * \f[ h(x) = Ax + e \f] - * and thus we have - * \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f] - * where \f$ b = z - e \f$. - * - * This factor can involve an arbitrary number of variables, and in the - * above example \f$ x \f$ would almost always be only be a subset of the variables - * in the entire factor graph. There are special constructors for 1-, 2-, and 3- - * way JacobianFactors, and additional constructors for creating n-way JacobianFactors. - * The Jacobian matrix \f$ A \f$ is passed to these constructors in blocks, - * for example, for a 2-way factor, the constructor would accept \f$ A1 \f$ and \f$ A2 \f$, - * as well as the variable indices \f$ j1 \f$ and \f$ j2 \f$ - * and the negative log-likelihood represented by this factor would be - * \f[ E(x) = \frac{1}{2} (A_1 x_{j1} + A_2 x_{j2} - b)^T \Sigma^{-1} (A_1 x_{j1} + A_2 x_{j2} - b) . \f] - */ - 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; - - - /** Convert from other GaussianFactor */ - explicit JacobianFactor(const GaussianFactor& gf); - - /** Copy constructor */ - JacobianFactor(const JacobianFactor& jf) : Base(jf), Ab_(jf.Ab_), model_(jf.model_) {} - - /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ - explicit JacobianFactor(const HessianFactor& hf); - - /** default constructor for I/O */ - JacobianFactor(); - - /** Construct Null factor */ - explicit JacobianFactor(const Vector& b_in); - - /** Construct unary factor */ - JacobianFactor(Key i1, const Matrix& A1, - const Vector& b, const SharedDiagonal& model = SharedDiagonal()); - - /** Construct binary factor */ - JacobianFactor(Key i1, const Matrix& A1, - Key i2, const Matrix& A2, - const Vector& b, const SharedDiagonal& model = SharedDiagonal()); - - /** Construct ternary factor */ - JacobianFactor(Key i1, const Matrix& A1, Key i2, - const Matrix& A2, Key i3, const Matrix& A3, - const Vector& b, const SharedDiagonal& model = SharedDiagonal()); - - /** Construct four-ary factor */ - JacobianFactor(Key i1, const Matrix& A1, Key i2, - const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, - const Vector& b, const SharedDiagonal& model = SharedDiagonal()); - - /** Construct five-ary factor */ - JacobianFactor(Key i1, const Matrix& A1, Key i2, - const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, - Key i5, const Matrix& A5, const Vector& b, const SharedDiagonal& model = SharedDiagonal()); - - /** Construct six-ary factor */ - JacobianFactor(Key i1, const Matrix& A1, Key i2, - const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, - Key i5, const Matrix& A5, Key i6, const Matrix& A6, - const Vector& b, const SharedDiagonal& model = SharedDiagonal()); - - /** Construct an n-ary factor - * @tparam TERMS A container whose value type is std::pair, specifying the - * collection of keys and matrices making up the factor. */ - template - JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal()); - - /** Constructor with arbitrary number keys, and where the augmented matrix is given all together - * instead of in block terms. Note that only the active view of the provided augmented matrix - * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed - * factor. */ - template - JacobianFactor( - const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); - - /** - * 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 - * computation performed. */ - explicit JacobianFactor( - const GaussianFactorGraph& graph, + * 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 + * computation performed. */ + explicit JacobianFactor(const GaussianFactorGraph& graph, boost::optional ordering = boost::none, boost::optional variableSlots = boost::none); - /** Virtual destructor */ - virtual ~JacobianFactor() {} + /** Virtual destructor */ + virtual ~JacobianFactor() { + } - /** Clone this JacobianFactor */ - virtual GaussianFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - boost::make_shared(*this)); - } + /** Clone this JacobianFactor */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + boost::make_shared(*this)); + } - // Implementing Testable interface - virtual void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const; - virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; + // Implementing Testable interface + virtual void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const; + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; - Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */ - Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */ - virtual double error(const VectorValues& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */ + Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */ + Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */ + virtual double error(const VectorValues& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */ - /** Return the augmented information matrix represented by this GaussianFactor. - * The augmented information matrix contains the information matrix with an - * additional column holding the information vector, and an additional row - * holding the transpose of the information vector. The lower-right entry - * contains the constant error term (when \f$ \delta x = 0 \f$). The - * augmented information matrix is described in more detail in HessianFactor, - * which in fact stores an augmented information matrix. - */ - virtual Matrix augmentedInformation() const; - - /** Return the non-augmented information matrix represented by this - * GaussianFactor. - */ - virtual Matrix information() const; - - /// Return the diagonal of the Hessian for this factor - virtual VectorValues hessianDiagonal() const; + /** Return the augmented information matrix represented by this GaussianFactor. + * The augmented information matrix contains the information matrix with an + * additional column holding the information vector, and an additional row + * holding the transpose of the information vector. The lower-right entry + * contains the constant error term (when \f$ \delta x = 0 \f$). The + * augmented information matrix is described in more detail in HessianFactor, + * which in fact stores an augmented information matrix. + */ + virtual Matrix augmentedInformation() const; - /* ************************************************************************* */ - virtual void hessianDiagonal(double* d) const; + /** Return the non-augmented information matrix represented by this + * GaussianFactor. + */ + virtual Matrix information() const; - /// Return the block diagonal of the Hessian for this factor - virtual std::map hessianBlockDiagonal() const; + /// Return the diagonal of the Hessian for this factor + virtual VectorValues hessianDiagonal() const; - /** - * @brief Returns (dense) A,b pair associated with factor, bakes in the weights - */ - virtual std::pair jacobian() const; - - /** - * @brief Returns (dense) A,b pair associated with factor, does not bake in weights - */ - std::pair jacobianUnweighted() const; + /* ************************************************************************* */ + virtual void hessianDiagonal(double* d) const; - /** Return (dense) matrix associated with factor. The returned system is an augmented matrix: - * [A b] - * weights are baked in */ - virtual Matrix augmentedJacobian() const; + /// Return the block diagonal of the Hessian for this factor + virtual std::map hessianBlockDiagonal() const; - /** Return (dense) matrix associated with factor. The returned system is an augmented matrix: - * [A b] - * weights are not baked in */ - Matrix augmentedJacobianUnweighted() const; + /** + * @brief Returns (dense) A,b pair associated with factor, bakes in the weights + */ + virtual std::pair jacobian() const; - /** Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ - const VerticalBlockMatrix& matrixObject() const { return Ab_; } + /** + * @brief Returns (dense) A,b pair associated with factor, does not bake in weights + */ + std::pair jacobianUnweighted() const; - /** Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ - VerticalBlockMatrix& matrixObject() { return Ab_; } + /** Return (dense) matrix associated with factor. The returned system is an augmented matrix: + * [A b] + * weights are baked in */ + virtual Matrix augmentedJacobian() const; - /** - * Construct the corresponding anti-factor to negate information - * stored stored in this factor. - * @return a HessianFactor with negated Hessian matrices - */ - virtual GaussianFactor::shared_ptr negate() const; + /** Return (dense) matrix associated with factor. The returned system is an augmented matrix: + * [A b] + * weights are not baked in */ + Matrix augmentedJacobianUnweighted() const; - /** Check if the factor is empty. TODO: How should this be defined? */ - virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } + /** Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ + const VerticalBlockMatrix& matrixObject() const { + return Ab_; + } - /** is noise model constrained ? */ - bool isConstrained() const { return model_ && model_->isConstrained(); } + /** Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ + VerticalBlockMatrix& matrixObject() { + return Ab_; + } - /** Return the dimension of the variable pointed to by the given key iterator - * todo: Remove this in favor of keeping track of dimensions with variables? - */ - virtual DenseIndex getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); } + /** + * Construct the corresponding anti-factor to negate information + * stored stored in this factor. + * @return a HessianFactor with negated Hessian matrices + */ + virtual GaussianFactor::shared_ptr negate() const; - /** - * return the number of rows in the corresponding linear system - */ - size_t rows() const { return Ab_.rows(); } + /** Check if the factor is empty. TODO: How should this be defined? */ + virtual bool empty() const { + return size() == 0 /*|| rows() == 0*/; + } - /** - * return the number of columns in the corresponding linear system - */ - size_t cols() const { return Ab_.cols(); } + /** is noise model constrained ? */ + bool isConstrained() const { + return model_ && model_->isConstrained(); + } - /** get a copy of model */ - const SharedDiagonal& get_model() const { return model_; } + /** Return the dimension of the variable pointed to by the given key iterator + * todo: Remove this in favor of keeping track of dimensions with variables? + */ + virtual DenseIndex getDim(const_iterator variable) const { + return Ab_(variable - begin()).cols(); + } - /** get a copy of model (non-const version) */ - SharedDiagonal& get_model() { return model_; } + /** + * return the number of rows in the corresponding linear system + */ + size_t rows() const { + return Ab_.rows(); + } - /** Get a view of the r.h.s. vector b, not weighted by noise */ - const constBVector getb() const { return Ab_(size()).col(0); } + /** + * return the number of columns in the corresponding linear system + */ + size_t cols() const { + return Ab_.cols(); + } - /** Get a view of the A matrix for the variable pointed to by the given key iterator */ - constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); } + /** get a copy of model */ + const SharedDiagonal& get_model() const { + return model_; + } - /** Get a view of the A matrix, not weighted by noise */ - constABlock getA() const { return Ab_.range(0, size()); } + /** get a copy of model (non-const version) */ + SharedDiagonal& get_model() { + return model_; + } - /** Get a view of the r.h.s. vector b (non-const version) */ - BVector getb() { return Ab_(size()).col(0); } + /** Get a view of the r.h.s. vector b, not weighted by noise */ + const constBVector getb() const { + return Ab_(size()).col(0); + } - /** Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version) */ - ABlock getA(iterator variable) { return Ab_(variable - begin()); } + /** Get a view of the A matrix for the variable pointed to by the given key iterator */ + constABlock getA(const_iterator variable) const { + return Ab_(variable - begin()); + } - /** Get a view of the A matrix */ - ABlock getA() { return Ab_.range(0, size()); } + /** Get a view of the A matrix, not weighted by noise */ + constABlock getA() const { + return Ab_.range(0, size()); + } - /** Return A*x */ - Vector operator*(const VectorValues& x) const; + /** Get a view of the r.h.s. vector b (non-const version) */ + BVector getb() { + return Ab_(size()).col(0); + } - /** x += A'*e. If x is initially missing any values, they are created and assumed to start as - * zero vectors. */ - void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const; + /** Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version) */ + ABlock getA(iterator variable) { + return Ab_(variable - begin()); + } - /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; + /** Get a view of the A matrix */ + ABlock getA() { + return Ab_.range(0, size()); + } - void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const; + /** Return A*x */ + Vector operator*(const VectorValues& x) const; - void multiplyHessianAdd(double alpha, const double* x, double* y) const {}; + /** x += A'*e. If x is initially missing any values, they are created and assumed to start as + * zero vectors. */ + void transposeMultiplyAdd(double alpha, const Vector& e, + VectorValues& x) const; - /// A'*b for Jacobian - VectorValues gradientAtZero(const boost::optional dual = boost::none) const; + /** y += alpha * A'*A*x */ + void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const; - /* ************************************************************************* */ - virtual void gradientAtZero(double* d) const; + void multiplyHessianAdd(double alpha, const double* x, double* y, + std::vector keys) const; - /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ - JacobianFactor whiten() const; + void multiplyHessianAdd(double alpha, const double* x, double* y) const { + } + ; - /** Eliminate the requested variables. */ - std::pair, boost::shared_ptr > - eliminate(const Ordering& keys); + /// A'*b for Jacobian + VectorValues gradientAtZero( + const boost::optional dual = boost::none) const; - /** set noiseModel correctly */ - void setModel(bool anyConstrained, const Vector& sigmas); - void setModel(const noiseModel::Diagonal::shared_ptr& model); - - /** - * Densely partially eliminate with QR factorization, this is usually provided as an argument to - * one of the factor graph elimination functions (see EliminateableFactorGraph). HessianFactors - * are first factored with Cholesky decomposition to produce JacobianFactors, by calling the - * conversion constructor JacobianFactor(const HessianFactor&). Variables are eliminated in the - * order specified in \c keys. - * @param factors Factors to combine and eliminate - * @param keys The variables to eliminate in the order as specified here in \c keys - * @return The conditional and remaining factor - * - * \addtogroup LinearSolving */ - friend GTSAM_EXPORT std::pair, boost::shared_ptr > - EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); + /* ************************************************************************* */ + virtual void gradientAtZero(double* d) const; - /** - * splits a pre-factorized factor into a conditional, and changes the current - * factor to be the remaining component. Performs same operation as eliminate(), - * but without running QR. - */ - boost::shared_ptr splitConditional(size_t nrFrontals); + /// Compute the gradient wrt a key at any values + Vector gradient(Key key, const VectorValues& x) const; - protected: + /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ + JacobianFactor whiten() const; - /// Internal function to fill blocks and set dimensions - template - void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel); - - private: + /** Eliminate the requested variables. */ + std::pair, + boost::shared_ptr > + eliminate(const Ordering& keys); - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(Ab_); - ar & BOOST_SERIALIZATION_NVP(model_); - } - }; // JacobianFactor + /** set noiseModel correctly */ + void setModel(bool anyConstrained, const Vector& sigmas); + void setModel(const noiseModel::Diagonal::shared_ptr& model); -} // gtsam + /** + * Densely partially eliminate with QR factorization, this is usually provided as an argument to + * one of the factor graph elimination functions (see EliminateableFactorGraph). HessianFactors + * are first factored with Cholesky decomposition to produce JacobianFactors, by calling the + * conversion constructor JacobianFactor(const HessianFactor&). Variables are eliminated in the + * order specified in \c keys. + * @param factors Factors to combine and eliminate + * @param keys The variables to eliminate in the order as specified here in \c keys + * @return The conditional and remaining factor + * + * \addtogroup LinearSolving */ + friend GTSAM_EXPORT std::pair, + boost::shared_ptr > + EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); + + /** + * splits a pre-factorized factor into a conditional, and changes the current + * factor to be the remaining component. Performs same operation as eliminate(), + * but without running QR. + */ + boost::shared_ptr splitConditional(size_t nrFrontals); + + /// return the dual key associated with this factor if it is a constraint + Key dualKey() const { + if (!dualKey_) + throw std::runtime_error("Error: Request for a dual key which does not exit in this JacobianFactor!"); + return *dualKey_; + } + +protected: + + /// Internal function to fill blocks and set dimensions + template + void fillTerms(const TERMS& terms, const Vector& b, + const SharedDiagonal& noiseModel); + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(Ab_); + ar & BOOST_SERIALIZATION_NVP(model_); + } +}; +// JacobianFactor + +}// gtsam #include - diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 17f5037a7..7f7efb9ae 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -17,7 +17,6 @@ */ // \callgraph - #pragma once #include @@ -28,7 +27,6 @@ #include #include - /** * Macro to add a standard clone function to a derived factor * @deprecated: will go away shortly - just add the clone function directly @@ -59,6 +57,8 @@ protected: typedef Factor Base; typedef NonlinearFactor This; + boost::optional dualKey_; //!< Key for the dual variable associated with this factor if it is a constraint + public: typedef boost::shared_ptr shared_ptr; @@ -67,14 +67,23 @@ public: /// @{ /** Default constructor for I/O only */ - NonlinearFactor() {} + NonlinearFactor() : + dualKey_(boost::none) { + } + + /** Construct with a dual key */ + NonlinearFactor(const boost::optional& dualKey) : + dualKey_(dualKey) { + } /** * Constructor from a collection of the keys involved in this factor */ template - NonlinearFactor(const CONTAINER& keys) : - Base(keys) {} + NonlinearFactor(const CONTAINER& keys, const boost::optional& dualKey = + boost::none) : + Base(keys), dualKey_(dualKey) { + } /// @} /// @name Testable @@ -82,10 +91,9 @@ public: /** print */ virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const - { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << " keys = { "; - BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; } + BOOST_FOREACH(Key key, this->keys()){ std::cout << keyFormatter(key) << " ";} std::cout << "}" << std::endl; } @@ -99,8 +107,8 @@ public: /// @{ /** Destructor */ - virtual ~NonlinearFactor() {} - + virtual ~NonlinearFactor() { + } /** * Calculate the error of the factor @@ -122,7 +130,9 @@ public: * when the constraint is *NOT* fulfilled. * @return true if the constraint is active */ - virtual bool active(const Values& c) const { return true; } + virtual bool active(const Values& c) const { + return true; + } /** linearize to a GaussianFactor */ virtual boost::shared_ptr @@ -138,7 +148,6 @@ public: // indices[j] = ordering[this->keys()[j]]; // return IndexFactor::shared_ptr(new IndexFactor(indices)); //} - /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow * for subclasses @@ -147,7 +156,8 @@ public: */ virtual shared_ptr clone() const { // TODO: choose better exception to throw here - throw std::runtime_error("NonlinearFactor::clone(): Attempting to clone factor with no clone() implemented!"); + throw std::runtime_error( + "NonlinearFactor::clone(): Attempting to clone factor with no clone() implemented!"); return shared_ptr(); } @@ -155,11 +165,11 @@ public: * Creates a shared_ptr clone of the factor with different keys using * a map from old->new keys */ - shared_ptr rekey(const std::map& rekey_mapping) const { + shared_ptr rekey(const std::map& rekey_mapping) const { shared_ptr new_factor = clone(); - for (size_t i=0; isize(); ++i) { + for (size_t i = 0; i < new_factor->size(); ++i) { Key& cur_key = new_factor->keys()[i]; - std::map::const_iterator mapping = rekey_mapping.find(cur_key); + std::map::const_iterator mapping = rekey_mapping.find(cur_key); if (mapping != rekey_mapping.end()) cur_key = mapping->second; } @@ -177,8 +187,27 @@ public: return new_factor; } + /** + * Return the optional dual variable's key + */ + Key dualKey() const { + if (!dualKey_) + throw std::runtime_error("Error: Request for a dual key which does not exist in this factor!"); + return *dualKey_; + } -}; // \class NonlinearFactor + /** + * Create a Hessian factor scaled by the dual variable corresponding to + * the nonlinear equality constraint, used in SQP scheme for solving constrained problems. + * By default, return a null shared_ptr if it's not a constraint. + */ + virtual GaussianFactor::shared_ptr multipliedHessian(const Values& x, + const VectorValues& duals) const { + return GaussianFactor::shared_ptr(); + } + +}; +// \class NonlinearFactor /* ************************************************************************* */ /** @@ -206,41 +235,52 @@ public: typedef boost::shared_ptr shared_ptr; /** Default constructor for I/O only */ - NoiseModelFactor() {} + NoiseModelFactor() : + Base() { + } /** Destructor */ - virtual ~NoiseModelFactor() {} + virtual ~NoiseModelFactor() { + } /** * Constructor */ template - NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys) : - Base(keys), noiseModel_(noiseModel) {} + NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys, + const boost::optional& dualKey = boost::none) : + Base(keys, dualKey), noiseModel_(noiseModel) { + } protected: /** * Constructor - only for subclasses, as this does not set keys. */ - NoiseModelFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) {} + NoiseModelFactor(const SharedNoiseModel& noiseModel, + const boost::optional& dualKey = boost::none) : Base(dualKey), + noiseModel_(noiseModel) { + } public: /** Print */ virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const - { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); this->noiseModel_->print(" noise model: "); + if (dualKey_) { + std::cout << "Dual Key: " << this->dualKey() << std::endl; + } } /** Check if two factors are equal */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const NoiseModelFactor* e = dynamic_cast(&f); - return e && Base::equals(f, tol) && - ((!noiseModel_ && !e->noiseModel_) || - (noiseModel_ && e->noiseModel_ && noiseModel_->equals(*e->noiseModel_, tol))); + return e && Base::equals(f, tol) + && ((!noiseModel_ && !e->noiseModel_) + || (noiseModel_ && e->noiseModel_ + && noiseModel_->equals(*e->noiseModel_, tol))); } /** get the dimension of the factor (number of rows on linearization) */ @@ -259,7 +299,8 @@ public: * If any of the optional Matrix reference arguments are specified, it should compute * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...). */ - virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const = 0; + virtual Vector unwhitenedError(const Values& x, + boost::optional&> H = boost::none) const = 0; /** * Vector of errors, whitened @@ -267,8 +308,9 @@ public: */ Vector whitenedError(const Values& c) const { const Vector unwhitenedErrorVec = unwhitenedError(c); - if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim()) - throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); + if ((size_t) unwhitenedErrorVec.size() != noiseModel_->dim()) + throw std::invalid_argument( + "This factor was created with a NoiseModel of incorrect dimension."); return noiseModel_->whiten(unwhitenedErrorVec); } @@ -281,8 +323,9 @@ public: virtual double error(const Values& c) const { if (this->active(c)) { const Vector unwhitenedErrorVec = unwhitenedError(c); - if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim()) - throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); + if ((size_t) unwhitenedErrorVec.size() != noiseModel_->dim()) + throw std::invalid_argument( + "This factor was created with a NoiseModel of incorrect dimension."); return 0.5 * noiseModel_->distance(unwhitenedErrorVec); } else { return 0.0; @@ -304,29 +347,30 @@ public: // Call evaluate error to get Jacobians and b vector std::vector A(this->size()); b = -unwhitenedError(x, A); - if(noiseModel_) - { - if((size_t) b.size() != noiseModel_->dim()) - throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); + if (noiseModel_) { + if ((size_t) b.size() != noiseModel_->dim()) + throw std::invalid_argument( + "This factor was created with a NoiseModel of incorrect dimension."); - this->noiseModel_->WhitenSystem(A,b); + this->noiseModel_->WhitenSystem(A, b); } std::vector > terms(this->size()); // Fill in terms - for(size_t j=0; jsize(); ++j) { + for (size_t j = 0; j < this->size(); ++j) { terms[j].first = this->keys()[j]; terms[j].second.swap(A[j]); } // TODO pass unwhitened + noise model to Gaussian factor // For now, only linearized constrained factors have noise model at linear level!!! - if(noiseModel_) - { + if (noiseModel_) { noiseModel::Constrained::shared_ptr constrained = - boost::dynamic_pointer_cast(this->noiseModel_); - if(constrained) - return GaussianFactor::shared_ptr(new JacobianFactor(terms, b, constrained->unit())); + boost::dynamic_pointer_cast( + this->noiseModel_); + if (constrained) + return GaussianFactor::shared_ptr( + new JacobianFactor(terms, b, constrained->unit(), dualKey_)); } return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); @@ -338,13 +382,14 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor", - boost::serialization::base_object(*this)); + ar + & boost::serialization::make_nvp("NonlinearFactor", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(noiseModel_); } -}; // \class NoiseModelFactor - +}; +// \class NoiseModelFactor /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 1 @@ -365,27 +410,35 @@ protected: public: /** Default constructor for I/O only */ - NoiseModelFactor1() {} + NoiseModelFactor1() : + Base() { + } - virtual ~NoiseModelFactor1() {} + virtual ~NoiseModelFactor1() { + } - inline Key key() const { return keys_[0]; } + inline Key key() const { + return keys_[0]; + } /** * Constructor * @param noiseModel shared pointer to noise model * @param key1 by which to look up X value in Values */ - NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) : - Base(noiseModel, cref_list_of<1>(key1)) {} + NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1, + const boost::optional& dualKey = boost::none) : + Base(noiseModel, cref_list_of<1>(key1), dualKey) { + } /** Calls the 1-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - if(this->active(x)) { + virtual Vector unwhitenedError(const Values& x, + boost::optional&> H = boost::none) const { + if (this->active(x)) { const X& x1 = x.at(keys_[0]); - if(H) { + if (H) { return evaluateError(x1, (*H)[0]); } else { return evaluateError(x1); @@ -409,11 +462,12 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + ar + & boost::serialization::make_nvp("NoiseModelFactor", + boost::serialization::base_object(*this)); } -};// \class NoiseModelFactor1 - +}; +// \class NoiseModelFactor1 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 2 @@ -437,7 +491,9 @@ public: /** * Default Constructor for I/O */ - NoiseModelFactor2() {} + NoiseModelFactor2() : + Base() { + } /** * Constructor @@ -445,22 +501,30 @@ public: * @param j1 key of the first variable * @param j2 key of the second variable */ - NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) : - Base(noiseModel, cref_list_of<2>(j1)(j2)) {} + NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2, + const boost::optional& dualKey = boost::none) : + Base(noiseModel, cref_list_of<2>(j1)(j2), dualKey) { + } - virtual ~NoiseModelFactor2() {} + virtual ~NoiseModelFactor2() { + } /** methods to retrieve both keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } + inline Key key1() const { + return keys_[0]; + } + inline Key key2() const { + return keys_[1]; + } /** Calls the 2-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - if(this->active(x)) { + virtual Vector unwhitenedError(const Values& x, + boost::optional&> H = boost::none) const { + if (this->active(x)) { const X1& x1 = x.at(keys_[0]); const X2& x2 = x.at(keys_[1]); - if(H) { + if (H) { return evaluateError(x1, x2, (*H)[0], (*H)[1]); } else { return evaluateError(x1, x2); @@ -476,8 +540,8 @@ public: * both the function evaluation and its derivative(s) in X1 (and/or X2). */ virtual Vector - evaluateError(const X1&, const X2&, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const = 0; + evaluateError(const X1&, const X2&, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const = 0; private: @@ -485,10 +549,12 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + ar + & boost::serialization::make_nvp("NoiseModelFactor", + boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor2 +}; +// \class NoiseModelFactor2 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 3 @@ -513,7 +579,9 @@ public: /** * Default Constructor for I/O */ - NoiseModelFactor3() {} + NoiseModelFactor3() : + Base() { + } /** * Constructor @@ -522,24 +590,36 @@ public: * @param j2 key of the second variable * @param j3 key of the third variable */ - NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) : - Base(noiseModel, cref_list_of<3>(j1)(j2)(j3)) {} + NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, + const boost::optional& dualKey = boost::none) : + Base(noiseModel, cref_list_of<3>(j1)(j2)(j3), dualKey) { + } - virtual ~NoiseModelFactor3() {} + virtual ~NoiseModelFactor3() { + } /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } + inline Key key1() const { + return keys_[0]; + } + inline Key key2() const { + return keys_[1]; + } + inline Key key3() const { + return keys_[2]; + } /** Calls the 3-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), (*H)[0], (*H)[1], (*H)[2]); + virtual Vector unwhitenedError(const Values& x, + boost::optional&> H = boost::none) const { + if (this->active(x)) { + if (H) + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), + x.at(keys_[2]), (*H)[0], (*H)[1], (*H)[2]); else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2])); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), + x.at(keys_[2])); } else { return zero(this->dim()); } @@ -551,9 +631,8 @@ public: * both the function evaluation and its derivative(s) in X1 (and/or X2, X3). */ virtual Vector - evaluateError(const X1&, const X2&, const X3&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, + evaluateError(const X1&, const X2&, const X3&, boost::optional H1 = + boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const = 0; private: @@ -562,10 +641,12 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + ar + & boost::serialization::make_nvp("NoiseModelFactor", + boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor3 +}; +// \class NoiseModelFactor3 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 4 @@ -591,7 +672,9 @@ public: /** * Default Constructor for I/O */ - NoiseModelFactor4() {} + NoiseModelFactor4() : + Base() { + } /** * Constructor @@ -601,25 +684,40 @@ public: * @param j3 key of the third variable * @param j4 key of the fourth variable */ - NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) : - Base(noiseModel, cref_list_of<4>(j1)(j2)(j3)(j4)) {} + NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, + Key j4, const boost::optional& dualKey = boost::none) : + Base(noiseModel, cref_list_of<4>(j1)(j2)(j3)(j4), dualKey) { + } - virtual ~NoiseModelFactor4() {} + virtual ~NoiseModelFactor4() { + } /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } - inline Key key4() const { return keys_[3]; } + inline Key key1() const { + return keys_[0]; + } + inline Key key2() const { + return keys_[1]; + } + inline Key key3() const { + return keys_[2]; + } + inline Key key4() const { + return keys_[3]; + } /** Calls the 4-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]); + virtual Vector unwhitenedError(const Values& x, + boost::optional&> H = boost::none) const { + if (this->active(x)) { + if (H) + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), + x.at(keys_[2]), x.at(keys_[3]), (*H)[0], (*H)[1], (*H)[2], + (*H)[3]); else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3])); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), + x.at(keys_[2]), x.at(keys_[3])); } else { return zero(this->dim()); } @@ -632,9 +730,8 @@ public: */ virtual Vector evaluateError(const X1&, const X2&, const X3&, const X4&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none, boost::optional H3 = boost::none, boost::optional H4 = boost::none) const = 0; private: @@ -643,10 +740,12 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + ar + & boost::serialization::make_nvp("NoiseModelFactor", + boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor4 +}; +// \class NoiseModelFactor4 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 5 @@ -673,7 +772,9 @@ public: /** * Default Constructor for I/O */ - NoiseModelFactor5() {} + NoiseModelFactor5() : + Base() { + } /** * Constructor @@ -684,26 +785,43 @@ public: * @param j4 key of the fourth variable * @param j5 key of the fifth variable */ - NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) : - Base(noiseModel, cref_list_of<5>(j1)(j2)(j3)(j4)(j5)) {} + NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, + Key j4, Key j5, const boost::optional& dualKey = boost::none) : + Base(noiseModel, cref_list_of<5>(j1)(j2)(j3)(j4)(j5), dualKey) { + } - virtual ~NoiseModelFactor5() {} + virtual ~NoiseModelFactor5() { + } /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } - inline Key key4() const { return keys_[3]; } - inline Key key5() const { return keys_[4]; } + inline Key key1() const { + return keys_[0]; + } + inline Key key2() const { + return keys_[1]; + } + inline Key key3() const { + return keys_[2]; + } + inline Key key4() const { + return keys_[3]; + } + inline Key key5() const { + return keys_[4]; + } /** Calls the 5-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); + virtual Vector unwhitenedError(const Values& x, + boost::optional&> H = boost::none) const { + if (this->active(x)) { + if (H) + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), + x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), (*H)[0], + (*H)[1], (*H)[2], (*H)[3], (*H)[4]); else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4])); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), + x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4])); } else { return zero(this->dim()); } @@ -716,11 +834,10 @@ public: */ virtual Vector evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const = 0; + boost::optional H1 = boost::none, boost::optional H2 = + boost::none, boost::optional H3 = boost::none, + boost::optional H4 = boost::none, boost::optional H5 = + boost::none) const = 0; private: @@ -728,15 +845,18 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + ar + & boost::serialization::make_nvp("NoiseModelFactor", + boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor5 +}; +// \class NoiseModelFactor5 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 6 * variables. To derive from this class, implement evaluateError(). */ -template +template class NoiseModelFactor6: public NoiseModelFactor { public: @@ -759,7 +879,9 @@ public: /** * Default Constructor for I/O */ - NoiseModelFactor6() {} + NoiseModelFactor6() : + Base() { + } /** * Constructor @@ -771,27 +893,48 @@ public: * @param j5 key of the fifth variable * @param j6 key of the fifth variable */ - NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) : - Base(noiseModel, cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {} + NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, + Key j4, Key j5, Key j6, const boost::optional& dualKey = boost::none) : + Base(noiseModel, cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6), dualKey) { + } - virtual ~NoiseModelFactor6() {} + virtual ~NoiseModelFactor6() { + } /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } - inline Key key4() const { return keys_[3]; } - inline Key key5() const { return keys_[4]; } - inline Key key6() const { return keys_[5]; } + inline Key key1() const { + return keys_[0]; + } + inline Key key2() const { + return keys_[1]; + } + inline Key key3() const { + return keys_[2]; + } + inline Key key4() const { + return keys_[3]; + } + inline Key key5() const { + return keys_[4]; + } + inline Key key6() const { + return keys_[5]; + } /** Calls the 6-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); + virtual Vector unwhitenedError(const Values& x, + boost::optional&> H = boost::none) const { + if (this->active(x)) { + if (H) + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), + x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), + x.at(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], + (*H)[5]); else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5])); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), + x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), + x.at(keys_[5])); } else { return zero(this->dim()); } @@ -803,13 +946,12 @@ public: * both the function evaluation and its derivative(s) in X1 (and/or X2, X3). */ virtual Vector - evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none, - boost::optional H6 = boost::none) const = 0; + evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, + const X6&, boost::optional H1 = boost::none, + boost::optional H2 = boost::none, boost::optional H3 = + boost::none, boost::optional H4 = boost::none, + boost::optional H5 = boost::none, boost::optional H6 = + boost::none) const = 0; private: @@ -817,11 +959,13 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + ar + & boost::serialization::make_nvp("NoiseModelFactor", + boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor6 +}; +// \class NoiseModelFactor6 /* ************************************************************************* */ -} // \namespace gtsam +}// \namespace gtsam